LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

上层和底盘均改

Browse Source
This commit is contained in:
ZHAISHUI04 2025-05-07 23:28:50 +08:00
parent 52668517bd
commit 0b0ba31a48
14 changed files with 212 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Core/Inc/main.h
View File

@ -75,11 +75,17 @@ void Error_Handler(void);
#define ACCL_INT_Pin GPIO_PIN_4
#define ACCL_INT_GPIO_Port GPIOC
#define ACCL_INT_EXTI_IRQn EXTI4_IRQn
#define LIGHT_C_Pin GPIO_PIN_13
#define LIGHT_C_GPIO_Port GPIOE
#define GYRO_INT_Pin GPIO_PIN_5
#define GYRO_INT_GPIO_Port GPIOC
#define GYRO_INT_EXTI_IRQn EXTI9_5_IRQn
#define FlagForUpper_Pin GPIO_PIN_9
#define FlagForUpper_GPIO_Port GPIOE
#define LIGHT_A_Pin GPIO_PIN_9
#define LIGHT_A_GPIO_Port GPIOE
#define LIGHT_B_Pin GPIO_PIN_11
#define LIGHT_B_GPIO_Port GPIOE
#define RELAY_Pin GPIO_PIN_14
#define RELAY_GPIO_Port GPIOE
#define GYRO_CS_Pin GPIO_PIN_0
#define GYRO_CS_GPIO_Port GPIOB

14
Core/Src/gpio.c
View File

@ -61,7 +61,7 @@ void MX_GPIO_Init(void)
HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(FlagForUpper_GPIO_Port, FlagForUpper_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(RELAY_GPIO_Port, RELAY_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
@ -92,12 +92,18 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pins : PEPin PEPin */
GPIO_InitStruct.Pin = LIGHT_C_Pin|LIGHT_B_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = FlagForUpper_Pin;
GPIO_InitStruct.Pin = RELAY_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(FlagForUpper_GPIO_Port, &GPIO_InitStruct);
HAL_GPIO_Init(RELAY_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GYRO_CS_Pin;

2
Core/Src/main.c
View File

@ -80,7 +80,7 @@ int main(void)
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
HAL_Init();
/* USER CODE BEGIN Init */

93
MDK-ARM/R2.uvoptx
View File

@ -208,7 +208,12 @@
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>\\R2\../User/task/up_task.c\UP.motorfeedback.DJmotor_feedback</ItemText>
<ItemText>Nor_Vx</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>Nor_Vy</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
@ -972,18 +977,6 @@
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\bsp\map.c</PathWithFileName>
<FilenameWithoutPath>map.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>56</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\bsp\bsp_can.c</PathWithFileName>
<FilenameWithoutPath>bsp_can.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
@ -991,7 +984,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>57</FileNumber>
<FileNumber>56</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1003,7 +996,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>58</FileNumber>
<FileNumber>57</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1015,7 +1008,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>59</FileNumber>
<FileNumber>58</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1027,7 +1020,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>60</FileNumber>
<FileNumber>59</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1039,7 +1032,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>61</FileNumber>
<FileNumber>60</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1051,7 +1044,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>62</FileNumber>
<FileNumber>61</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1071,7 +1064,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>63</FileNumber>
<FileNumber>62</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1083,7 +1076,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>64</FileNumber>
<FileNumber>63</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1095,7 +1088,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>65</FileNumber>
<FileNumber>64</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1107,7 +1100,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>66</FileNumber>
<FileNumber>65</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1119,7 +1112,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>67</FileNumber>
<FileNumber>66</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1131,7 +1124,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>68</FileNumber>
<FileNumber>67</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1143,7 +1136,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber>
<FileNumber>68</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1155,7 +1148,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber>
<FileNumber>69</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1167,7 +1160,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>71</FileNumber>
<FileNumber>70</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1179,7 +1172,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>72</FileNumber>
<FileNumber>71</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1199,7 +1192,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>73</FileNumber>
<FileNumber>72</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1211,7 +1204,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>74</FileNumber>
<FileNumber>73</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1223,7 +1216,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>75</FileNumber>
<FileNumber>74</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1235,7 +1228,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>76</FileNumber>
<FileNumber>75</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1247,7 +1240,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>77</FileNumber>
<FileNumber>76</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1259,7 +1252,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>78</FileNumber>
<FileNumber>77</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1279,7 +1272,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>79</FileNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1291,7 +1284,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber>
<FileNumber>79</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1303,7 +1296,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber>
<FileNumber>80</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1315,7 +1308,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>81</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1327,7 +1320,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1339,7 +1332,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1351,7 +1344,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1371,7 +1364,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1383,7 +1376,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1395,7 +1388,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1415,7 +1408,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>12</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1435,7 +1428,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1447,7 +1440,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1459,7 +1452,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1471,7 +1464,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

5
MDK-ARM/R2.uvprojx
View File

@ -1143,11 +1143,6 @@
<FileType>1</FileType>
<FilePath>..\User\bsp\pwm.c</FilePath>
</File>
<File>
<FileName>map.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\map.c</FilePath>
</File>
<File>
<FileName>bsp_can.c</FileName>
<FileType>1</FileType>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

52
R2.ioc
View File

@ -155,25 +155,28 @@ Mcu.Pin22=PD14
Mcu.Pin23=PA0-WKUP
Mcu.Pin24=PA4
Mcu.Pin25=PC4
Mcu.Pin26=PC5
Mcu.Pin27=PE9
Mcu.Pin28=PA7
Mcu.Pin29=PB0
Mcu.Pin26=PE13
Mcu.Pin27=PC5
Mcu.Pin28=PE9
Mcu.Pin29=PE11
Mcu.Pin3=PB3
Mcu.Pin30=VP_CRC_VS_CRC
Mcu.Pin31=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin32=VP_SYS_VS_Systick
Mcu.Pin33=VP_TIM4_VS_ClockSourceINT
Mcu.Pin34=VP_TIM7_VS_ClockSourceINT
Mcu.Pin35=VP_TIM10_VS_ClockSourceINT
Mcu.Pin36=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
Mcu.Pin30=PE14
Mcu.Pin31=PA7
Mcu.Pin32=PB0
Mcu.Pin33=VP_CRC_VS_CRC
Mcu.Pin34=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin35=VP_SYS_VS_Systick
Mcu.Pin36=VP_TIM4_VS_ClockSourceINT
Mcu.Pin37=VP_TIM7_VS_ClockSourceINT
Mcu.Pin38=VP_TIM10_VS_ClockSourceINT
Mcu.Pin39=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
Mcu.Pin4=PA14
Mcu.Pin5=PA13
Mcu.Pin6=PB7
Mcu.Pin7=PB6
Mcu.Pin8=PD0
Mcu.Pin9=PC11
Mcu.PinsNb=37
Mcu.PinsNb=40
Mcu.ThirdPartyNb=0
Mcu.UserConstants=
Mcu.UserName=STM32F407IGHx
@ -285,12 +288,28 @@ PD14.GPIOParameters=GPIO_Label
PD14.GPIO_Label=Buzzer
PD14.Locked=true
PD14.Signal=S_TIM4_CH3
PE11.GPIOParameters=GPIO_PuPd,GPIO_Label
PE11.GPIO_Label=LIGHT_B
PE11.GPIO_PuPd=GPIO_PULLDOWN
PE11.Locked=true
PE11.Signal=GPIO_Input
PE13.GPIOParameters=GPIO_PuPd,GPIO_Label
PE13.GPIO_Label=LIGHT_C
PE13.GPIO_PuPd=GPIO_PULLDOWN
PE13.Locked=true
PE13.Signal=GPIO_Input
PE14.GPIOParameters=GPIO_PuPd,GPIO_Label
PE14.GPIO_Label=RELAY
PE14.GPIO_PuPd=GPIO_PULLDOWN
PE14.Locked=true
PE14.Signal=GPIO_Output
PE9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label
PE9.GPIO_Label=FlagForUpper
PE9.GPIO_PuPd=GPIO_PULLUP
PE9.GPIO_Label=LIGHT_A
PE9.GPIO_PuPd=GPIO_PULLDOWN
PE9.Locked=true
PE9.PinState=GPIO_PIN_SET
PE9.Signal=GPIO_Output
PE9.Signal=SharedStack_PE9
PE9.Stacked=true
PF6.GPIOParameters=GPIO_Speed,GPIO_Label
PF6.GPIO_Label=IMU_HEAT_PWM
PF6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
@ -401,6 +420,9 @@ SH.S_TIM10_CH1.0=TIM10_CH1,PWM Generation1 CH1
SH.S_TIM10_CH1.ConfNb=1
SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3
SH.S_TIM4_CH3.ConfNb=1
SH.SharedStack_PE9.0=GPIO_Output+0
SH.SharedStack_PE9.1=GPIO_Input
SH.SharedStack_PE9.ConfNb=2
SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_16
SPI1.CLKPhase=SPI_PHASE_2EDGE
SPI1.CLKPolarity=SPI_POLARITY_HIGH

42
User/Algorithm/user_math.c
View File

@ -332,3 +332,45 @@ int abs_value(int num) {
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}
/**
* @brief 读取标志位状态
* @param flag 当前标志位的值（0 或 1）
* @return 返回状态值：0、1、2...
*/
int read_flag_state(uint8_t flag) {
static uint8_t last_flag = 0; // 保存上一次的标志位状态
static int state = 0; // 当前状态值
if (flag == 1 && last_flag == 0) {
// 标志位从 0 变为 1，状态值递增
state++;
}
// 更新上一次的标志位状态
last_flag = flag;
return state;
}
// 归一化函数，将正方形坐标映射到单位圆
void normalize_vector(double x, double y, double *out_x, double *out_y) {
// 处理原点情况
if (x == 0.0 && y == 0.0) {
*out_x = 0.0;
*out_y = 0.0;
return;
}
// 计算最大坐标绝对值和原始模长
const double abs_x = fabs(x);
const double abs_y = fabs(y);
const double s = fmax(abs_x, abs_y); // 最大坐标绝对值
const double r = sqrt(x*x + y*y); // 原始向量模长
const double scale = s / r; // 缩放因子
// 应用缩放并保持方向
*out_x = x * scale;
*out_y = y * scale;
}

2
User/Algorithm/user_math.h
View File

@ -161,5 +161,7 @@ uint8_t average(uint8_t arr[], uint8_t n);
int abs_value(int num);
float expo_map(float value, float expo_factor) ;
bool is_reached(float current, float target, float mistake) ;
int read_flag_state(uint8_t flag) ;
void normalize_vector(double x, double y, double *out_x, double *out_y) ;
#endif

13
User/Module/Chassis.c
View File

@ -82,15 +82,16 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
}
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算,
{
fp64 Nor_Vx,Nor_Vy;//归一化后的数据
normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw + c->pos088 .bmi088.gyro.z;//左前
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左前
}

12
User/Module/config.c
View File

@ -37,7 +37,7 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f,
},
.Pitch_M2006_angle_param = {
.p = 400.0f,
.p = 600.0f,
.i = 0.0f,
.d = 3.0f,
.i_limit = 2000.0f,
@ -96,10 +96,16 @@ static const ConfigParam_t param ={
/*上层其他参数*/
/*运球*/
.DribbleConfig_Config = {
.dribble_flag=0,
.m3508_init_angle = 50,
.m3508_translate_angle = 1200,
.m3508_translate_angle = -930,
.m3508_dribble_Reverse_speed=-3000,
. m3508_dribble_speed= 3000, // 转动速度
.m3508_dribble_init_speed=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
.light_ball_flag=0,//检测球的flag
},
/*投球*/
@ -109,7 +115,7 @@ static const ConfigParam_t param ={
.go1_init_position = -50,
.go1_release_threshold =-210,
.m2006_Screw_init=0,
.Pitch_angle =57,
.Pitch_angle =56,
},

56
User/Module/up.c
View File

@ -3,6 +3,15 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
/*接线
上层用到三个光电
1.PE9 发射用 输出高电平 LIGHTA
2.PE11 运球上3508 输出高电平 LIGHTB
3.PE13 三摩擦检测球 输出高电平LIGHTC
*/
#define GEAR_RATIO_2006 (36) // 2006减速比
#define GEAR_RATIO_3508 (19)
@ -15,8 +24,7 @@
// 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
@ -257,20 +265,19 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
/*投篮*/
if(is_pitch){
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
is_pitch=0;
}
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
is_pitch=0;
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
/*运球*/
RELAY1_TOGGLE(0);//关闭气缸
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
u->motor_target.Dribble_M3508_speed=0;
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
break;
@ -284,7 +291,8 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
Pitch_Process(u,out,c);
break ;
case UP_Adjust:
case UP_Adjust: //测试用
u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
@ -297,6 +305,10 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
u->DribbleContext .DribbleState=DRIBBLE_PROCESS;
}
//光电状态更新
u->DribbleContext .DribbleConfig .light_ball_flag =read_flag_state(HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin));
u->DribbleContext .DribbleConfig .light_3508_flag =read_flag_state(HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin));
Dribble_Process(u,out);
}break ;
@ -344,7 +356,7 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
case PITCH_PULL_TRIGGER:
if(is_reached ( u->motorfeedback .DJmotor_feedback [0].total_angle,u->PitchContext .PitchConfig .m2006_trigger_angle,1.0f)) //当2006的总角度小于1，可以认为已经勾上,误差为1
if(is_reached ( u->motorfeedback .DJmotor_feedback [4].total_angle,u->PitchContext .PitchConfig .m2006_trigger_angle,1.0f)) //当2006的总角度小于1，可以认为已经勾上,误差为1
{
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
}
@ -373,22 +385,36 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
switch (u->DribbleContext.DribbleState) {
case DRIBBLE_TRANSLATE:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_translate_angle,1.0f))
{
u->DribbleContext .DribbleState=DRIBBLE_PROCESS;//到达位置后，转移状态
}
break;
case DRIBBLE_PROCESS:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
if(is_reached(u->motorfeedback .DJmotor_feedback [0].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&&
is_reached(u->motorfeedback .DJmotor_feedback [1].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&&
is_reached(u->motorfeedback .DJmotor_feedback [2].rpm,u->motor_target.Dribble_M3508_speed,70.0f)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
}
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测，反转
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
}
}
if(u->DribbleContext .DribbleConfig .light_ball_flag == 2){//球上升检测，速度给0，转移状态
u->DribbleContext .DribbleState=DRIBBLE_DONE;
}
break;
case DRIBBLE_DONE:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
/*标志位清零*/
u->DribbleContext.DribbleConfig.light_ball_flag=0;
u->DribbleContext.DribbleConfig.light_3508_flag=0;
break;
default:

10
User/Module/up.h
View File

@ -85,11 +85,19 @@ typedef enum {
/* 参数配置结构体 */
typedef struct {
int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
fp32 m3508_init_angle; // 平移前位置
fp32 m3508_translate_angle; // 平移后位置
fp32 m3508_dribble_init_speed;
fp32 m3508_dribble_speed; // 转动速度
fp32 m3508_dribble_Reverse_speed;
/*光电标志位，初始值均为0，触发为1*/
uint8_t light_3508_flag;//3508平移处的光电，0初始，1到位置
uint8_t light_ball_flag;//检测球的flag
} DribbleConfig_t;
/* 状态机上下文 */
@ -99,6 +107,8 @@ typedef struct {
uint8_t is_initialized;
} DribbleContext_t;

2
User/task/chassis_task.c
View File

@ -52,8 +52,6 @@ void Task_Chassis(void *argument)
uint32_t tick = osKernelGetTickCount();
HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除
while(1)
{
#ifdef DEBUG