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

sick

Browse Source
This commit is contained in:
ZHAISHUI04 2025-06-24 21:01:44 +08:00
parent 8570b03ca2
commit 4859c69fad
17 changed files with 132 additions and 507 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -471,3 +471,7 @@
[info] Log at : 2025/6/22|14:26:08|GMT+0800 [info] Log at : 2025/6/22|14:26:08|GMT+0800
[info] Log at : 2025/6/23|20:59:34|GMT+0800
[info] Log at : 2025/6/24|03:28:50|GMT+0800

59
MDK-ARM/R2.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel> <nTsel>6</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@ -114,13 +114,13 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key> <Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U00260035480000034E575152 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name> <Name>-U00260035480000034E575152 -O206 -SF1000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -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>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -140,12 +140,12 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name></Name> <Name>(105=-1,-1,-1,-1,0)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S9 -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> <Name>-X"Any" -UAny -O206 -S0 -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>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -158,73 +158,38 @@
<Ww> <Ww>
<count>0</count> <count>0</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText> <ItemText>NUC_send,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>1</count> <count>1</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText> <ItemText>SendBuffer,0x10</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>2</count> <count>2</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText> <ItemText>f,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>NUC_send,0x0A</ItemText> <ItemText>a,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>4</count> <count>4</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>pid,0x0A</ItemText> <ItemText>chassis</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>pid_param,0x0A</ItemText> <ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>6</count> <count>6</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd_rc,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>cbuf,0x10</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>cmd</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf,0x0A</ItemText> <ItemText>nucbuf,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>LD_raw,0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -273,7 +238,7 @@
<EnableFlashSeq>0</EnableFlashSeq> <EnableFlashSeq>0</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>5000000</DbgClock> <DbgClock>1000000</DbgClock>
</DebugDescription> </DebugDescription>
</TargetOption> </TargetOption>
</Target> </Target>

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

Binary file not shown.

2
User/Algorithm/error_detect.h
View File

@ -1,6 +1,4 @@
#ifndef ERROR_DETECT_H #ifndef ERROR_DETECT_H
#define ERROR_DETECT_H #define ERROR_DETECT_H
#include "struct_typedef.h" #include "struct_typedef.h"

43
User/Module/Chassis.c
View File

@ -48,7 +48,7 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam)); PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam));
PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam)); PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam));
PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam)); PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam));
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波 LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波 LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波
@ -60,8 +60,6 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
return CHASSIS_OK; return CHASSIS_OK;
} }
fp32 gyro_kp = 1.0f;
fp32 PIAN = 0;
fp32 pian_yaw; fp32 pian_yaw;
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; fp64 Nor_Vx, Nor_Vy;
@ -107,7 +105,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->move_vec.Vw = ctrl->Vw * 6000; c->move_vec.Vw = ctrl->Vw * 6000;
break; break;
case Chassis_Adjust: case Chassis_Adjust_Sick:
sick_calibration(c, ctrl, out); sick_calibration(c, ctrl, out);
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0; c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
@ -127,7 +125,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
abs_limit_fp(&c->move_vec.Vx, 5000.0f); abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f);
c->NUC_send.send[0] = 1; // c->NUC_send.send[0] = 1;
break; break;
case AUTO_MID360_Pitch: case AUTO_MID360_Pitch:
@ -138,6 +136,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->move_vec.Vy = ctrl->Vx * 6000; c->move_vec.Vy = ctrl->Vx * 6000;
break; break;
case PassBall: //
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);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break ;
default: default:
break; break;
@ -194,7 +200,7 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
fp32 diff_yaw = -(fp32)(sick1 - sick2); fp32 diff_yaw = -(fp32)(sick1 - sick2);
fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set); fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set); fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0; c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0;
@ -219,37 +225,20 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
return CHASSIS_OK; return CHASSIS_OK;
} }
pid_type_def pid;
pid_param_t pid_param={
.p = 10.0f,
.i = 0.00f,
.d = 0.00f,
.i_limit = 2000.0f,
.out_limit =5000.0f,
};
fp32 cur_angle,pian_angel;
int8_t Chassis_AngleCompensate(Chassis_t *c) int8_t Chassis_AngleCompensate(Chassis_t *c)
{ {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
// int is_in;//是否初始化 static fp32 pian_angel,cur_angle;
// if(is_in==0){
PID_init(&pid,PID_POSITION,&pid_param);
// is_in=1;
// }
if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0)) if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
{ {
pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw); pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
} }
else { else {
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw); cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0; pian_angel=0;
} }
pian_yaw = PID_calc(&pid,pian_angel,0); pian_yaw = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
} }

5
User/Module/Chassis.h
View File

@ -83,7 +83,7 @@ typedef struct
pid_param_t Sick_CaliWparam; pid_param_t Sick_CaliWparam;
pid_param_t Sick_CaliYparam; pid_param_t Sick_CaliYparam;
pid_param_t Sick_CaliXparam; pid_param_t Sick_CaliXparam;
pid_param_t Chassis_AngleAdjust_param;
sickparam_t sickparam; sickparam_t sickparam;
@ -100,6 +100,7 @@ typedef struct
}ChassisMove_Vec; }ChassisMove_Vec;
typedef struct typedef struct
{ {
fp32 send[4]; fp32 send[4];
}NUC_send_t; }NUC_send_t;
@ -154,7 +155,7 @@ typedef struct{
/*存在较低误差*/ /*存在较低误差*/
pid_type_def chassis_PICKWzPID_LOW; pid_type_def chassis_PICKWzPID_LOW;
pid_type_def Chassis_AngleAdjust;
pid_type_def SickCaliWzPID; pid_type_def SickCaliWzPID;
pid_type_def SickCaliVxPID; pid_type_def SickCaliVxPID;

47
User/Module/config.c
View File

@ -58,28 +58,6 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.Dribble_M3508_speed_param={ //三摩擦速度环
.p = 30.0f,
.i = 0.45f,
.d = 0.0f,
.i_limit = 3000.0f,
.out_limit = 5000.0f,
},
.Dribble_translate_M3508_speed_param={//平移用3508速度环
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.Dribble_translate_M3508_angle_param= { //平移用3508角度环
.p = 25.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.out_limit = 3000.0f,
},
.go_cmd={ .go_cmd={
.id =0, .id =0,
@ -94,20 +72,7 @@ static const ConfigParam_t param ={
/*上层其他参数*/ /*上层其他参数*/
/*运球*/
.DribbleCfg = {
.dribble_flag=0,
.m3508_init_ang = -5,
.m3508_translate_angle = 1000,
.rev_spd=-2000,
.spd= 4200, // 转动速度
.init_spd=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
.light_ball_flag=0,//检测球的flag
},
/*投球*/ /*投球*/
.PitchCfg = { .PitchCfg = {
.m2006_init =-150, //释放的角度 .m2006_init =-150, //释放的角度
@ -167,11 +132,19 @@ static const ConfigParam_t param ={
.i_limit = 500.0f, .i_limit = 500.0f,
.out_limit =3000.0f, .out_limit =3000.0f,
}, },
.Chassis_AngleAdjust_param={
.p = 10.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =1000.0f,
},
.sickparam={ .sickparam={
.w_error=5, .w_error=5,
.xy_error=5, .xy_error=5,
.x_set=927, .x_set=600,
.y_set=1255, .y_set=100,
}, },

250
User/Module/up.c
View File

@ -5,11 +5,6 @@
#include "bsp_delay.h" #include "bsp_delay.h"
/*接线 /*接线
上层用到三个光电
1.PE9 发射用 输出高电平 LIGHTA
2.PE11 运球上3508 输出高电平 LIGHTB
3.PE13 三摩擦检测球 输出高电平LIGHTC
*/ */
@ -42,24 +37,10 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param )); PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param ));
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param )); PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param ));
PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param ));
for(int i=0;i<3;i++){
PID_init (&u->pid .Dribble_M3508_speed[i] ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param ));
}
u->go_cmd =u->param ->go_cmd ; u->go_cmd =u->param ->go_cmd ;
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f); LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
// 初始化上层状态机
if (!u->DribbleCont .is_init) { //检查是否为第一次运行状态机，运球
u->DribbleCont .DribbleConfig = u->param ->DribbleCfg ;//赋值
u->DribbleCont .DribbleState = DRIBBLE_PREPARE;
u->DribbleCont .is_init = 1;
}
if (!u->Pitch_Cfg .is_init) { if (!u->Pitch_Cfg .is_init) {
u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值 u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
@ -95,10 +76,6 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
} }
u->cmd =c; u->cmd =c;
/*光电状态更新*/
u->DribbleCont .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
u->DribbleCont .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
return 0; return 0;
} }
@ -202,21 +179,10 @@ int8_t GO_SendData(UP_t *u, float pos, float limit)
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{ {
//电机控制 ，传进can //电机控制 ，传进can
DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed ,
u->motor_target .Dribble_translate_M3508_angle );
DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] , DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] ,
&u->pid .Shoot_M2006angle , &u->pid .Shoot_M2006angle ,
&u->pid .Shoot_M2006speed , &u->pid .Shoot_M2006speed ,
u->motor_target .Shoot_M2006_angle ); u->motor_target .Shoot_M2006_angle );
// DJ_Angle_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5] ,
// &u->pid .Pitch_M2006_angle ,
// &u->pid .Pitch_M2006_speed ,
// u->motor_target .Pitch_M2006_angle );
DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed, DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
(PID_calc (&(u->pid .Pitch_M2006_angle ), (PID_calc (&(u->pid .Pitch_M2006_angle ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle)) u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
@ -252,7 +218,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
/*指针简化*/ /*指针简化*/
PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig; PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;
DribbleCfg_t *DribbleCont = &u->DribbleCont.DribbleConfig;
up_motor_target_t *target=&u->motor_target ; up_motor_target_t *target=&u->motor_target ;
static int is_pitch=1; static int is_pitch=1;
@ -278,17 +243,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ; target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
/*运球*/
RELAY1_TOGGLE(0);//关闭气缸
for(int i=0;i<3;i++){
target->Dribble_M3508_speed[i]=0;
}
target->Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_translate_angle;
u->DribbleCont .DribbleState = DRIBBLE_PREPARE; //重置最初状态
break; break;
case Pitch : case Pitch :
@ -309,16 +263,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos; target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos;
target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle; target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
break ; break ;
case Dribble:
{
if(u->DribbleCont.DribbleState== DRIBBLE_PREPARE){
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;
}
//光电状态更新
Dribble_Process(u,out);
}break ;
default: default:
@ -335,12 +279,8 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮 u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
} }
/*根据距离实时计算所需pos*/ /*根据距离实时计算所需pos*/
pitch_cfg ->go_release_pos=c->pos; pitch_cfg ->go_release_pos=c->pos;
// if(u->CoContext .CoState == CoPITCH){
// u->CoContext.CoState=CoPITCH_MID360;
// }
// Co_Process(u,out);
Pitch_Process(u,out); Pitch_Process(u,out);
break ; break ;
@ -349,8 +289,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
target->Shoot_M2006_angle =pitch_cfg ->m2006_init ; target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;
u->Pitch_Cfg .PitchState = PITCH_PREPARE; u->Pitch_Cfg .PitchState = PITCH_PREPARE;
u->DribbleCont .DribbleState = DRIBBLE_PREPARE;
break ; break ;
@ -371,8 +309,6 @@ return 0;
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
{ {
@ -425,187 +361,3 @@ return 0;
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
static int common_speed_flag=0;//是否共速
static int common_speed_reverse_flag=0;//是否共速
DribbleCfg_t *DribbleCfg = &u->DribbleCont.DribbleConfig;
up_motor_target_t *target=&u->motor_target ;
DribbleState_t *DribbleState=&u->DribbleCont.DribbleState;
switch (u->DribbleCont.DribbleState) {
case DRIBBLE_TRANSLATE:
target->Dribble_translate_M3508_angle =DribbleCfg->m3508_translate_angle;//平行移动
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
{
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
}
break;
case DRIBBLE_PROCESS_DOWN:
target->Dribble_M3508_speed[0]=DribbleCfg->spd;
target->Dribble_M3508_speed[1]=DribbleCfg->spd;
target->Dribble_M3508_speed[2]=DribbleCfg->spd;
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm,
u->motorfeedback .DJmotor_feedback [2].rpm,
DribbleCfg->spd,
DribbleCfg->spd,
DribbleCfg->spd, 50.0f,50)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
common_speed_flag =1;
}
if(common_speed_flag){
if(u->DribbleCont .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
target->Dribble_M3508_speed[0]=DribbleCfg->rev_spd;
target->Dribble_M3508_speed[1]=DribbleCfg->rev_spd;
target->Dribble_M3508_speed[2]=DribbleCfg->rev_spd;
*DribbleState=DRIBBLE_PROCESS_UP;
}
}
break;
case DRIBBLE_PROCESS_UP:
common_speed_flag =0;
if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [1].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [2].rpm<-500)
){
common_speed_reverse_flag=1;
}
if(common_speed_reverse_flag){
if(DribbleCfg->light_ball_flag == 1){
*DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸
}
}
break ;
case DRIBBLE_DONE:
common_speed_reverse_flag=0;
for(int i=0;i<3;i++){
target->Dribble_M3508_speed[i]=DribbleCfg->init_spd ;//三摩擦速度归0
}
/*标志位清零*/
DribbleCfg->light_ball_flag=0;
DribbleCfg->light_3508_flag=0;
break;
default:
// 处理未知状态
break;
}
return 0;
}
// CoPREPARE, // 准备阶段
// CoTRANSLATE_OUT,//上方平移,去运球
// CoDRIBBLE, //运球阶段
// CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
// CoPITCH, //发射
// CoDONE,
//int8_t Trans_Process (UP_t *u, CAN_Output_t *out)
//{
//
//}
int8_t Co_Process(UP_t *u, CAN_Output_t *out)
{
switch(u->CoContext .CoState )
{
case CoPREPARE:
break ;
case CoTRANSLATE_OUT:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 500)//速度为0认为卡主
{
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态
u->Pitch_Cfg .PitchState = PITCH_START;
u->CoContext .CoState =CoDRIBBLE;
}
break;
case CoDRIBBLE:
Dribble_Process(u,out); //状态停在DRIBBLE_DONE
Pitch_Process(u,out); //状态停在PITCH_LAUNCHING
//状态停在对应位置时,平移去给发射送球
if(u->DribbleCont .DribbleState ==DRIBBLE_DONE && u->Pitch_Cfg.PitchState ==PITCH_LAUNCHING)
{
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_init_ang;
}
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleCont .DribbleConfig.m3508_init_ang,1.0f))
{
u->motor_target.Dribble_M3508_speed[0]=1000;
u->motor_target.Dribble_M3508_speed[1]=1000;
u->motor_target.Dribble_M3508_speed[2]=1000;
u->CoContext .CoState =CoTRANSLATE_IN;
}
break ;
case CoTRANSLATE_IN:
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm,
u->motorfeedback .DJmotor_feedback [2].rpm,
u->motor_target.Dribble_M3508_speed [0],
u->motor_target.Dribble_M3508_speed [1],
u->motor_target.Dribble_M3508_speed[2],
50.0f,100)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸,送给发射
}
if(u->DribbleCont .DribbleConfig.light_ball_flag == 0)
{
u->motor_target.Dribble_M3508_speed[0]=0;
u->motor_target.Dribble_M3508_speed[1]=0;
u->motor_target.Dribble_M3508_speed[2]=0;
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
}
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主,卡到最右端
{
u->CoContext .CoState =CoPITCH;
}
break ;
case CoPITCH:
case CoPITCH_MID360:
u->motor_target .go_shoot=u->Pitch_Cfg.PitchConfig.go_init;
break ;
break ;
}
}

78
User/Module/up.h
View File

@ -27,27 +27,6 @@
*/ */
/*配合过程状态*/
typedef enum {
CoPREPARE, // 准备阶段
CoTRANSLATE_OUT,//上方平移,去运球
CoDRIBBLE, //运球和蓄力阶段,再平移回去
CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
CoPITCH, //发射
CoPITCH_MID360, //雷达发射
CoDONE,
}CoState_t;
/*总配合上下文*/
typedef struct {
CoState_t CoState;
uint8_t is_init ;
} CoCon_t;
/* 投球状态定义 */ /* 投球状态定义 */
typedef enum { typedef enum {
@ -72,16 +51,7 @@ typedef struct {
fp32 go_release_pos;//go松开，发射位置 fp32 go_release_pos;//go松开，发射位置
} PitchCfg_t; } PitchCfg_t;
//fp32 go_pull_pos;
// fp32 m2006_init; // M2006初始角度
// fp32 m2006_trig; // M2006触发角度
// fp32 go_init; // GO初始位置
// fp32 go_release; // GO释放阈值
// fp32 go_recv; // GO接球位置
// fp32 screw_init; // 螺杆初始值
// fp32 pitch_ang; // 投球角度
// fp32 pull_spd; // 拉动速度
/* 投球控制上下文 */
typedef struct { typedef struct {
PitchState_t PitchState; PitchState_t PitchState;
PitchCfg_t PitchConfig; PitchCfg_t PitchConfig;
@ -99,33 +69,6 @@ typedef enum {
DRIBBLE_DONE // 运球结束 DRIBBLE_DONE // 运球结束
} DribbleState_t; } DribbleState_t;
/* 参数配置结构体 */
typedef struct {
int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
fp32 m3508_init_ang; // 3508平移前位置
fp32 m3508_translate_angle; // 平移后位置
fp32 init_spd;
fp32 spd; // 转动速度
fp32 rev_spd;
/*光电标志位，初始值均为0，触发为1*/
int light_3508_flag;//3508平移处的光电，0初始，1到位置
int light_ball_flag;//检测球的flag
} DribbleCfg_t;
/* 状态机上下文 */
typedef struct {
DribbleState_t DribbleState;
DribbleCfg_t DribbleConfig;
uint8_t is_init;
} DribbleCont_t;
typedef struct { typedef struct {
BMI088_t bmi088; BMI088_t bmi088;
@ -148,13 +91,6 @@ typedef struct
pid_param_t Pitch_Angle_M2006_speed_param; pid_param_t Pitch_Angle_M2006_speed_param;
pid_param_t Dribble_M3508_speed_param;//三摩擦用的速度环
pid_param_t Dribble_translate_M3508_speed_param;//平移用的速度环
pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环
DribbleCfg_t DribbleCfg;
PitchCfg_t PitchCfg; PitchCfg_t PitchCfg;
GO_MotorCmd_t go_cmd; GO_MotorCmd_t go_cmd;
@ -168,8 +104,6 @@ typedef struct{
fp32 Pitch_M2006_angle; fp32 Pitch_M2006_angle;
fp32 go_shoot; fp32 go_shoot;
fp32 Dribble_M3508_speed[3];//运球转速
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
fp32 Shoot_Pitch_angle; fp32 Shoot_Pitch_angle;
@ -189,11 +123,6 @@ typedef struct{
pid_type_def Pitch_Angle_M2006_speed; pid_type_def Pitch_Angle_M2006_speed;
pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环
pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
}up_pid_t; }up_pid_t;
typedef struct typedef struct
@ -213,9 +142,6 @@ typedef struct{
uint8_t up_task_run; uint8_t up_task_run;
const UP_Param_t *param; const UP_Param_t *param;
CoCon_t CoContext;
/*运球过程*/
DribbleCont_t DribbleCont;
/*投篮过程*/ /*投篮过程*/
Pitch_Cfg_t Pitch_Cfg; Pitch_Cfg_t Pitch_Cfg;
@ -272,9 +198,7 @@ int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle); int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle);
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed); int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out); int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
int8_t Co_Process(UP_t *u, CAN_Output_t *out);
#endif #endif

75
User/device/can_use.c
View File

@ -2,6 +2,9 @@
/* /*
CAN总线上的设备1到7 CAN总线上的设备1到7
将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制 将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
底盘4个3508，3个dt35接收板，终端电阻勿开，会出现数据收发异常情况
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
@ -11,13 +14,9 @@
#include "error_detect.h" #include "error_detect.h"
#define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */ #define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */
#define CAN_MOTOR_CUR_RES (16384) #define CAN_MOTOR_CUR_RES (16384)
#define CAN_PITCH6020_CTRL_ID (0x2ff) #define CAN_PITCH6020_CTRL_ID (0x2ff)
#define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200) #define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200)
/*CAN2上预留>4电机*/
/*CAN2上有6个大疆电机*/
#define CAN_G3508_CTRL_ALL_UP_SHOOT_ID (0x1ff) #define CAN_G3508_CTRL_ALL_UP_SHOOT_ID (0x1ff)
#define CAN_G3508_CTRL_ALL_UP_Dribble_ID (0x200) #define CAN_G3508_CTRL_ALL_UP_Dribble_ID (0x200)
@ -57,20 +56,33 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
feedback->torque_current = feedback->torque_current =
raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES; raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
} }
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,int id,
const uint8_t *raw) { const uint8_t *raw) {
if (feedback == NULL || raw == NULL) return; if (feedback == NULL || raw == NULL) return;
static uint16_t sick_rec[4]; static uint16_t sick_rec[4];
switch (id)
{
case CAN_SICK_ID_1:
feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);
sick_rec[0] = (uint16_t)(raw[0] << 8 | raw[1]); break ;
sick_rec[1] = (uint16_t)(raw[2] << 8 | raw[3]);
sick_rec[2] = (uint16_t)(raw[4] << 8 | raw[5]); case CAN_SICK_ID_2:
sick_rec[3] = (uint16_t)(raw[6] << 8 | raw[7]); feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
case CAN_SICK_ID_3:
feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
case CAN_SICK_ID_4:
feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
}
feedback ->raw_dis [0]= sick_rec[0];
feedback ->raw_dis [1]= sick_rec[1];
feedback ->raw_dis [2]= sick_rec[2];
feedback ->raw_dis [3]= sick_rec[3];
} }
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) { const uint8_t *raw) {
@ -284,25 +296,6 @@ void CAN_Encoder_Control(CAN_t *can)
return DEVICE_OK; return DEVICE_OK;
} }
//用来问答接收来自sick的数据
void CAN_Sick_Control(CAN_t *can)
{
raw_tx.tx_header.StdId = 0x301;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.DLC = 0x08;
raw_tx.tx_header.RTR = CAN_RTR_DATA;//˽ߝ֡
raw_tx.tx_data[0] = 0x00;
raw_tx.tx_data[1] = 0x00;
raw_tx.tx_data[2] = 0x00;
raw_tx.tx_data[3] = 0x00;
raw_tx.tx_data[4] = 0x00;
raw_tx.tx_data[5] = 0x00;
raw_tx.tx_data[6] = 0x00;
raw_tx.tx_data[7] = 0x00;
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->sick),&raw_tx.tx_header,raw_tx.tx_data,&(can->mailbox.chassis));
}
int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){ int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){
if (output == NULL) return DEVICE_ERR_NULL; if (output == NULL) return DEVICE_ERR_NULL;
int Byte[4]; int Byte[4];
@ -374,7 +367,19 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data); CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index); detect_hook(CHASSIS3508M1_TOE + index);
break; break;
case CAN_SICK_ID_1:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data);
break;
case CAN_SICK_ID_2:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data);
break;
case CAN_SICK_ID_3:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data);
break;
case CAN_SICK_ID_4:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data);
break;
default: default:
break; break;
@ -405,9 +410,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data); CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data);
break; break;
case CAN_SICK_ID:
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
break;
case CAN_Encoder_ID: case CAN_Encoder_ID:
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data); CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
can->recive_flag |= 1 << 10; can->recive_flag |= 1 << 10;

9
User/device/can_use.h
View File

@ -18,7 +18,10 @@ typedef enum {
CAN_UP_M3508_M5_ID = 0x205, /* 1 */ CAN_UP_M3508_M5_ID = 0x205, /* 1 */
CAN_UP_M3508_M6_ID = 0x206, /* 2 */ CAN_UP_M3508_M6_ID = 0x206, /* 2 */
CAN_SICK_ID=0x305, CAN_SICK_ID_1=0x301,
CAN_SICK_ID_2=0x302,
CAN_SICK_ID_3=0x303,
CAN_SICK_ID_4=0x304,
CAN_Encoder_ID=0x01, CAN_Encoder_ID=0x01,
@ -192,12 +195,10 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group,CAN_Output_t *output,CAN_t *c
int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can); int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can);
void CAN_Sick_Control(CAN_t *can);
void CAN_Encoder_Control(CAN_t *can); void CAN_Encoder_Control(CAN_t *can);
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) ; const uint8_t *raw) ;
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,int id,
const uint8_t *raw) ; const uint8_t *raw) ;
#endif #endif

6
User/device/cmd.c
View File

@ -146,7 +146,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_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust; //左上，右上，手动调整 if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust_Sick; //左上，右上，手动调整
if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust; if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust;
} }
@ -204,10 +204,10 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
} }
else if(cmd ->CMD_CtrlType ==RCcontrol){ else if(cmd ->CMD_CtrlType ==RCcontrol){
/*手动下的*/ /*手动下的*/
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ; if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Normal ;
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch; else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust; else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust; else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust_Sick;
else cmd ->CMD_mode =Normal; else cmd ->CMD_mode =Normal;
} }
} }

6
User/device/cmd.h
View File

@ -28,9 +28,11 @@ typedef enum{
AUTO_MID360_Adjust,//雷达调整 AUTO_MID360_Adjust,//雷达调整
UP_Adjust,//上层调整 UP_Adjust,//上层调整
Chassis_Adjust, Chassis_Adjust_Sick,
PassBall,
Dribble , //运球
Pitch, //投篮,底盘锁定 Pitch, //投篮,底盘锁定

27
User/device/nuc.c
View File

@ -7,7 +7,7 @@ static volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
uint8_t nucbuf[31]; uint8_t nucbuf[31];
static char SendBuffer[19]; char SendBuffer[7];
static void NUC_IdleCallback(void) { static void NUC_IdleCallback(void) {
@ -33,22 +33,22 @@ int8_t NUC_StartSending(fp32 *data) {
union union
{ {
float x[4]; float x[1];
char data[16]; char data[4];
}instance; }instance;
for (int i = 0; i < 4; i++) { // for (int i = 0; i < 1; i++) {
instance.x[i] = data[i]; instance.x[0] = data[0];
} // }
SendBuffer[0] = 0xFC; //发送ID SendBuffer[0] = 0xFC; //帧头
SendBuffer[1] = 0x01; //控制帧 SendBuffer[1] = 0x01; //控制帧
for(int i = 2; i < 18; i++) for(int i = 2; i < 6; i++)
{ {
SendBuffer[i] = instance.data[i-2]; SendBuffer[i] = instance.data[i-2];
} }
SendBuffer[18] = 0xFD; //结束符 SendBuffer[6] = 0xFD; //帧尾
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC), if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)SendBuffer, (uint8_t *)SendBuffer,
@ -146,6 +146,15 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
n->MID360.angle = instance.x[4];// n->MID360.angle = instance.x[4];//
n->MID360.flag = nucbuf[23];// n->MID360.flag = nucbuf[23];//
break; break;

6
User/device/rc.c
View File

@ -1,5 +1,9 @@
/* /*
DR16接收机 遥控
dr16：dbus 乐迪：s-ubs
dr16掉线不输出信号，可以使用超时检测方式判断掉线
即RC_SBUS_WaitDmaCplt
乐迪掉线依然有信号输出，只是部分通道变为特定值，无法使用以上方式
*/ */

3
User/task/nuc_task.c
View File

@ -14,7 +14,7 @@ NUC_send_t NUC_send;
#endif #endif
fp32 send[4]={11.0f,45.0,1.f,4.0f}; fp32 send[4]={11.0f,45.0,1.f,4.0f};
int a=0;
void Task_nuc(void *argument){ void Task_nuc(void *argument){
(void)argument; /**/ (void)argument; /**/
@ -33,6 +33,7 @@ void Task_nuc(void *argument){
task_runtime.last_up_time.nuc= tick; task_runtime.last_up_time.nuc= tick;
#endif #endif
a++;
NUC_StartReceiving(); NUC_StartReceiving();
NUC_StartSending(NUC_send.send); NUC_StartSending(NUC_send.send);
// NUC_StartSending (send); // NUC_StartSending (send);

3
User/task/rc_task.c
View File

@ -33,7 +33,7 @@ static LD_Data_t LD;
* \brief dr16接收机 * \brief dr16接收机
* *
* \param argument 未使用 * \param argument 未使用
*/int aaaaaaaaaaaaaaaaaaa=0; */
void Task_rc(void *argument) { void Task_rc(void *argument) {
(void)argument; /* 未使用，消除警告 */ (void)argument; /* 未使用，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC;
@ -48,7 +48,6 @@ void Task_rc(void *argument) {
#endif #endif
/* 开启DMA */ /* 开启DMA */
RC_StartDmaRecv(); RC_StartDmaRecv();
aaaaaaaaaaaaaaaaaaa++;
if (RC_WaitDmaCplt(30)) { if (RC_WaitDmaCplt(30)) {
RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc); RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);