Compare commits
6 Commits
2403b3bf52
...
2a15ca25ed
| Author | SHA1 | Date | |
|---|---|---|---|
| 2a15ca25ed | |||
| bea23c3860 | |||
| 6cf2fb649b | |||
| 04a3906b2c | |||
| c4bc8288c7 | |||
| de7321138f |
@ -81,6 +81,7 @@ int main(void)
|
||||
|
||||
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
|
||||
|
||||
|
||||
HAL_Init();
|
||||
|
||||
/* USER CODE BEGIN Init */
|
||||
|
||||
2
MDK-ARM/.vscode/keil-assistant.log
vendored
2
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -485,3 +485,5 @@
|
||||
|
||||
[info] Log at : 2025/7/1|14:01:22|GMT+0800
|
||||
|
||||
[info] Log at : 2025/7/7|02:36:22|GMT+0800
|
||||
|
||||
|
||||
@ -134,13 +134,13 @@
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name></Name>
|
||||
<Key>CMSIS_AGDI</Key>
|
||||
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>CMSIS_AGDI</Key>
|
||||
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name></Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -168,72 +168,42 @@
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>UP,0x0A</ItemText>
|
||||
<ItemText>posss</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>chassis,0x0A</ItemText>
|
||||
<ItemText>UP,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd,0x0A</ItemText>
|
||||
<ItemText>chassis,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nucbuf,0x0A</ItemText>
|
||||
<ItemText>nuc_raw</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_fromnuc</ItemText>
|
||||
<ItemText>cmd_fromnuc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>NUC_send,0x0A</ItemText>
|
||||
<ItemText>drop_message,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>NUC_send</ItemText>
|
||||
<ItemText>\\R2\../User/task/chassis_task.c\chassis.param->M3508_param.d</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nuc_raw,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ang_58</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>12</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ang_66</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>13</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_fromnuc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>14</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>Nuc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>15</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rec_flag</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>16</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ctrl</ItemText>
|
||||
<ItemText>\\R2\../User/task/nuc_task.c\cmd_fromnuc</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
@ -938,7 +908,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/bsp</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1218,7 +1188,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/Algorithm</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1390,7 +1360,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Application/User/USB_DEVICE/App</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1434,7 +1404,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Application/User/USB_DEVICE/Target</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
Binary file not shown.
@ -405,4 +405,19 @@ bool is_reached_multiple(float current1, float current2, float current3, float t
|
||||
count++; // 所有条件都满足，计数加 1
|
||||
}
|
||||
return false; // 未满足条件达到阈值，返回 0
|
||||
}
|
||||
/**
|
||||
* @brief 底盘无头模式转换函数
|
||||
* @param vx 输入 X 方向速度（全局坐标系），输出转换后的底盘 X 速度
|
||||
* @param vy 输入 Y 方向速度（全局坐标系），输出转换后的底盘 Y 速度
|
||||
* @param vw 旋转速度（直接控制，不受影响）
|
||||
* @param yaw 当前陀螺仪 yaw 角度（弧度制）
|
||||
*/
|
||||
void ChassisHeadlessMode(double *vx, double *vy, double yaw) {
|
||||
double vx_global = *vx;
|
||||
double vy_global = *vy;
|
||||
|
||||
// 标准二维旋转变换（逆时针为正）
|
||||
*vx = vx_global * cosf(yaw) - vy_global * sinf(yaw);
|
||||
*vy = vx_global * sinf(yaw) + vy_global * cosf(yaw);
|
||||
}
|
||||
@ -165,5 +165,6 @@ void normalize_vector(double x, double y, double *out_x, double *out_y) ;
|
||||
bool is_reached(float current, float target, float mistake) ;
|
||||
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
|
||||
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
|
||||
void ChassisHeadlessMode(double *vx, double *vy, double yaw) ;
|
||||
|
||||
#endif
|
||||
|
||||
@ -35,10 +35,6 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
|
||||
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿
|
||||
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
|
||||
|
||||
c->vofa_send[7] =c->sick_cali.sick_dis[1]-c->sick_cali.sick_dis[2];
|
||||
c->vofa_send[6] = c->sick_cali.sick_dis[0];
|
||||
c->vofa_send[5] =c->sick_cali.sick_dis[1];
|
||||
c->vofa_send[4] =c->sick_cali.sick_dis[2];
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
@ -46,9 +42,11 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
|
||||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||||
c->param = param;
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param));
|
||||
}
|
||||
PID_init(&(c->pid.chassis_3508VecPID[3]), PID_POSITION_D, &(c->param->M3508_param_other));
|
||||
|
||||
PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
|
||||
PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
|
||||
/*sick*/
|
||||
@ -58,6 +56,12 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
|
||||
PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam));
|
||||
PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam));
|
||||
PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam));
|
||||
|
||||
PID_init(&(c->pid.Mid360AngleInPID), PID_POSITION, &(c->param->Mid360AngleInPID_param));
|
||||
PID_init(&(c->pid.Mid360AngleOutPID), PID_POSITION, &(c->param->Mid360AngleOutPID_param));
|
||||
|
||||
|
||||
|
||||
/*修正 */
|
||||
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
|
||||
|
||||
@ -77,10 +81,22 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
//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] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
|
||||
// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
|
||||
// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
|
||||
// c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前
|
||||
//
|
||||
// Chassis_AngleCompensate(c);
|
||||
|
||||
//}
|
||||
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);
|
||||
|
||||
ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw);
|
||||
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
|
||||
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
|
||||
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
|
||||
@ -98,16 +114,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
|
||||
|
||||
/*初始数据*/
|
||||
c->move_vec.Vx = 0;
|
||||
c->move_vec.Vy = 0;
|
||||
c->move_vec.Vw = 0;
|
||||
|
||||
c->to_nuc.send = 0;
|
||||
c->sick_cali.is_reach = 0;
|
||||
|
||||
c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
|
||||
c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
|
||||
c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]);
|
||||
c->move_vec.Vw=0;
|
||||
c->move_vec.Vx = 0;
|
||||
c->move_vec.Vy = 0;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -131,9 +145,16 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
break;
|
||||
case Chassis_Adjust_Sick:
|
||||
|
||||
if(abs(ctrl->cmd_MID360.angle)<1.0f){
|
||||
c->move_vec.Vx = ctrl->cmd_MID360.dis * 3000;
|
||||
}
|
||||
if(-ctrl->cmd_MID360.dis>0.02f){
|
||||
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
|
||||
}
|
||||
|
||||
// sick_calibration(c, ctrl, out);
|
||||
// c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0;
|
||||
break;
|
||||
break;
|
||||
|
||||
}
|
||||
break;
|
||||
@ -141,22 +162,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
case AUTO:
|
||||
switch (c->chassis_ctrl.mode) {
|
||||
case AUTO_MID360:
|
||||
// c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
|
||||
// c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
|
||||
// c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
|
||||
c->move_vec.Vw =-ctrl->cmd_MID360.posw * 3000;
|
||||
|
||||
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||||
c->move_vec.Vw = ctrl->Vw * 8000;
|
||||
c->move_vec.Vx = ctrl->Vy * 8000;
|
||||
c->move_vec.Vy = ctrl->Vx * 8000;
|
||||
// c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
|
||||
// abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||||
|
||||
break;
|
||||
case AUTO_MID360_Pitch:
|
||||
// c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
|
||||
// c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
|
||||
// c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
|
||||
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
|
||||
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
|
||||
c->move_vec.Vx = ctrl->Vy * 8000;
|
||||
c->move_vec.Vy = ctrl->Vx * 8000;
|
||||
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||||
break;
|
||||
case REPOSITION:
|
||||
@ -172,15 +188,9 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
switch (c->chassis_ctrl.mode)
|
||||
{
|
||||
case PB_UP:
|
||||
c->move_vec.Vw = ctrl->Vw * 6000;
|
||||
c->move_vec.Vx = ctrl->Vy * 6000;
|
||||
c->move_vec.Vy = ctrl->Vx * 6000;
|
||||
break ;
|
||||
case PB_MID:
|
||||
case PB_DOWN:
|
||||
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
|
||||
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
|
||||
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
|
||||
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
|
||||
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||||
@ -314,3 +324,4 @@ int8_t Chassis_AngleCompensate(Chassis_t *c)
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -72,7 +72,9 @@ typedef struct
|
||||
|
||||
|
||||
/*该部分决定PID的参数整定在config中修改*/
|
||||
pid_param_t M3508_param;
|
||||
pid_param_t M3508_param;
|
||||
pid_param_t M3508_param_other;
|
||||
|
||||
|
||||
pid_param_t chassis_PICKWzPID_HIGN_param;
|
||||
pid_param_t chassis_PICKWzPID_LOW_param;
|
||||
@ -90,6 +92,12 @@ typedef struct
|
||||
|
||||
pid_param_t Chassis_AngleAdjust_param;
|
||||
|
||||
|
||||
pid_param_t Mid360AngleInPID_param;
|
||||
pid_param_t Mid360AngleOutPID_param;
|
||||
|
||||
|
||||
|
||||
sickparam_t sickparam;
|
||||
|
||||
}Chassis_Param_t;
|
||||
@ -163,6 +171,10 @@ typedef struct{
|
||||
pid_type_def SickCaliVyInPID;
|
||||
pid_type_def SickCaliVyOutPID;
|
||||
|
||||
pid_type_def Mid360AngleInPID;
|
||||
pid_type_def Mid360AngleOutPID;
|
||||
|
||||
|
||||
|
||||
}pid;
|
||||
|
||||
|
||||
@ -75,12 +75,12 @@ static const ConfigParam_t param ={
|
||||
|
||||
/*投球*/
|
||||
.LaunchCfg = {
|
||||
.m2006_init = -130.0f, // M2006初始角度
|
||||
.m2006_init = -140.0f, // M2006初始角度
|
||||
.m2006_trig = 0.0f, // M2006触发角度
|
||||
.go_pull_pos = -214.0f, // go上升去合并扳机，扳机位置
|
||||
.go_up_speed = 12.0f, // 上升速度
|
||||
.go_down_speed = 6.0f, // 下降速度
|
||||
.Pitch_angle = 58, //俯仰角
|
||||
.go_pull_pos = -210.0f, // go上升去合并扳机，扳机位置
|
||||
.go_up_speed = 15.0f, // 上升速度
|
||||
.go_down_speed = 12.0f, // 下降速度
|
||||
.Pitch_angle = 66, //俯仰角
|
||||
.go_init = -50
|
||||
},
|
||||
.PitchCfg = {
|
||||
@ -88,20 +88,20 @@ static const ConfigParam_t param ={
|
||||
.go_init = -50, //仅用在go上电，初始位置
|
||||
.go_release_pos=-200,
|
||||
.Pitch_angle =66,
|
||||
.go_up_speed =12,
|
||||
.go_down_speed =6,
|
||||
.go_up_speed =15,
|
||||
.go_down_speed =10,
|
||||
},
|
||||
.PassCfg={
|
||||
.go_wait =-10,
|
||||
.go_release_pos =-150,
|
||||
.Pitch_angle=66,
|
||||
.go_up_speed =12,
|
||||
.go_down_speed =6,
|
||||
.go_up_speed =15,
|
||||
.go_down_speed =10,
|
||||
},
|
||||
.MID360Cfg={
|
||||
|
||||
.go_release_pos=-200, // GO电机发射位置
|
||||
.go_up_speed=20, // GO电机上升速度
|
||||
.go_up_speed=15, // GO电机上升速度
|
||||
.go_down_speed=10, // GO电机下降速度
|
||||
|
||||
|
||||
@ -111,11 +111,29 @@ static const ConfigParam_t param ={
|
||||
|
||||
|
||||
.M3508_param = {
|
||||
.p = 15.1f,
|
||||
.i = 0.02f,
|
||||
// .p = 15.1f,
|
||||
// .i = 0.02f,
|
||||
// .d = 3.2f,
|
||||
// .i_limit = 200.0f,
|
||||
// .out_limit =6000.0f,
|
||||
.p = 20.5f,
|
||||
.i = 0.00005f,
|
||||
.d = 3.2f,
|
||||
.i_limit = 200.0f,
|
||||
.out_limit =6000.0f,
|
||||
|
||||
|
||||
},
|
||||
|
||||
.M3508_param_other= {
|
||||
|
||||
.p = 16.5f,
|
||||
.i = 0.00001f,
|
||||
.d = 3.2f,
|
||||
.i_limit = 200.0f,
|
||||
.out_limit =6000.0f,
|
||||
|
||||
|
||||
},
|
||||
/*视觉*/
|
||||
.chassis_PICKWzPID_HIGN_param ={
|
||||
@ -235,6 +253,27 @@ static const ConfigParam_t param ={
|
||||
.out_limit =1000.0f,
|
||||
|
||||
},
|
||||
|
||||
.Mid360AngleInPID_param={
|
||||
.p = 0.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit =1000.0f,
|
||||
|
||||
},
|
||||
.Mid360AngleOutPID_param={
|
||||
.p = 10.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit =1000.0f,
|
||||
|
||||
},
|
||||
|
||||
|
||||
|
||||
|
||||
.sickparam={
|
||||
.w_error=2,
|
||||
.xy_error=3,
|
||||
|
||||
@ -23,6 +23,7 @@
|
||||
// 定义继电器控制
|
||||
#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)
|
||||
{
|
||||
u->param = param; /*初始化参数 */
|
||||
@ -40,6 +41,9 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
|
||||
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
|
||||
|
||||
u->go_cmd =u->param ->go_cmd ;
|
||||
|
||||
|
||||
|
||||
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
|
||||
|
||||
|
||||
@ -126,6 +130,12 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
|
||||
|
||||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||||
{
|
||||
/*目标值限位*/
|
||||
abs_limit_min_max_fp(&u->motor_target.go_shoot ,-215.0f,0.0f);
|
||||
abs_limit_min_max_fp(&u->motor_target.Pitch_angle ,48.0f,71.5f);
|
||||
|
||||
|
||||
|
||||
//电机控制 ，传进can
|
||||
DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] ,
|
||||
&u->pid .Shoot_M2006angle ,
|
||||
@ -175,16 +185,9 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
|
||||
up_motor_target_t *target=&u->motor_target ;
|
||||
|
||||
/*config值限位*/
|
||||
abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go_release_pos ,-215.0f,0.0f);
|
||||
abs_limit_min_max_fp(&u->PitchContext.PitchConfig.Pitch_angle ,48.0f,67.0f);
|
||||
|
||||
/*部分数据更新*/
|
||||
static int is_pitch=1;
|
||||
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve);
|
||||
u->vofa_send[0] = u->MID360Context.MID360Cfg.go_release_pos;
|
||||
u->vofa_send[1] = u->motorfeedback.go_data.Pos;
|
||||
u->vofa_send[2] = c->pos;
|
||||
|
||||
switch (c->CMD_CtrlType )
|
||||
{
|
||||
@ -202,7 +205,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
is_pitch=0;
|
||||
} //初始上电
|
||||
//LaunchCfg->go_up_speed=15;
|
||||
target->Pitch_angle =LaunchCfg->Pitch_angle;
|
||||
// target->Pitch_angle =LaunchCfg->Pitch_angle;
|
||||
target->go_pull_speed=LaunchCfg->go_up_speed;
|
||||
target->Shoot_M2006_angle=LaunchCfg->m2006_init;
|
||||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
|
||||
@ -220,7 +223,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
|
||||
break ;
|
||||
case UP_Adjust: //测试用,手动使用pitch下的cfg
|
||||
LaunchCfg->go_up_speed=5;
|
||||
|
||||
pitch_cfg ->go_release_pos += c->Vx ;
|
||||
LaunchCfg->Pitch_angle += c->Vy/100;
|
||||
|
||||
@ -239,7 +242,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
break;
|
||||
case RELAXED:
|
||||
// target->go_shoot= pitch_cfg .
|
||||
target->Pitch_angle = 58;
|
||||
target->Pitch_angle = 66;
|
||||
target->go_shoot = -190.0f;
|
||||
//失控下，最好切手动，手动用pitch
|
||||
pitch_cfg ->go_release_pos=-190;
|
||||
@ -257,13 +260,13 @@ return 0;
|
||||
|
||||
|
||||
//复用发射，
|
||||
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
|
||||
int8_t Pitch_Launch_Sequence(UP_t *u,CMD_t *c, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
|
||||
/*电机位置到达判断*/
|
||||
|
||||
bool is_GoStartReach=is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f); //go开始位置
|
||||
bool is_GoSpeedReach=is_reached(u->motorfeedback.go_data.W,0,1.0f) ; //go速度归零判断
|
||||
bool is_GoEndReach =(u->motorfeedback .go_data .Pos < -209); //go去上拉钩子位置，判断到达
|
||||
bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-10); //2006钩子放下判断
|
||||
bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-5.0f); //2006钩子放下判断
|
||||
bool is_Shoot=(u->motorfeedback.DJmotor_feedback[4].total_angle<-10);//是否发射判断
|
||||
|
||||
|
||||
@ -294,8 +297,6 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
|
||||
if( is_HookDone ){ //当2006的总角度小于1，可以认为已经勾上,误差为1
|
||||
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
|
||||
u->motor_target.go_shoot = EndPos ;
|
||||
// if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
|
||||
// LaunchContext->LaunchState = Launch_SHOOT_WAIT;
|
||||
} break;
|
||||
|
||||
case Launch_SHOOT_WAIT:
|
||||
@ -318,10 +319,12 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
|
||||
LaunchContext_t *LaunchContext = &u->LaunchContext;
|
||||
// 可根据实际需要传入不同的起始和末位置,起始：当前位置
|
||||
LaunchContext->LaunchCfg.go_up_speed= cfg->go_up_speed;
|
||||
LaunchContext->LaunchCfg.go_down_speed= cfg->go_down_speed
|
||||
;
|
||||
LaunchContext->LaunchCfg.go_down_speed= cfg->go_down_speed;
|
||||
abs_limit_min_max_fp(&cfg->go_release_pos ,-215.0f,0.0f);
|
||||
abs_limit_min_max_fp(&cfg->Pitch_angle,48.0f,67.0f);
|
||||
|
||||
Pitch_Launch_Sequence(u, LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
|
||||
|
||||
Pitch_Launch_Sequence(u, c,LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
|
||||
|
||||
|
||||
switch(*state){
|
||||
@ -357,15 +360,27 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
|
||||
PassCfg_t *PassCfg = &u->PassContext.PassCfg;
|
||||
PassState_t *state = &u->PassContext.PassState;
|
||||
up_motor_target_t *target=&u->motor_target ;
|
||||
PassContext_t *PassContext = &u->PassContext;
|
||||
up_motor_target_t *target=&u->motor_target ;
|
||||
LaunchContext_t *LaunchContext = &u->LaunchContext;
|
||||
|
||||
Pass_Sequence_Check(u,c);
|
||||
|
||||
/*俯仰角度，力度转换*/
|
||||
PassCfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.3,4.3,&u->PassContext.Curve);
|
||||
|
||||
|
||||
if (u->PassContext.Curve == CURVE_58) {
|
||||
|
||||
target->Pitch_angle = 58;
|
||||
PassCfg->Pitch_angle=58;
|
||||
}
|
||||
else {
|
||||
target->Pitch_angle = 66;
|
||||
PassCfg->Pitch_angle=66;
|
||||
|
||||
}
|
||||
abs_limit_min_max_fp(&PassCfg->go_release_pos ,-215.0f,0.0f);
|
||||
|
||||
PassCfg ->go_release_pos =
|
||||
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve);
|
||||
|
||||
switch (*state) { //遥控器按键进行状态切换
|
||||
case PASS_STOP:
|
||||
|
||||
@ -376,7 +391,7 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
case PASS_PREPARE:
|
||||
target->go_pull_speed=PassCfg->go_up_speed;
|
||||
|
||||
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
|
||||
Pitch_Launch_Sequence(u,c,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
|
||||
|
||||
|
||||
break;
|
||||
@ -387,7 +402,8 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
}
|
||||
break ;
|
||||
case PASS_POS_PREPARE:
|
||||
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射
|
||||
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射
|
||||
|
||||
break;
|
||||
case PASS_COMPLETE:
|
||||
break;
|
||||
@ -403,20 +419,28 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
|
||||
LaunchContext_t *LaunchContext = &u->LaunchContext;
|
||||
MID360Context_t *MID360Context=&u->MID360Context;
|
||||
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
|
||||
if(u->MID360Context.Curve==CURVE_58){
|
||||
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.25,3.2,4.3,&u->MID360Context.Curve);
|
||||
}
|
||||
else {
|
||||
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve);
|
||||
|
||||
}
|
||||
|
||||
u->PitchContext.PitchConfig.go_release_pos =MID360Cfg->go_release_pos;
|
||||
/*俯仰角度，力度转换，加数值限位*/
|
||||
if(c->cmd_MID360.dis<1.3f){
|
||||
MID360Cfg->go_release_pos=-150.0f;
|
||||
}else {
|
||||
if(u->MID360Context.Curve==CURVE_58){
|
||||
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.20f,3.3,4.3,&u->MID360Context.Curve);
|
||||
}
|
||||
else {
|
||||
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.1f,3.3,4.3,&u->MID360Context.Curve);
|
||||
|
||||
}
|
||||
}
|
||||
if (u->MID360Context.Curve == CURVE_58) {
|
||||
target->Pitch_angle = 58;
|
||||
} else {
|
||||
target->Pitch_angle = 66;
|
||||
}
|
||||
|
||||
}
|
||||
abs_limit_min_max_fp(&MID360Cfg->go_release_pos ,-215.0f,0.0f);
|
||||
|
||||
|
||||
LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
|
||||
LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
|
||||
|
||||
@ -429,13 +453,14 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
|
||||
MID360Context->IsLaunch=1;
|
||||
LaunchContext->LaunchState = Launch_PREPARE;
|
||||
}
|
||||
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
|
||||
Pitch_Launch_Sequence(u,c,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
|
||||
|
||||
break ;
|
||||
|
||||
case AUTO_MID360:
|
||||
target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init;
|
||||
MID360Context->IsLaunch=0;
|
||||
// u->cmd_from_nuc.lock=0;
|
||||
break ;
|
||||
default:
|
||||
break;
|
||||
|
||||
@ -190,8 +190,8 @@ typedef struct{
|
||||
|
||||
typedef struct{
|
||||
|
||||
uint8_t up_task_run;
|
||||
const UP_Param_t *param;
|
||||
uint8_t up_task_run;
|
||||
const UP_Param_t *param;
|
||||
LaunchContext_t LaunchContext;
|
||||
/*投篮过程*/
|
||||
PitchContext_t PitchContext;
|
||||
@ -215,6 +215,7 @@ typedef struct{
|
||||
uint32_t ecd;
|
||||
float angle;
|
||||
}Encoder;
|
||||
|
||||
}motorfeedback;
|
||||
|
||||
|
||||
@ -234,7 +235,11 @@ typedef struct{
|
||||
|
||||
|
||||
}final_out;
|
||||
|
||||
// struct {
|
||||
// float angle;
|
||||
// float dis;
|
||||
// uint8_t lock;
|
||||
// }cmd_from_nuc;
|
||||
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
|
||||
|
||||
fp32 vofa_send[8];
|
||||
@ -254,7 +259,7 @@ int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed
|
||||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
|
||||
int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c);
|
||||
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c);
|
||||
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
|
||||
int8_t Pitch_Launch_Sequence(UP_t *u,CMD_t *c, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
|
||||
int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c);
|
||||
|
||||
|
||||
|
||||
@ -65,15 +65,17 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
|
||||
}
|
||||
|
||||
|
||||
// 计算66度曲线（偏上）
|
||||
|
||||
// 计算66度曲线（偏上）
|
||||
static float curve_66(float d) {
|
||||
return 3.7028f * d * d + 11.2126f * d -142.9446f;
|
||||
return 2.9851104*pow(d,4) -36.164382*pow(d,3) + 159.54844*pow(d,2) -273.62856*d + 43.092452;
|
||||
}
|
||||
// 计算58度曲线（偏下）
|
||||
// 计算58度曲线（偏下）
|
||||
static float curve_58(float d) {
|
||||
return 0.9242f * d * d + 19.4246f * d - 154.9055f;
|
||||
// return 0.9242f * d * d + 19.4246f * d - 154.9055f;
|
||||
// return 2.638517*pow(d,4) -47.996138*pow(d,3) + 325.38515*pow(d,2) -950.18155*d + 919.86543;
|
||||
return 2.7775f * d * d - 0.5798f * d - 113.1488;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
@ -81,13 +83,14 @@ static float curve_58(float d) {
|
||||
迟滞区x-y
|
||||
曲线x重合区，根据当前函数和变化方向切换
|
||||
*/
|
||||
int abdddd=0;
|
||||
fp32 ang_58;
|
||||
fp32 ang_58;
|
||||
fp32 ang_66;
|
||||
|
||||
|
||||
float CurveChange(float d, float x, float y, CurveType *cs)
|
||||
{
|
||||
if (d<3.2) abdddd++;
|
||||
|
||||
|
||||
if (*cs == CURVE_66) {
|
||||
if (d > y) {
|
||||
*cs = CURVE_58;
|
||||
@ -167,3 +170,4 @@ int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -37,19 +37,11 @@ typedef struct __attribute__((packed)) {
|
||||
typedef struct __attribute__((packed)) {
|
||||
|
||||
Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确
|
||||
uint8_t status; /* 控制命令 */
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
float posy; /*pitch轴*/
|
||||
float posx; /*yaw轴*/
|
||||
char cmd;
|
||||
float dis; /*距离*/
|
||||
}pick;
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
float vx; /* x轴移动速度 */
|
||||
float vy; /* y轴移动速度 */
|
||||
float wz; /* z轴转动速度 */
|
||||
float dis;
|
||||
}navi;
|
||||
|
||||
} Protocol_DownDataChassis_t;
|
||||
@ -83,17 +75,17 @@ typedef struct __attribute__((packed)) {
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
fp32 status;
|
||||
uint8_t status;
|
||||
|
||||
|
||||
} Protocol_UpDataCMD_t;
|
||||
|
||||
/* 视觉 -> 电控 上层机构数据结构体*/
|
||||
typedef struct __attribute__((packed)) {
|
||||
|
||||
uint8_t status; /* 控制命令 */
|
||||
|
||||
float angle; // 角度信息
|
||||
float dis; // 位移信息
|
||||
} Protocol_DownDataUpper_t;
|
||||
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
Protocol_UpDataMCU_t data;
|
||||
} Protocol_UpPackageMCU_t;
|
||||
@ -106,6 +98,7 @@ typedef struct __attribute__((packed)) {
|
||||
typedef struct __attribute__((packed)) {
|
||||
Protocol_DownDataUpper_t data;
|
||||
} Protocol_DownPackageUpper_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -88,7 +88,7 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
|
||||
const uint8_t *raw) {
|
||||
if (feedback == NULL || raw == NULL) return;
|
||||
switch(raw[1])//判断编码器id
|
||||
{
|
||||
{
|
||||
case 0x01:
|
||||
|
||||
switch(raw[2])//判断指令id
|
||||
|
||||
@ -87,44 +87,25 @@ return 0;
|
||||
|
||||
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
|
||||
if(cmd == NULL) return -1;
|
||||
if(n == NULL) return -1;
|
||||
if(n == NULL) return -1;
|
||||
|
||||
cmd->cmd_status = n->status_fromnuc;
|
||||
cmd->raw_status = n->ctrl_status;
|
||||
|
||||
for (int i = 0; i < 7; ++i)
|
||||
{ // 从最低位到最高位遍历
|
||||
cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
|
||||
}
|
||||
switch(cmd->cmd_status){
|
||||
|
||||
|
||||
case MID:
|
||||
cmd->cmd_MID360.posx = n->MID360.vx;
|
||||
cmd->cmd_MID360.posy = n->MID360.vy;
|
||||
cmd->cmd_MID360.posw = n->MID360.wz;
|
||||
|
||||
cmd->pos =n->MID360 .pos ;
|
||||
break;
|
||||
|
||||
case VISION :
|
||||
cmd ->cmd_pick .posx =n->camera.data1 ;
|
||||
cmd ->cmd_pick .posy =n->camera.data2 ;
|
||||
cmd ->cmd_pick .posw =n->camera.data3 ;
|
||||
cmd->cmd_MID360.angle = n->MID360.angle;
|
||||
cmd->cmd_MID360.dis = n->MID360.dis;
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
cmd->cmd_MID360.posx = 0;
|
||||
cmd->cmd_MID360.posy = 0;
|
||||
cmd->cmd_MID360.posw = 0;
|
||||
|
||||
|
||||
cmd->pos =0 ;
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -156,7 +137,7 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
|
||||
|
||||
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
|
||||
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust_Sick; //左上，右上，手动调整
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，
|
||||
|
||||
if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust;
|
||||
}
|
||||
@ -169,15 +150,15 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
|
||||
|
||||
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =AUTO_MID360; //左中，右中，无模式
|
||||
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =REPOSITION; //左中，右下，视觉
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉
|
||||
|
||||
}
|
||||
else if(rc->dr16.sw_l==CMD_SW_DOWN)
|
||||
{
|
||||
cmd ->CMD_CtrlType =PASS_BALL;
|
||||
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =PB_UP; //左下，右上，无模式
|
||||
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = PB_MID; //左下，右中，无模式
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =PB_DOWN; //左下，右上，无模式
|
||||
cmd ->CMD_CtrlType =RCcontrol;
|
||||
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
|
||||
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -42,19 +42,12 @@ typedef enum{
|
||||
Dribbl_transfer
|
||||
}CMD_mode_t;
|
||||
typedef struct {
|
||||
uint8_t status_fromnuc;
|
||||
uint8_t ctrl_status;
|
||||
|
||||
|
||||
struct
|
||||
{
|
||||
fp32 vx;
|
||||
fp32 vy;
|
||||
fp32 wz;
|
||||
|
||||
fp32 pos;
|
||||
|
||||
fp32 angle;
|
||||
|
||||
char flag;
|
||||
fp32 dis;
|
||||
|
||||
}MID360;
|
||||
struct
|
||||
@ -149,9 +142,9 @@ typedef struct {
|
||||
}cmd_pick;
|
||||
struct
|
||||
{
|
||||
fp32 posx;
|
||||
fp32 posy;
|
||||
fp32 posw;
|
||||
fp32 angle;
|
||||
fp32 dis;
|
||||
|
||||
}cmd_MID360;
|
||||
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@ volatile uint32_t drop_message = 0;
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
|
||||
uint8_t nucbuf[25];
|
||||
uint8_t nucbuf[10];
|
||||
char SendBuffer[7];
|
||||
|
||||
static void NUC_CBLTCallback(void)
|
||||
@ -58,86 +58,29 @@ int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
|
||||
return DEVICE_ERR_NULL;
|
||||
union
|
||||
{
|
||||
float x[5];
|
||||
char data[20];
|
||||
float x[2];
|
||||
char data[8];
|
||||
} instance;
|
||||
if (nucbuf[0] != HEAD)
|
||||
goto error; // 发送ID不是底盘
|
||||
else
|
||||
{
|
||||
n->status_fromnuc = nucbuf[1];
|
||||
n->ctrl_status = nucbuf[2];
|
||||
switch (n->status_fromnuc)
|
||||
{
|
||||
case VISION: // 控制帧0x02
|
||||
/* 协议格式
|
||||
0xFF HEAD
|
||||
0x02 控制帧
|
||||
0x01 相机帧
|
||||
vx fp32
|
||||
vy fp32
|
||||
wz fp32
|
||||
0xFE TAIL
|
||||
*/
|
||||
|
||||
instance.data[3] = nucbuf[3];
|
||||
instance.data[2] = nucbuf[4];
|
||||
instance.data[1] = nucbuf[5];
|
||||
instance.data[0] = nucbuf[6];
|
||||
n->camera.data1 = instance.x[0]; //
|
||||
instance.data[7] = nucbuf[7];
|
||||
instance.data[6] = nucbuf[8];
|
||||
instance.data[5] = nucbuf[9];
|
||||
instance.data[4] = nucbuf[10];
|
||||
n->camera.data2 = instance.x[1]; //
|
||||
instance.data[11] = nucbuf[11];
|
||||
instance.data[10] = nucbuf[12];
|
||||
instance.data[9] = nucbuf[13];
|
||||
instance.data[8] = nucbuf[14];
|
||||
n->camera.data3 = instance.x[2]; //
|
||||
break;
|
||||
case MID: // 控制帧0x08
|
||||
/* 协议格式
|
||||
0xFF HEAD
|
||||
0x09 控制帧
|
||||
0x01 相机帧
|
||||
vx fp32
|
||||
vy fp32
|
||||
wz fp32
|
||||
0xFE TAIL
|
||||
*/
|
||||
if (nucbuf[24] != TAIL)
|
||||
if (nucbuf[9] != TAIL)
|
||||
goto error;
|
||||
instance.data[3] = nucbuf[6];
|
||||
instance.data[2] = nucbuf[5];
|
||||
instance.data[1] = nucbuf[4];
|
||||
instance.data[0] = nucbuf[3];
|
||||
n->MID360.vx = instance.x[0]; //
|
||||
instance.data[7] = nucbuf[10];
|
||||
instance.data[6] = nucbuf[9];
|
||||
instance.data[5] = nucbuf[8];
|
||||
instance.data[4] = nucbuf[7];
|
||||
n->MID360.vy = instance.x[1]; //
|
||||
instance.data[11] = nucbuf[14];
|
||||
instance.data[10] = nucbuf[13];
|
||||
instance.data[9] = nucbuf[12];
|
||||
instance.data[8] = nucbuf[11];
|
||||
n->MID360.wz = instance.x[2]; //
|
||||
instance.data[15] = nucbuf[18];
|
||||
instance.data[14] = nucbuf[17];
|
||||
instance.data[13] = nucbuf[16];
|
||||
instance.data[12] = nucbuf[15];
|
||||
n->MID360.pos = instance.x[3]; //
|
||||
instance.data[19] = nucbuf[22];
|
||||
instance.data[18] = nucbuf[21];
|
||||
instance.data[17] = nucbuf[20];
|
||||
instance.data[16] = nucbuf[19];
|
||||
n->MID360.angle = instance.x[4]; //
|
||||
|
||||
|
||||
instance.data[3] = nucbuf[4];
|
||||
instance.data[2] = nucbuf[3];
|
||||
instance.data[1] = nucbuf[2];
|
||||
instance.data[0] = nucbuf[1];
|
||||
n->MID360.angle = instance.x[0]; //
|
||||
instance.data[7] = nucbuf[8];
|
||||
instance.data[6] = nucbuf[7];
|
||||
instance.data[5] = nucbuf[6];
|
||||
instance.data[4] = nucbuf[5];
|
||||
n->MID360.dis = instance.x[1]; //
|
||||
|
||||
n->MID360.flag = nucbuf[23]; //
|
||||
|
||||
break;
|
||||
}
|
||||
nuc->unc_online = true; // 设置为在线状态
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
@ -34,7 +34,7 @@ static Chassis_Ctrl_t ctrl;
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
fp32 freq;
|
||||
|
||||
/**
|
||||
* \brief 底盘任务
|
||||
@ -50,7 +50,8 @@ void Task_Chassis(void *argument)
|
||||
Chassis_init(&chassis,&(task_runtime.config.config->chassis),TASK_FREQ_CHASSIS);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
uint32_t last_tick=tick;
|
||||
|
||||
|
||||
while(1)
|
||||
{
|
||||
@ -59,6 +60,8 @@ void Task_Chassis(void *argument)
|
||||
task_runtime.freq.chassis=TASK_FREQ_CHASSIS;
|
||||
task_runtime.last_up_time.chassis=tick;
|
||||
#endif
|
||||
freq=(tick-last_tick)/1000.0f;
|
||||
last_tick =tick;
|
||||
|
||||
/*imu数据获取*/
|
||||
osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
|
||||
|
||||
@ -16,7 +16,7 @@ static CMD_NUC_t Nuc;
|
||||
#endif
|
||||
|
||||
|
||||
int rec_flag=0;
|
||||
|
||||
|
||||
void Task_cmd(void *argument){
|
||||
(void)argument; /*未使用传递参数 消除警告*/
|
||||
@ -38,6 +38,8 @@ void Task_cmd(void *argument){
|
||||
osKernelLock(); /*锁住RTOS内核调度*/
|
||||
|
||||
/*将各任务接收到的原始数据解析为通用的控制命令*/
|
||||
|
||||
|
||||
|
||||
|
||||
if((osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)){ //nuc
|
||||
@ -47,7 +49,8 @@ void Task_cmd(void *argument){
|
||||
memset(&Nuc, 0, sizeof(CMD_NUC_t));
|
||||
}
|
||||
CMD_ParseNuc(&cmd,&Nuc);
|
||||
|
||||
|
||||
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
|
||||
CMD_ParseRc(&cmd, &rc_ctrl);
|
||||
|
||||
|
||||
@ -24,7 +24,6 @@ int a=0;
|
||||
|
||||
|
||||
|
||||
|
||||
void Task_nuc(void *argument){
|
||||
(void)argument; /**/
|
||||
|
||||
@ -40,7 +39,6 @@ void Task_nuc(void *argument){
|
||||
#ifdef DEBUG
|
||||
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
a++;
|
||||
NUC_StartReceiving();
|
||||
if (NUC_WaitDmaCplt()){
|
||||
NUC_RawParse(&cmd_fromnuc, &nuc_raw);
|
||||
|
||||
@ -13,14 +13,14 @@
|
||||
|
||||
/* 所有任务都要定义自己的任务运行频率 */
|
||||
|
||||
// 分配的频率该如何给定?
|
||||
|
||||
#define TASK_FREQ_CHASSIS (900u)
|
||||
#define TASK_FREQ_UP (900u)
|
||||
|
||||
#define TASK_FREQ_CTRL_CMD (100u)
|
||||
#define TASK_FREQ_NUC (100u)
|
||||
#define TASK_FREQ_CAN (1000u)
|
||||
#define TASK_FREQ_RC (1000u)
|
||||
#define TASK_FREQ_RC (100u)
|
||||
|
||||
#define TASK_FREQ_ERROR_DTC (3u)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user