sick
This commit is contained in:
parent
8570b03ca2
commit
4859c69fad
4
MDK-ARM/.vscode/keil-assistant.log
vendored
4
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -471,3 +471,7 @@
|
||||
|
||||
[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
|
||||
|
||||
|
||||
@ -103,7 +103,7 @@
|
||||
<bEvRecOn>1</bEvRecOn>
|
||||
<bSchkAxf>0</bSchkAxf>
|
||||
<bTchkAxf>0</bTchkAxf>
|
||||
<nTsel>3</nTsel>
|
||||
<nTsel>6</nTsel>
|
||||
<sDll></sDll>
|
||||
<sDllPa></sDllPa>
|
||||
<sDlgDll></sDlgDll>
|
||||
@ -114,13 +114,13 @@
|
||||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>BIN\CMSIS_AGDI.dll</pMon>
|
||||
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
|
||||
</DebugOpt>
|
||||
<TargetDriverDllRegistry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<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>
|
||||
<Number>0</Number>
|
||||
@ -140,12 +140,12 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name></Name>
|
||||
<Name>(105=-1,-1,-1,-1,0)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<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>
|
||||
<Number>0</Number>
|
||||
@ -158,73 +158,38 @@
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>can,0x0A</ItemText>
|
||||
<ItemText>NUC_send,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>1</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>chassis,0x0A</ItemText>
|
||||
<ItemText>SendBuffer,0x10</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>UP,0x0A</ItemText>
|
||||
<ItemText>f,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>NUC_send,0x0A</ItemText>
|
||||
<ItemText>a,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>pid,0x0A</ItemText>
|
||||
<ItemText>chassis</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>pid_param,0x0A</ItemText>
|
||||
<ItemText>cmd_fromnuc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<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>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>13</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>LD_raw,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -273,7 +238,7 @@
|
||||
<EnableFlashSeq>0</EnableFlashSeq>
|
||||
<EnableLog>0</EnableLog>
|
||||
<Protocol>2</Protocol>
|
||||
<DbgClock>5000000</DbgClock>
|
||||
<DbgClock>1000000</DbgClock>
|
||||
</DebugDescription>
|
||||
</TargetOption>
|
||||
</Target>
|
||||
|
||||
Binary file not shown.
@ -1,6 +1,4 @@
|
||||
#ifndef ERROR_DETECT_H
|
||||
|
||||
|
||||
#define ERROR_DETECT_H
|
||||
|
||||
#include "struct_typedef.h"
|
||||
|
||||
@ -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.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam));
|
||||
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[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;
|
||||
}
|
||||
|
||||
fp32 gyro_kp = 1.0f;
|
||||
fp32 PIAN = 0;
|
||||
fp32 pian_yaw;
|
||||
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
|
||||
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;
|
||||
|
||||
break;
|
||||
case Chassis_Adjust:
|
||||
case Chassis_Adjust_Sick:
|
||||
|
||||
sick_calibration(c, ctrl, out);
|
||||
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.Vy, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||||
c->NUC_send.send[0] = 1;
|
||||
// c->NUC_send.send[0] = 1;
|
||||
break;
|
||||
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;
|
||||
|
||||
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:
|
||||
|
||||
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_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;
|
||||
@ -219,37 +225,20 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
|
||||
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)
|
||||
{
|
||||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||||
// int is_in;//是否初始化
|
||||
// if(is_in==0){
|
||||
PID_init(&pid,PID_POSITION,&pid_param);
|
||||
// is_in=1;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
static fp32 pian_angel,cur_angle;
|
||||
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 {
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -82,8 +82,8 @@ typedef struct
|
||||
|
||||
pid_param_t Sick_CaliWparam;
|
||||
pid_param_t Sick_CaliYparam;
|
||||
pid_param_t Sick_CaliXparam;
|
||||
|
||||
pid_param_t Sick_CaliXparam;
|
||||
pid_param_t Chassis_AngleAdjust_param;
|
||||
|
||||
sickparam_t sickparam;
|
||||
|
||||
@ -100,6 +100,7 @@ typedef struct
|
||||
}ChassisMove_Vec;
|
||||
typedef struct
|
||||
{
|
||||
|
||||
fp32 send[4];
|
||||
|
||||
}NUC_send_t;
|
||||
@ -154,7 +155,7 @@ typedef struct{
|
||||
/*存在较低误差*/
|
||||
pid_type_def chassis_PICKWzPID_LOW;
|
||||
|
||||
|
||||
pid_type_def Chassis_AngleAdjust;
|
||||
|
||||
pid_type_def SickCaliWzPID;
|
||||
pid_type_def SickCaliVxPID;
|
||||
|
||||
@ -58,28 +58,6 @@ static const ConfigParam_t param ={
|
||||
.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={
|
||||
.id =0,
|
||||
@ -93,21 +71,8 @@ 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 = {
|
||||
.m2006_init =-150, //释放的角度
|
||||
@ -167,11 +132,19 @@ static const ConfigParam_t param ={
|
||||
.i_limit = 500.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={
|
||||
.w_error=5,
|
||||
.xy_error=5,
|
||||
.x_set=927,
|
||||
.y_set=1255,
|
||||
.x_set=600,
|
||||
.y_set=100,
|
||||
|
||||
},
|
||||
|
||||
|
||||
256
User/Module/up.c
256
User/Module/up.c
@ -5,11 +5,6 @@
|
||||
#include "bsp_delay.h"
|
||||
/*接线
|
||||
|
||||
上层用到三个光电
|
||||
1.PE9 发射用 输出高电平 LIGHTA
|
||||
2.PE11 运球上3508 输出高电平 LIGHTB
|
||||
3.PE13 三摩擦检测球 输出高电平LIGHTC
|
||||
|
||||
|
||||
*/
|
||||
|
||||
@ -41,24 +36,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_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 ;
|
||||
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) {
|
||||
u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值
|
||||
@ -95,10 +76,6 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *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;
|
||||
}
|
||||
@ -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)
|
||||
{
|
||||
//电机控制 ，传进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] ,
|
||||
&u->pid .Shoot_M2006angle ,
|
||||
&u->pid .Shoot_M2006speed ,
|
||||
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,
|
||||
(PID_calc (&(u->pid .Pitch_M2006_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;
|
||||
DribbleCfg_t *DribbleCont = &u->DribbleCont.DribbleConfig;
|
||||
|
||||
up_motor_target_t *target=&u->motor_target ;
|
||||
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 ;
|
||||
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;
|
||||
|
||||
case Pitch :
|
||||
@ -309,17 +263,7 @@ 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->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
|
||||
break ;
|
||||
case Dribble:
|
||||
{
|
||||
|
||||
if(u->DribbleCont.DribbleState== DRIBBLE_PREPARE){
|
||||
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;
|
||||
}
|
||||
//光电状态更新
|
||||
Dribble_Process(u,out);
|
||||
}break ;
|
||||
|
||||
|
||||
|
||||
|
||||
default:
|
||||
break;
|
||||
@ -334,13 +278,9 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
{
|
||||
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
}
|
||||
/*根据距离实时计算所需pos*/
|
||||
|
||||
/*根据距离实时计算所需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);
|
||||
break ;
|
||||
@ -349,9 +289,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;
|
||||
|
||||
u->Pitch_Cfg .PitchState = PITCH_PREPARE;
|
||||
u->DribbleCont .DribbleState = DRIBBLE_PREPARE;
|
||||
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
}
|
||||
@ -371,8 +309,6 @@ return 0;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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 ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -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 {
|
||||
@ -72,16 +51,7 @@ typedef struct {
|
||||
fp32 go_release_pos;//go松开，发射位置
|
||||
|
||||
} 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 {
|
||||
PitchState_t PitchState;
|
||||
PitchCfg_t PitchConfig;
|
||||
@ -99,33 +69,6 @@ typedef enum {
|
||||
DRIBBLE_DONE // 运球结束
|
||||
} 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 {
|
||||
|
||||
BMI088_t bmi088;
|
||||
@ -147,14 +90,7 @@ typedef struct
|
||||
pid_param_t Pitch_M2006_angle_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;
|
||||
|
||||
GO_MotorCmd_t go_cmd;
|
||||
@ -168,8 +104,6 @@ typedef struct{
|
||||
fp32 Pitch_M2006_angle;
|
||||
|
||||
fp32 go_shoot;
|
||||
fp32 Dribble_M3508_speed[3];//运球转速
|
||||
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
|
||||
|
||||
fp32 Shoot_Pitch_angle;
|
||||
|
||||
@ -189,11 +123,6 @@ typedef struct{
|
||||
|
||||
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;
|
||||
typedef struct
|
||||
@ -213,9 +142,6 @@ typedef struct{
|
||||
|
||||
uint8_t up_task_run;
|
||||
const UP_Param_t *param;
|
||||
CoCon_t CoContext;
|
||||
/*运球过程*/
|
||||
DribbleCont_t DribbleCont;
|
||||
/*投篮过程*/
|
||||
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_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 Co_Process(UP_t *u, CAN_Output_t *out);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -2,6 +2,9 @@
|
||||
/*
|
||||
CAN总线上的设备1到7
|
||||
将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
|
||||
|
||||
|
||||
底盘4个3508，3个dt35接收板，终端电阻勿开，会出现数据收发异常情况
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
@ -11,13 +14,9 @@
|
||||
#include "error_detect.h"
|
||||
#define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */
|
||||
#define CAN_MOTOR_CUR_RES (16384)
|
||||
|
||||
|
||||
#define CAN_PITCH6020_CTRL_ID (0x2ff)
|
||||
|
||||
#define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200)
|
||||
|
||||
/*CAN2上有6个大疆电机*/
|
||||
/*CAN2上预留>4电机*/
|
||||
#define CAN_G3508_CTRL_ALL_UP_SHOOT_ID (0x1ff)
|
||||
#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 =
|
||||
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) {
|
||||
if (feedback == NULL || raw == NULL) return;
|
||||
static uint16_t sick_rec[4];
|
||||
switch (id)
|
||||
{
|
||||
case CAN_SICK_ID_1:
|
||||
feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);
|
||||
|
||||
break ;
|
||||
|
||||
case CAN_SICK_ID_2:
|
||||
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 ;
|
||||
}
|
||||
|
||||
sick_rec[0] = (uint16_t)(raw[0] << 8 | raw[1]);
|
||||
sick_rec[1] = (uint16_t)(raw[2] << 8 | raw[3]);
|
||||
sick_rec[2] = (uint16_t)(raw[4] << 8 | raw[5]);
|
||||
sick_rec[3] = (uint16_t)(raw[6] << 8 | raw[7]);
|
||||
|
||||
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,
|
||||
const uint8_t *raw) {
|
||||
@ -282,26 +294,7 @@ void CAN_Encoder_Control(CAN_t *can)
|
||||
break;
|
||||
}
|
||||
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){
|
||||
if (output == NULL) return DEVICE_ERR_NULL;
|
||||
@ -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);
|
||||
detect_hook(CHASSIS3508M1_TOE + index);
|
||||
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:
|
||||
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);
|
||||
|
||||
break;
|
||||
case CAN_SICK_ID:
|
||||
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
|
||||
break;
|
||||
|
||||
case CAN_Encoder_ID:
|
||||
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
|
||||
can->recive_flag |= 1 << 10;
|
||||
|
||||
@ -18,7 +18,10 @@ typedef enum {
|
||||
CAN_UP_M3508_M5_ID = 0x205, /* 1 */
|
||||
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,
|
||||
@ -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);
|
||||
|
||||
void CAN_Sick_Control(CAN_t *can);
|
||||
|
||||
void CAN_Encoder_Control(CAN_t *can);
|
||||
|
||||
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
|
||||
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) ;
|
||||
#endif
|
||||
|
||||
@ -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_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;
|
||||
}
|
||||
@ -204,10 +204,10 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
|
||||
}
|
||||
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_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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -28,9 +28,11 @@ typedef enum{
|
||||
AUTO_MID360_Adjust,//雷达调整
|
||||
|
||||
UP_Adjust,//上层调整
|
||||
Chassis_Adjust,
|
||||
Chassis_Adjust_Sick,
|
||||
|
||||
PassBall,
|
||||
|
||||
Dribble , //运球
|
||||
|
||||
Pitch, //投篮,底盘锁定
|
||||
|
||||
|
||||
|
||||
@ -7,7 +7,7 @@ static volatile uint32_t drop_message = 0;
|
||||
static osThreadId_t thread_alert;
|
||||
|
||||
uint8_t nucbuf[31];
|
||||
static char SendBuffer[19];
|
||||
char SendBuffer[7];
|
||||
|
||||
|
||||
static void NUC_IdleCallback(void) {
|
||||
@ -33,22 +33,22 @@ int8_t NUC_StartSending(fp32 *data) {
|
||||
|
||||
union
|
||||
{
|
||||
float x[4];
|
||||
char data[16];
|
||||
float x[1];
|
||||
char data[4];
|
||||
}instance;
|
||||
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
instance.x[i] = data[i];
|
||||
}
|
||||
// for (int i = 0; i < 1; i++) {
|
||||
instance.x[0] = data[0];
|
||||
// }
|
||||
|
||||
SendBuffer[0] = 0xFC; //发送ID
|
||||
SendBuffer[0] = 0xFC; //帧头
|
||||
SendBuffer[1] = 0x01; //控制帧
|
||||
for(int i = 2; i < 18; i++)
|
||||
for(int i = 2; i < 6; i++)
|
||||
{
|
||||
SendBuffer[i] = instance.data[i-2];
|
||||
}
|
||||
SendBuffer[18] = 0xFD; //结束符
|
||||
SendBuffer[6] = 0xFD; //帧尾
|
||||
|
||||
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
|
||||
(uint8_t *)SendBuffer,
|
||||
@ -146,6 +146,15 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
|
||||
n->MID360.angle = instance.x[4];//
|
||||
|
||||
n->MID360.flag = nucbuf[23];//
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
break;
|
||||
|
||||
|
||||
|
||||
@ -1,5 +1,9 @@
|
||||
/*
|
||||
DR16接收机
|
||||
遥控
|
||||
dr16：dbus 乐迪：s-ubs
|
||||
dr16掉线不输出信号，可以使用超时检测方式判断掉线
|
||||
即RC_SBUS_WaitDmaCplt
|
||||
乐迪掉线依然有信号输出，只是部分通道变为特定值，无法使用以上方式
|
||||
|
||||
*/
|
||||
|
||||
|
||||
@ -14,7 +14,7 @@ NUC_send_t NUC_send;
|
||||
|
||||
#endif
|
||||
fp32 send[4]={11.0f,45.0,1.f,4.0f};
|
||||
|
||||
int a=0;
|
||||
void Task_nuc(void *argument){
|
||||
(void)argument; /**/
|
||||
|
||||
@ -33,6 +33,7 @@ void Task_nuc(void *argument){
|
||||
task_runtime.last_up_time.nuc= tick;
|
||||
|
||||
#endif
|
||||
a++;
|
||||
NUC_StartReceiving();
|
||||
NUC_StartSending(NUC_send.send);
|
||||
// NUC_StartSending (send);
|
||||
|
||||
@ -33,7 +33,7 @@ static LD_Data_t LD;
|
||||
* \brief dr16接收机
|
||||
*
|
||||
* \param argument 未使用
|
||||
*/int aaaaaaaaaaaaaaaaaaa=0;
|
||||
*/
|
||||
void Task_rc(void *argument) {
|
||||
(void)argument; /* 未使用，消除警告 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC;
|
||||
@ -48,7 +48,6 @@ void Task_rc(void *argument) {
|
||||
#endif
|
||||
/* 开启DMA */
|
||||
RC_StartDmaRecv();
|
||||
aaaaaaaaaaaaaaaaaaa++;
|
||||
if (RC_WaitDmaCplt(30)) {
|
||||
|
||||
RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user