Compare commits
3 Commits
Cooperatio
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
| 8ef2a0a348 | |||
| 7499e75a50 | |||
| 79f6f5f9c0 |
@ -80,6 +80,7 @@ int main(void)
|
||||
/* MCU Configuration--------------------------------------------------------*/
|
||||
|
||||
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
|
||||
|
||||
HAL_Init();
|
||||
|
||||
/* USER CODE BEGIN Init */
|
||||
|
||||
@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void)
|
||||
|
||||
/* USER CODE END USART1_Init 1 */
|
||||
huart1.Instance = USART1;
|
||||
huart1.Init.BaudRate = 115200;
|
||||
huart1.Init.BaudRate = 4000000;
|
||||
huart1.Init.WordLength = UART_WORDLENGTH_8B;
|
||||
huart1.Init.StopBits = UART_STOPBITS_1;
|
||||
huart1.Init.Parity = UART_PARITY_NONE;
|
||||
@ -104,7 +104,7 @@ void MX_USART6_UART_Init(void)
|
||||
|
||||
/* USER CODE END USART6_Init 1 */
|
||||
huart6.Instance = USART6;
|
||||
huart6.Init.BaudRate = 4000000;
|
||||
huart6.Init.BaudRate = 115200;
|
||||
huart6.Init.WordLength = UART_WORDLENGTH_8B;
|
||||
huart6.Init.StopBits = UART_STOPBITS_1;
|
||||
huart6.Init.Parity = UART_PARITY_NONE;
|
||||
|
||||
@ -223,27 +223,92 @@
|
||||
<Ww>
|
||||
<count>13</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>a</ItemText>
|
||||
<ItemText>b</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>14</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>b</ItemText>
|
||||
<ItemText>count,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>15</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>count,0x0A</ItemText>
|
||||
<ItemText>count</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>16</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>count</ItemText>
|
||||
<ItemText>raw_rx1,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>17</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>raw_rx1,0x0A</ItemText>
|
||||
<ItemText>gyro_kp,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>18</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nucbuf,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>19</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>vofa_send,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>20</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>SendBuffer,0x10</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>21</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>NUC_StartSending,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>22</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>PIAN</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>23</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>pid</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>24</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>bmi088</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>25</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ist8310</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>26</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>imu_eulr,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>27</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>imu_temp,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>28</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>htim10,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>29</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>pulse</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>30</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>imu_temp</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
|
||||
Binary file not shown.
@ -81,13 +81,17 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
|
||||
fp32 gyro_kp=1.0f;
|
||||
fp32 PIAN=0;
|
||||
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算,
|
||||
{
|
||||
fp64 Nor_Vx,Nor_Vy;//归一化后的数据
|
||||
normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
|
||||
|
||||
|
||||
// c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw;//右前
|
||||
// c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
|
||||
// c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
|
||||
// c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
|
||||
// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
|
||||
// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
|
||||
// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
|
||||
@ -96,6 +100,14 @@ void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆
|
||||
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw ;//右后
|
||||
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw ;//左后
|
||||
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw ;//左前
|
||||
|
||||
// if(!Vw){
|
||||
// PIAN = jiuzheng(c->pos088 .imu_eulr .yaw );
|
||||
// }
|
||||
// c->hopemotorout.OmniSpeedOut[0] += PIAN ;//右前
|
||||
// c->hopemotorout.OmniSpeedOut[1] += PIAN ;//右后
|
||||
// c->hopemotorout.OmniSpeedOut[2] += PIAN ;//左后
|
||||
// c->hopemotorout.OmniSpeedOut[3] += PIAN ;//左前
|
||||
|
||||
}
|
||||
|
||||
@ -139,87 +151,57 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
|
||||
|
||||
break;
|
||||
|
||||
case AUTO : //在自动模式下
|
||||
|
||||
switch(c->chassis_ctrl.mode ){
|
||||
|
||||
case AUTO: // 自动模式
|
||||
switch (c->chassis_ctrl.mode) {
|
||||
|
||||
case AUTO_NAVI: //自动雷达
|
||||
// //这套是全向轮的方向,一定要注意这里的xy方向
|
||||
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 =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
|
||||
// c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
|
||||
// c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
|
||||
//
|
||||
// c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw);
|
||||
// c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx);
|
||||
// c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy);
|
||||
// //电机速度限幅
|
||||
case AUTO_MID360: // 自动雷达
|
||||
// 全向轮方向, 注意xy方向
|
||||
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,2000.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.Vw, 5000.0f);
|
||||
|
||||
abs_limit_fp(&c->move_vec.Vy,2000.0f);
|
||||
c->NUC_send .send [0]=0;
|
||||
break;
|
||||
|
||||
abs_limit_fp(&c->move_vec.Vw,2000.0f);
|
||||
case AUTO_MID360_Pitch:
|
||||
c->move_vec.Vw = 0;
|
||||
c->move_vec.Vy = 0;
|
||||
c->move_vec.Vx = 0;
|
||||
|
||||
c->NUC_send .send [0]=0;
|
||||
break;
|
||||
case AUTO_MID360_Adjust:
|
||||
c->move_vec.Vw = ctrl->Vw * 6000;
|
||||
c->move_vec.Vx = ctrl->Vy * 6000;
|
||||
c->move_vec.Vy = ctrl->Vx * 6000;
|
||||
|
||||
c->NUC_send .send [0]=1;
|
||||
break;
|
||||
default:
|
||||
c->move_vec.Vw = 0;
|
||||
c->move_vec.Vx = 0;
|
||||
c->move_vec.Vy = 0;
|
||||
c->NUC_send .send [0]=0;
|
||||
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
if(ctrl->status[5] ==1)
|
||||
{
|
||||
c->move_vec.Vw = c->move_vec.Vw * 0.8f;
|
||||
c->move_vec.Vx = c->move_vec.Vx * 0.5f;
|
||||
c->move_vec.Vy = c->move_vec.Vy * 0.5f;
|
||||
}
|
||||
default:
|
||||
c->move_vec.Vw = 0;
|
||||
c->move_vec.Vx = 0;
|
||||
c->move_vec.Vy = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case AUTO_PICK: //自动视觉
|
||||
|
||||
c->move_vec.Vx =ctrl->Vx*6000 ;
|
||||
c->move_vec.Vy =ctrl->Vy *6000;
|
||||
c->move_vec .Vw = -ctrl->cmd_pick .posx;
|
||||
|
||||
|
||||
|
||||
if(abs_value(ctrl ->cmd_pick .posx )>20)
|
||||
{
|
||||
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0);
|
||||
|
||||
}
|
||||
else if(abs_value(ctrl ->cmd_pick .posx )<0.1)
|
||||
{
|
||||
c->move_vec.Vw =0;
|
||||
}
|
||||
else
|
||||
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_LOW),(c->move_vec .Vw) ,0);
|
||||
|
||||
c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
|
||||
|
||||
|
||||
c->vofa_send[0]=c->move_vec.Vw;
|
||||
c->vofa_send[1]=-ctrl->cmd_pick .posx;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
|
||||
|
||||
}
|
||||
break ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
//电机速度限幅
|
||||
|
||||
abs_limit_fp(&c->move_vec.Vx,6000.0f);
|
||||
|
||||
abs_limit_fp(&c->move_vec.Vy,6000.0f);
|
||||
|
||||
abs_limit_fp(&c->move_vec.Vw,6000.0f);
|
||||
// 电机速度限幅
|
||||
abs_limit_fp(&c->move_vec.Vx, 6000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vy, 6000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vw, 6000.0f);
|
||||
|
||||
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);
|
||||
|
||||
@ -232,17 +214,54 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
|
||||
}
|
||||
|
||||
|
||||
// c->vofa_send[0]=c->pos088.bmi088.gyro.x;
|
||||
// c->vofa_send[1]=c->pos088.bmi088.gyro.y;
|
||||
// c->vofa_send[2]=c->pos088.bmi088.gyro.z;
|
||||
// c->vofa_send[3]=c->pos088.bmi088.accl.x;
|
||||
// c->vofa_send[4]=c->pos088.bmi088.accl.y;
|
||||
// c->vofa_send[5]=c->pos088.bmi088.accl.z;
|
||||
|
||||
// c->vofa_send[0]=c->pos088.bmi088.gyro.z;
|
||||
|
||||
// c->vofa_send[1]=c->motorfeedback .rotor_rpm3508 [0];
|
||||
// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [1];
|
||||
// c->vofa_send[3]=c->motorfeedback .rotor_rpm3508 [2];
|
||||
// c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [3];
|
||||
//
|
||||
// c->vofa_send[0]=c->hopemotorout .OmniSpeedOut [0];
|
||||
// c->vofa_send[1]=c->hopemotorout .OmniSpeedOut [0];
|
||||
//// c->vofa_send[2]=c->hopemotorout .OmniSpeedOut [2];
|
||||
// c->vofa_send[3]=c->hopemotorout .OmniSpeedOut [3];
|
||||
|
||||
//// c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [0];
|
||||
// c->vofa_send[0]=c->motorfeedback .rotor_rpm3508 [0];
|
||||
//// c->vofa_send[6]=c->motorfeedback .rotor_rpm3508 [2];
|
||||
// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [3];
|
||||
//
|
||||
// c->vofa_send[5]=c->pos088 .imu_eulr .yaw ;
|
||||
|
||||
return CHASSIS_OK;
|
||||
|
||||
}
|
||||
pid_type_def pid;
|
||||
|
||||
pid_param_t pid_param={
|
||||
.p = 0.50f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 50.0f,
|
||||
.out_limit = 100.0f
|
||||
};
|
||||
fp32 jiuzheng(fp32 yaw)
|
||||
{
|
||||
static fp32 pian_yaw=0;
|
||||
static fp32 shang_yaw=0;
|
||||
static int is=0;
|
||||
|
||||
|
||||
|
||||
if(is==0)
|
||||
{
|
||||
PID_init (&pid,PID_POSITION ,&pid_param);
|
||||
is=1;
|
||||
}
|
||||
|
||||
pian_yaw+= (yaw - shang_yaw);
|
||||
shang_yaw=yaw ;
|
||||
|
||||
return PID_calc(&pid,pian_yaw,0);
|
||||
}
|
||||
|
||||
|
||||
@ -190,6 +190,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
|
||||
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
|
||||
|
||||
|
||||
fp32 jiuzheng(fp32 yaw);
|
||||
|
||||
|
||||
|
||||
|
||||
@ -97,7 +97,7 @@ static const ConfigParam_t param ={
|
||||
/*运球*/
|
||||
.DribbleConfig_Config = {
|
||||
.dribble_flag=0,
|
||||
.m3508_init_angle = 50,
|
||||
.m3508_init_angle = -5,
|
||||
.m3508_translate_angle = -930,
|
||||
.m3508_dribble_Reverse_speed=-3500,
|
||||
. m3508_dribble_speed= 4000, // 转动速度
|
||||
@ -110,9 +110,10 @@ static const ConfigParam_t param ={
|
||||
},
|
||||
/*投球*/
|
||||
.PitchConfig_Config = {
|
||||
.m2006_init_angle =-170,
|
||||
.m2006_init_angle =-90,
|
||||
.m2006_trigger_angle =0,
|
||||
.go1_init_position = -50,
|
||||
.go1_init_position = -50,
|
||||
.go1_Receive_ball = -5, //偏下
|
||||
.go1_release_threshold =-210,
|
||||
.m2006_Screw_init=0,
|
||||
.Pitch_angle =56,
|
||||
|
||||
189
User/Module/up.c
189
User/Module/up.c
@ -49,6 +49,8 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
|
||||
}
|
||||
|
||||
u->go_cmd =u->param ->go_cmd ;
|
||||
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
|
||||
|
||||
|
||||
// 初始化上层状态机
|
||||
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
|
||||
@ -93,7 +95,11 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
|
||||
}
|
||||
|
||||
u->cmd =c;
|
||||
|
||||
/*光电状态更新*/
|
||||
u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
|
||||
u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -254,7 +260,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
static int is_pitch=1;
|
||||
|
||||
|
||||
|
||||
|
||||
switch (c->CMD_CtrlType )
|
||||
{
|
||||
case RCcontrol: //在手动模式下
|
||||
@ -270,7 +276,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
is_pitch=0;
|
||||
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
|
||||
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
|
||||
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
|
||||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
|
||||
|
||||
/*运球*/
|
||||
@ -279,8 +284,10 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
for(int i=0;i<3;i++){
|
||||
u->motor_target.Dribble_M3508_speed[i]=0;
|
||||
}
|
||||
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
|
||||
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_translate_angle;
|
||||
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
|
||||
|
||||
|
||||
|
||||
break;
|
||||
|
||||
@ -290,15 +297,20 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
}
|
||||
|
||||
Pitch_Process(u,out,c);
|
||||
Pitch_Process(u,out);
|
||||
// if (u->PitchContext .PitchState ==PITCH_PREPARE)
|
||||
// {
|
||||
// u->CoContext .CoState =CoTRANSLATE_OUT;
|
||||
// }
|
||||
// Co_Process(u,out);
|
||||
|
||||
break ;
|
||||
case UP_Adjust: //测试用
|
||||
|
||||
u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
|
||||
u->PitchContext.PitchConfig.go1_Receive_ball += c->Vx ;
|
||||
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
|
||||
|
||||
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
|
||||
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball;
|
||||
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
|
||||
break ;
|
||||
case Dribble:
|
||||
@ -308,10 +320,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE;
|
||||
}
|
||||
//光电状态更新
|
||||
u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
|
||||
u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
|
||||
a=HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
|
||||
b=HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
|
||||
Dribble_Process(u,out);
|
||||
}break ;
|
||||
|
||||
@ -320,7 +328,32 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
default:
|
||||
break;
|
||||
}
|
||||
case AUTO:
|
||||
switch(c-> CMD_mode)
|
||||
{
|
||||
case AUTO_MID360_Pitch:
|
||||
u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos);
|
||||
|
||||
if (u->PitchContext .PitchState ==PITCH_PREPARE)
|
||||
{
|
||||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
}
|
||||
/*根据距离实时计算所需pos*/
|
||||
|
||||
// u->PitchContext .PitchConfig .go1_release_threshold=c->pos;
|
||||
Pitch_Process(u,out);
|
||||
break ;
|
||||
|
||||
case AUTO_MID360:
|
||||
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
|
||||
|
||||
u->PitchContext .PitchState = PITCH_PREPARE;
|
||||
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
@ -341,12 +374,14 @@ return 0;
|
||||
|
||||
|
||||
|
||||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
|
||||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
|
||||
{
|
||||
|
||||
switch(u->PitchContext .PitchState){
|
||||
|
||||
case PITCH_START:
|
||||
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
|
||||
|
||||
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
|
||||
|
||||
if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
|
||||
@ -361,11 +396,21 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
|
||||
|
||||
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
|
||||
{
|
||||
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
|
||||
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball;
|
||||
if(is_reached (u->motorfeedback .go_data ->Pos ,u->motor_target .go_shoot ,1.0f))
|
||||
{
|
||||
u->PitchContext .PitchState=PITCH_LAUNCHING;
|
||||
}
|
||||
}
|
||||
|
||||
break ;
|
||||
|
||||
|
||||
case PITCH_LAUNCHING: //等待发射
|
||||
u->PitchContext .PitchState=PITCH_COMPLETE;
|
||||
break ;
|
||||
case PITCH_COMPLETE: //发射完成
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
|
||||
}
|
||||
@ -385,11 +430,15 @@ 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;//是否共速
|
||||
|
||||
|
||||
switch (u->DribbleContext.DribbleState) {
|
||||
|
||||
case DRIBBLE_TRANSLATE:
|
||||
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_translate_angle,1.0f))
|
||||
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
|
||||
|
||||
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
|
||||
{
|
||||
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
|
||||
}
|
||||
@ -412,13 +461,13 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
|
||||
common_speed_flag =1;
|
||||
}
|
||||
if(common_speed_flag){
|
||||
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测，反转
|
||||
if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
|
||||
u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
|
||||
u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
|
||||
u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
|
||||
|
||||
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP;
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@ -426,18 +475,24 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
|
||||
|
||||
break;
|
||||
case DRIBBLE_PROCESS_UP:
|
||||
if((u->motorfeedback .DJmotor_feedback [0].rpm<0)&&
|
||||
(u->motorfeedback .DJmotor_feedback [1].rpm>0)&&
|
||||
(u->motorfeedback .DJmotor_feedback [2].rpm>0)
|
||||
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)
|
||||
){
|
||||
|
||||
if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){
|
||||
common_speed_reverse_flag=1;
|
||||
|
||||
}
|
||||
if(common_speed_reverse_flag){
|
||||
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){
|
||||
u->DribbleContext .DribbleState=DRIBBLE_DONE;
|
||||
RELAY1_TOGGLE(0); //关闭气缸
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break ;
|
||||
case DRIBBLE_DONE:
|
||||
common_speed_reverse_flag=0;
|
||||
for(int i=0;i<3;i++){
|
||||
u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
|
||||
}
|
||||
@ -454,5 +509,89 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
// CoPREPARE, // 准备阶段
|
||||
// CoTRANSLATE_OUT,//上方平移,去运球
|
||||
// CoDRIBBLE, //运球阶段
|
||||
// CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
|
||||
// CoPITCH, //发射
|
||||
// CoDONE,
|
||||
|
||||
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->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
|
||||
|
||||
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
|
||||
{
|
||||
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态
|
||||
u->PitchContext .PitchState = PITCH_START;
|
||||
|
||||
u->CoContext .CoState =CoDRIBBLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case CoDRIBBLE:
|
||||
Dribble_Process(u,out); //状态停在DRIBBLE_DONE
|
||||
Pitch_Process(u,out); //状态停在PITCH_PULL_TRIGGER
|
||||
//状态停在对应位置时,平移去给发射送球
|
||||
if(u->DribbleContext .DribbleState ==DRIBBLE_DONE && u->PitchContext.PitchState ==PITCH_LAUNCHING)
|
||||
{
|
||||
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
|
||||
}
|
||||
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_init_angle,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->DribbleContext .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->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
|
||||
|
||||
}
|
||||
|
||||
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主,卡到最右端
|
||||
{
|
||||
u->CoContext .CoState =CoPITCH;
|
||||
}
|
||||
break ;
|
||||
|
||||
case CoPITCH:
|
||||
|
||||
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -26,16 +26,24 @@
|
||||
运行函数，switch(状态) 运行相应函数
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
*/
|
||||
/*配合过程状态,co是合作的意思*/
|
||||
typedef enum {
|
||||
CoPREPARE, // 准备阶段
|
||||
CoTRANSLATE_OUT,//上方平移,去运球
|
||||
CoDRIBBLE, //运球和蓄力阶段,再平移回去
|
||||
CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
|
||||
CoPITCH, //发射
|
||||
CoDONE,
|
||||
|
||||
|
||||
}CoState_t;
|
||||
/*总配合上下文*/
|
||||
typedef struct {
|
||||
CoState_t CoState;
|
||||
|
||||
uint8_t is_initialized ;
|
||||
} CoContext_t;
|
||||
|
||||
|
||||
|
||||
@ -46,7 +54,9 @@ typedef enum {
|
||||
PITCH_START, //启动，拉扳机
|
||||
PITCH_PULL_TRIGGER,
|
||||
PITCH_LAUNCHING, // 发射中
|
||||
|
||||
PITCH_COMPLETE // 完成
|
||||
|
||||
} PitchState_t;
|
||||
|
||||
/* 投球参数配置 */
|
||||
@ -55,8 +65,10 @@ typedef struct {
|
||||
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机
|
||||
fp32 go1_init_position; // GO电机触发位置,0,初始位置
|
||||
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
|
||||
fp32 go1_Receive_ball; //用在配合过程，用来接平移后的球
|
||||
fp32 m2006_Screw_init;
|
||||
fp32 Pitch_angle;
|
||||
|
||||
} PitchConfig_t;
|
||||
|
||||
/* 投球控制上下文 */
|
||||
@ -108,8 +120,6 @@ typedef struct {
|
||||
|
||||
uint8_t is_initialized;
|
||||
|
||||
|
||||
|
||||
} DribbleContext_t;
|
||||
|
||||
|
||||
@ -164,7 +174,8 @@ typedef struct{
|
||||
|
||||
uint8_t up_task_run;
|
||||
const UP_Param_t *param;
|
||||
/*运球过程*/
|
||||
CoContext_t CoContext;
|
||||
/*运球过程*/
|
||||
DribbleContext_t DribbleContext;
|
||||
/*投篮过程*/
|
||||
PitchContext_t PitchContext;
|
||||
@ -262,7 +273,8 @@ int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *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,CMD_t *c);
|
||||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
|
||||
int8_t Co_Process(UP_t *u, CAN_Output_t *out);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -9,9 +9,9 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
|
||||
if (huart->Instance == USART3)
|
||||
return BSP_UART_REMOTE;
|
||||
else if (huart->Instance == USART1)
|
||||
return BSP_UART_NUC;
|
||||
else if (huart->Instance == USART6)
|
||||
return BSP_UART_RS485;
|
||||
else if (huart->Instance == USART6)
|
||||
return BSP_UART_NUC;
|
||||
/*
|
||||
else if (huart->Instance == USARTX)
|
||||
return BSP_UART_XXX;
|
||||
@ -95,9 +95,9 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
|
||||
case BSP_UART_REMOTE:
|
||||
return &huart3;
|
||||
case BSP_UART_RS485:
|
||||
return &huart6;
|
||||
case BSP_UART_NUC:
|
||||
return &huart1;
|
||||
case BSP_UART_NUC:
|
||||
return &huart6;
|
||||
/*
|
||||
case BSP_UART_XXX:
|
||||
return &huartX;
|
||||
|
||||
@ -13,6 +13,7 @@ typedef enum {
|
||||
BSP_UART_REMOTE,
|
||||
BSP_UART_RS485,
|
||||
BSP_UART_NUC,
|
||||
BSP_UART_VOFA,
|
||||
/* BSP_UART_XXX, */
|
||||
BSP_UART_NUM,
|
||||
BSP_UART_ERR,
|
||||
|
||||
@ -19,11 +19,11 @@ int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
uint16_t pulse;
|
||||
|
||||
int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle) {
|
||||
if (duty_cycle < 0.0f) duty_cycle = 0.f;
|
||||
|
||||
uint16_t pulse;
|
||||
/* 通过PWM通道对应定时器重载值和给定占空比，计算PWM周期值 */
|
||||
switch (ch) {
|
||||
case BSP_PWM_IMU_HEAT:
|
||||
@ -38,7 +38,9 @@ int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle) {
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// BSP_PWM_Stop(ch);
|
||||
pulse =0;
|
||||
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, pulse);
|
||||
//BSP_PWM_Stop(ch);
|
||||
}
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
@ -99,15 +99,17 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
|
||||
switch(cmd->cmd_status){
|
||||
|
||||
case MID:
|
||||
cmd->cmd_MID360.posx = n->navi.vx;
|
||||
cmd->cmd_MID360.posy = n->navi.vy;
|
||||
cmd->cmd_MID360.posw = n->navi.wz;
|
||||
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 PICK :
|
||||
cmd ->cmd_pick .posx =n->pick .posx ;
|
||||
cmd ->cmd_pick .posy =n->pick .posy ;
|
||||
cmd ->cmd_pick .posw =n->pick .posw ;
|
||||
case VISION :
|
||||
cmd ->cmd_pick .posx =n->camera.data1 ;
|
||||
cmd ->cmd_pick .posy =n->camera.data2 ;
|
||||
cmd ->cmd_pick .posw =n->camera.data3 ;
|
||||
|
||||
break;
|
||||
|
||||
@ -151,11 +153,11 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
|
||||
{
|
||||
|
||||
cmd ->CMD_CtrlType =AUTO;
|
||||
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达
|
||||
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360; //左中，右中，雷达
|
||||
|
||||
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
|
||||
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_PICK; //左中，右下，视觉
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_MID360_Pitch; //左中，右下，视觉
|
||||
|
||||
}
|
||||
else if(rc->dr16.sw_l==CMD_SW_DOWN)
|
||||
@ -189,8 +191,12 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
|
||||
if(cmd ->CMD_CtrlType ==AUTO){
|
||||
|
||||
/*自动下的*/
|
||||
if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =AUTO_NAVI;
|
||||
else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_PICK;
|
||||
if (rc->LD.key_C ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Adjust;
|
||||
else {
|
||||
|
||||
if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =Normal;
|
||||
else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Pitch;
|
||||
else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_MID360;
|
||||
else cmd ->CMD_mode =Normal ;
|
||||
|
||||
}
|
||||
@ -202,3 +208,4 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
|
||||
else cmd ->CMD_mode =Normal;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -8,7 +8,8 @@
|
||||
#include <string.h>
|
||||
|
||||
#define MID (0x09)
|
||||
#define PICK (0x06)
|
||||
#define VISION (0x02)
|
||||
#define NUC (0x08)
|
||||
|
||||
typedef enum{
|
||||
RCcontrol,//遥控器控制，左按键上，控制上层
|
||||
@ -22,8 +23,9 @@ typedef enum{
|
||||
|
||||
Normal, //无模式
|
||||
|
||||
AUTO_NAVI,
|
||||
AUTO_PICK,
|
||||
AUTO_MID360,
|
||||
AUTO_MID360_Pitch,
|
||||
AUTO_MID360_Adjust,//雷达调整
|
||||
|
||||
UP_Adjust,//上层调整
|
||||
|
||||
@ -41,17 +43,19 @@ typedef struct {
|
||||
fp32 vx;
|
||||
fp32 vy;
|
||||
fp32 wz;
|
||||
}navi;
|
||||
|
||||
fp32 pos;
|
||||
fp32 angle;
|
||||
char flag;
|
||||
|
||||
}MID360;
|
||||
struct
|
||||
{
|
||||
fp32 posx;
|
||||
fp32 posy;
|
||||
fp32 posw;
|
||||
}pick;
|
||||
struct
|
||||
{
|
||||
fp32 angle;
|
||||
}sick_cali;
|
||||
fp32 data1;
|
||||
fp32 data2;
|
||||
fp32 data3;
|
||||
}camera;//相机
|
||||
|
||||
} CMD_NUC_t;
|
||||
/* 拨杆位置 */
|
||||
typedef enum {
|
||||
@ -122,6 +126,8 @@ typedef struct {
|
||||
fp32 key_ctrl_l;
|
||||
fp32 key_ctrl_r;
|
||||
|
||||
fp32 pos;//雷达反馈go位置
|
||||
|
||||
fp32 Vx;
|
||||
fp32 Vy;
|
||||
fp32 Vw;
|
||||
|
||||
@ -8,6 +8,7 @@ static osThreadId_t thread_alert;
|
||||
|
||||
uint8_t nucbuf[31];
|
||||
|
||||
|
||||
static void NUC_IdleCallback(void) {
|
||||
osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
|
||||
detect_hook(NUC_TOE);
|
||||
@ -26,6 +27,36 @@ int8_t NUC_StartReceiving() {
|
||||
sizeof(nucbuf)) == HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
char SendBuffer[19];
|
||||
|
||||
int8_t NUC_StartSending(fp32 *data) {
|
||||
|
||||
|
||||
union
|
||||
{
|
||||
float x[4];
|
||||
char data[16];
|
||||
}instance;
|
||||
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
instance.x[i] = data[i];
|
||||
}
|
||||
|
||||
SendBuffer[0] = 0xFC; //发送ID
|
||||
SendBuffer[1] = 0x01; //控制帧
|
||||
for(int i = 2; i < 18; i++)
|
||||
{
|
||||
SendBuffer[i] = instance.data[i-2];
|
||||
}
|
||||
SendBuffer[18] = 0xFD; //结束符
|
||||
|
||||
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
|
||||
(uint8_t *)SendBuffer,
|
||||
sizeof(SendBuffer)) == HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
int8_t NUC_Restart(void) {
|
||||
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
|
||||
@ -42,8 +73,8 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
|
||||
if(n ==NULL) return DEVICE_ERR_NULL;
|
||||
union
|
||||
{
|
||||
float x[3];
|
||||
char data[12];
|
||||
float x[5];
|
||||
char data[20];
|
||||
}instance;
|
||||
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
|
||||
else{
|
||||
@ -51,7 +82,35 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
|
||||
n->ctrl_status =nucbuf[2];
|
||||
switch (n->status_fromnuc)
|
||||
{
|
||||
case MID://控制帧0x09
|
||||
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 控制帧
|
||||
@ -61,69 +120,37 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
|
||||
wz fp32
|
||||
0xFE TAIL
|
||||
*/
|
||||
if(nucbuf[15]!=TAIL)goto error;
|
||||
if(nucbuf[24]!=TAIL)goto error;
|
||||
instance.data[3] = nucbuf[6];
|
||||
instance.data[2] = nucbuf[5];
|
||||
instance.data[1] = nucbuf[4];
|
||||
instance.data[0] = nucbuf[3];
|
||||
n->navi.vx = instance.x[0]; //
|
||||
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->navi.vy = instance.x[1];//
|
||||
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->navi.wz = instance.x[2];//
|
||||
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];//
|
||||
|
||||
n->MID360.flag = nucbuf[23];//
|
||||
break;
|
||||
case PICK:
|
||||
/* 协议格式
|
||||
0xFF HEAD
|
||||
0x0X 控制帧
|
||||
0x01 相机帧
|
||||
cmd 8位
|
||||
dis 相机深度值
|
||||
posx 相机yaw轴值
|
||||
posy 相机pitch轴值
|
||||
0xFE TAIL
|
||||
*/
|
||||
// if(nucbuf[15]!=TAIL)goto error;
|
||||
instance.data[3] = nucbuf[6];
|
||||
instance.data[2] = nucbuf[5];
|
||||
instance.data[1] = nucbuf[4];
|
||||
instance.data[0] = nucbuf[3];
|
||||
n->pick.posx = instance.x[0]; //距离球中心的角度值
|
||||
instance.data[7] = nucbuf[10];
|
||||
instance.data[6] = nucbuf[9];
|
||||
instance.data[5] = nucbuf[8];
|
||||
instance.data[4] = nucbuf[7];
|
||||
n->pick.posy = instance.x[1];// 相机yaw轴
|
||||
instance.data[11] = nucbuf[14];
|
||||
instance.data[10] = nucbuf[13];
|
||||
instance.data[9] = nucbuf[12];
|
||||
instance.data[8] = nucbuf[11];
|
||||
n->pick.posw= instance.x[2];// 暂未用到
|
||||
break;
|
||||
// case SICK_CAIL:
|
||||
// if(nucbuf[15]!=TAIL)goto error;
|
||||
// instance.data[3] = nucbuf[14];
|
||||
// instance.data[2] = nucbuf[13];
|
||||
// instance.data[1] = nucbuf[12];
|
||||
// instance.data[0] = nucbuf[11];
|
||||
// n->sick_cali.angle = instance.x[0]; //
|
||||
// instance.data[7] = nucbuf[10];
|
||||
// instance.data[6] = nucbuf[9];
|
||||
// instance.data[5] = nucbuf[8];
|
||||
// instance.data[4] = nucbuf[7];
|
||||
// n->sick_cali.isleft = instance.x[1];//
|
||||
// instance.data[11] = nucbuf[14];
|
||||
// instance.data[10] = nucbuf[13];
|
||||
// instance.data[9] = nucbuf[12];
|
||||
// instance.data[8] = nucbuf[11];
|
||||
// n->pick.posw= instance.x[2];// 暂未用到
|
||||
// break;
|
||||
|
||||
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
@ -23,12 +23,13 @@ typedef struct {
|
||||
NUC_UpPackageMCU_t to_nuc; //发送的数据协议
|
||||
|
||||
uint8_t status;//控制状态
|
||||
uint8_t pit_status;//pit相机朝向
|
||||
|
||||
} NUC_t;
|
||||
|
||||
|
||||
int8_t NUC_Init(NUC_t *nuc);
|
||||
int8_t NUC_StartReceiving(void);
|
||||
int8_t NUC_StartSending(fp32 *data) ;
|
||||
bool_t NUC_WaitDmaCplt(void);
|
||||
int8_t NUC_RawParse(CMD_NUC_t *n);
|
||||
int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
|
||||
|
||||
@ -99,10 +99,25 @@ int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
|
||||
raw->ch[3] = 0.5*(raw->ch[3]); //w
|
||||
|
||||
|
||||
raw->map_ch[0]=map_fp32(raw->ch[0],-719,680,-1,1);
|
||||
raw->map_ch[1]=map_fp32(raw->ch[1],-653,746,-1,1);
|
||||
raw->map_ch[2]=map_fp32(raw->ch[2],95,1482,0,1);
|
||||
raw->map_ch[3]=map_fp32(raw->ch[3],-317,375,-1,1);
|
||||
if (raw->ch[0] < 0)
|
||||
raw->map_ch[0] = map_fp32(raw->ch[0], -696, 2, -1, 0);
|
||||
else
|
||||
raw->map_ch[0] = map_fp32(raw->ch[0], 2, 704, 0, 1);
|
||||
|
||||
// ch[1]
|
||||
if (raw->ch[1] < 0)
|
||||
raw->map_ch[1] = map_fp32(raw->ch[1], -638, 5, -1, 0);
|
||||
else
|
||||
raw->map_ch[1] = map_fp32(raw->ch[1], 5, 762, 0, 1);
|
||||
|
||||
raw->map_ch[2] = map_fp32(raw->ch[2], 2, 1377, 0, 1);
|
||||
|
||||
// ch[3]
|
||||
if (raw->ch[3] < 0)
|
||||
raw->map_ch[3] = map_fp32(raw->ch[3], -344, 0, -1, 0);
|
||||
else
|
||||
raw->map_ch[3] = map_fp32(raw->ch[3], 0, 348, 0, 1);
|
||||
|
||||
|
||||
/*非线性映射*/
|
||||
raw->map_ch[0]=expo_map(raw->map_ch[0],0.7f);
|
||||
|
||||
@ -28,13 +28,14 @@ void vofa_tx_main(float *data)
|
||||
|
||||
|
||||
/*在下面使用对应的串口发送函数*/
|
||||
// CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
|
||||
// osDelay(1);
|
||||
// CDC_Transmit_FS( tail, 4);
|
||||
// HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
|
||||
// osDelay(1);
|
||||
// HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_VOFA), tail, 4);
|
||||
// osDelay(1);
|
||||
CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
|
||||
osDelay(1);
|
||||
// CDC_Transmit_FS( tail, 4);
|
||||
// HAL_UART_Transmit_DMA(&huart1, tail, 4);
|
||||
// osDelay(1);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -11,7 +11,7 @@ static CMD_NUC_t cmd_fromnuc;
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
fp32 send[4]={11.0f,45.0,1.f,4.0f};
|
||||
void Task_nuc(void *argument){
|
||||
(void)argument; /**/
|
||||
|
||||
@ -29,6 +29,7 @@ void Task_nuc(void *argument){
|
||||
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
NUC_StartReceiving();
|
||||
// NUC_StartSending(NUC_send.send);
|
||||
if (NUC_WaitDmaCplt()){
|
||||
NUC_RawParse(&cmd_fromnuc);
|
||||
}
|
||||
|
||||
@ -70,7 +70,6 @@ void Task_up(void *argument)
|
||||
ALL_Motor_Control(&UP,&UP_CAN_out);
|
||||
|
||||
osDelay(1);
|
||||
|
||||
/*can上设备数据获取*/
|
||||
osMessageQueueGet(task_runtime.msgq.can.feedback.UP_CAN_feedback, &can, NULL, 0);
|
||||
|
||||
@ -92,8 +91,8 @@ void Task_up(void *argument)
|
||||
osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0);
|
||||
|
||||
|
||||
vofa_send [0]=UP.vofa_send [0];
|
||||
vofa_send [1]=UP.vofa_send [1];
|
||||
// vofa_send [0]=UP.vofa_send [0];
|
||||
// vofa_send [1]=UP.vofa_send [1];
|
||||
vofa_tx_main (vofa_send);
|
||||
|
||||
tick += delay_tick;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user