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

Compare commits

base: LYL:f9e93b88c15f9aa90615a1b07806aa5a1da70ebd
Branches Tags
LYL:main
LYL:ballgame
LYL:maingame
LYL:mid360_plan2
LYL:6.23
LYL:Cooperation
LYL:chassis
...
compare: LYL:40ca2de03870e52768d55866c673821ce0bd9692
Branches Tags
LYL:ballgame
LYL:main
LYL:maingame
LYL:mid360_plan2
LYL:6.23
LYL:Cooperation
LYL:chassis

3 Commits
f9e93b88c1 ... 40ca2de038

Author SHA1 Message Date
ZHAISHUI04
40ca2de038 sick改pid，远距离抖 2025-07-02 20:42:48 +08:00
ZHAISHUI04
67960d2805 比较完善的，手动，雷达，运球 2025-06-29 01:48:25 +08:00
ZHAISHUI04
866e4de877 优化 2025-06-27 23:05:03 +08:00
19 changed files with 838 additions and 414 deletions
Show Stats Expand all files Collapse all files

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

@ -475,3 +475,13 @@
[info] Log at : 2025/6/24|03:28:50|GMT+0800
[info] Log at : 2025/6/26|19:47:59|GMT+0800
[info] Log at : 2025/6/27|16:40:58|GMT+0800
[info] Log at : 2025/6/28|01:22:24|GMT+0800
[info] Log at : 2025/6/30|18:03:06|GMT+0800
[info] Log at : 2025/7/1|14:01:22|GMT+0800

3
MDK-ARM/.vscode/settings.json vendored
View File

@ -97,6 +97,7 @@
"gimbal_task.h": "c",
"gimgal.h": "c",
"navi.h": "c",
"up.h": "c"
"up.h": "c",
"bsp_usart.h": "c"
}
}

21
MDK-ARM/R2.uvoptx
View File

@ -158,42 +158,47 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>NUC_send,0x0A</ItemText>
<ItemText>tar_angle</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>SendBuffer,0x10</ItemText>
<ItemText>huart-&gt;ErrorCode</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText>
<ItemText>error_code</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf,0x10</ItemText>
<ItemText>chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>\\R2\../User/task/chassis_task.c\chassis.sick_cali.sickparam</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>a,0x0A</ItemText>
<ItemText>cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>posss</ItemText>
<ItemText>nucbuf,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>

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

Binary file not shown.

143
User/Module/Chassis.c
View File

@ -3,7 +3,6 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
/*
机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw)
@ -32,9 +31,14 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
//
// c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
// }
c->sick_cali.sick_dis[0]=can->sickfed.raw_dis[0] ;
c->sick_cali.sick_dis[1]=can->sickfed.raw_dis[1]+50 ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=can->sickfed.raw_dis[2] ;
c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ;
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
c->vofa_send[7] =c->sick_cali.sick_dis[1]-c->sick_cali.sick_dis[2];
c->vofa_send[6] = c->sick_cali.sick_dis[0];
c->vofa_send[5] =c->sick_cali.sick_dis[1];
c->vofa_send[4] =c->sick_cali.sick_dis[2];
return CHASSIS_OK;
}
@ -47,15 +51,25 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
}
PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
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));
/*sick*/
PID_init(&(c->pid.SickCaliWzInPID), PID_POSITION, &(c->param->SickCali_WInparam));
PID_init(&(c->pid.SickCaliWzOutPID), PID_POSITION, &(c->param->SickCali_WOutparam));
PID_init(&(c->pid.SickCaliVxInPID), PID_POSITION, &(c->param->SickCali_XInparam));
PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam));
PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam));
PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam));
/*修正 */
PID_init(&(c->pid.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滤波
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); // x滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f);
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f);
KalmanCreate(&c->extKalman[0],10,1);
KalmanCreate(&c->extKalman[1],10,1);
KalmanCreate(&c->extKalman[2],10,1);
c->sick_cali .sickparam=c->param ->sickparam ;
@ -66,7 +80,7 @@ fp32 pian_yaw;
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后
@ -84,12 +98,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
/*初始数据*/
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 0;
c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]);
switch (c->chassis_ctrl.ctrl) {
case RCcontrol:
switch (c->chassis_ctrl.mode) {
@ -112,9 +131,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
sick_calibration(c, ctrl, out);
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
break;
default:
break;
}
break;
@ -130,7 +147,12 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
// c->NUC_send.send[0] = 1;
break;
case AUTO_MID360_Pitch:
c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break;
case AUTO_MID360_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
@ -138,24 +160,48 @@ 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;
default:
break;
}
case PASS_BALL:
switch (c->chassis_ctrl.mode)
{
case PB_UP:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break ;
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
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;
}
}
break;
case RELAXED:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break ;
break;
break;
default:
break;
}
// 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f);
@ -171,7 +217,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
}
c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
return CHASSIS_OK;
@ -188,33 +234,50 @@ sick2,左1
*/
fp32 diff_yaw,diff_y,diff_x;
int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
{
if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL;
fp32 sick0 = c->sick_cali.sick_dis[0] ;
fp32 delta_w,delta_x,delta_y;
fp32 sick0 = c->sick_cali.sick_dis[0];
fp32 sick1 = c->sick_cali.sick_dis[1];
fp32 sick2 = c->sick_cali.sick_dis[2];
const sickparam_t *param = &c->sick_cali.sickparam;
diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2));
diff_y = LowPassFilter2p_Apply(&(c->filled[2]),-(fp32)(sick1 - c->sick_cali.sickparam.y_set));
diff_x = LowPassFilter2p_Apply(&(c->filled[3]),(fp32)(sick0 - c->sick_cali.sickparam.x_set));
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);
c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0;
c->move_vec.Vx = (fabsf(diff_x) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVxPID), diff_x, 0) : 0;
c->move_vec.Vy = (fabsf(diff_y) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVyPID), diff_y, 0) : 0;
delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
if(fabs(diff_yaw)>param->w_error){
c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0);
}
// if(fabs(diff_y)>param->xy_error){
//
// c->move_vec.Vy=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
// }
// if(fabs(diff_x)>param->xy_error){
//
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,-delta_x,0);
// }
static uint16_t reach_cnt = 0;
if (fabsf(diff_yaw) <= param->w_error &&
fabsf(diff_x) <= param->xy_error &&
fabsf(diff_y) <= param->xy_error) {
reach_cnt++;
if (reach_cnt >= 50) {
if (reach_cnt >= 20) {
c->sick_cali.is_reach = 1;
@ -235,7 +298,7 @@ int8_t Chassis_AngleCompensate(Chassis_t *c)
{
pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
}
else {
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0;

28
User/Module/Chassis.h
View File

@ -31,6 +31,7 @@
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include "kalman.h"
#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
@ -80,9 +81,13 @@ typedef struct
pid_param_t NaviVy_param;
pid_param_t NaviVw_param;
pid_param_t Sick_CaliWparam;
pid_param_t Sick_CaliYparam;
pid_param_t Sick_CaliXparam;
pid_param_t SickCali_WInparam;
pid_param_t SickCali_WOutparam;
pid_param_t SickCali_YInparam;
pid_param_t SickCali_YOutparam;
pid_param_t SickCali_XInparam;
pid_param_t SickCali_XOutparam;
pid_param_t Chassis_AngleAdjust_param;
sickparam_t sickparam;
@ -154,22 +159,27 @@ typedef struct{
pid_type_def chassis_PICKWzPID_HIGN;
/*存在较低误差*/
pid_type_def chassis_PICKWzPID_LOW;
pid_type_def Chassis_AngleAdjust;
pid_type_def SickCaliWzPID;
pid_type_def SickCaliVxPID;
pid_type_def SickCaliVyPID;
/*sick */
pid_type_def SickCaliWzInPID;
pid_type_def SickCaliWzOutPID;
pid_type_def SickCaliVxInPID;
pid_type_def SickCaliVxOutPID;
pid_type_def SickCaliVyInPID;
pid_type_def SickCaliVyOutPID;
}pid;
fp32 vofa_send[8];
/*卡尔曼滤波*/
extKalman_t extKalman[3];
LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
struct {
int32_t sick_dis[4]; //获取到的sick激光值
fp32 sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam;
int is_reach;
}sick_cali;

178
User/Module/config.c
View File

@ -37,18 +37,18 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f,
},
.Pitch_M2006_angle_param = {
.p = 600.0f,
.p = 1600.0f,
.i = 0.0f,
.d = 3.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
.i_limit = 4000.0f,
.out_limit = 20000.0f,
},
.Pitch_M2006_speed_param={
.p = 5.0f,
.i = 0.3f,
.p = 8.5f,
.i = 0.03f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
.out_limit = 15000.0f,
},
.Pitch_Angle_M2006_speed_param={ //俯仰跑速度环
.p = 20.0f,
@ -71,20 +71,40 @@ static const ConfigParam_t param ={
},
/*上层其他参数*/
/*上层其他参数,一些只是初始值，在运行过程中会被更改*/
/*投球*/
.PitchCfg = {
.m2006_init =-150, //释放的角度
.m2006_trig =0, //可以拉住发射的角度
.go_init = -50, //仅用在go上电，初始位置
.go_pull_pos =-214,
.Pitch_angle =58,
.pull_speed =5,
.LaunchCfg = {
.m2006_init = -150.0f, // M2006初始角度
.m2006_trig = 0.0f, // M2006触发角度
.go_pull_pos = -214.0f, // go上升去合并扳机，扳机位置
.go_up_speed = 12.0f, // 上升速度
.go_down_speed = 6.0f, // 下降速度
.Pitch_angle = 66, //俯仰角
.go_init = -50
},
.PitchCfg = {
.go_init = -50, //仅用在go上电，初始位置
.go_release_pos=-200,
.Pitch_angle =66,
},
.PassCfg={
.go_wait =-10,
.go_release_pos =-150,
.Pitch_angle=66,
.go_up_speed =12,
.go_down_speed =5,
},
.MID360Cfg={
.go_release_pos=-200, // GO电机发射位置
.go_up_speed=20, // GO电机上升速度
.go_down_speed=10, // GO电机下降速度
},
},
.chassis = {/**/
@ -111,28 +131,102 @@ static const ConfigParam_t param ={
.i_limit = 50.0f,
.out_limit =1000.0f,
},
.Sick_CaliWparam ={
.p = 4.5f,
.i = 0.005f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit =1000.0f,
},
.Sick_CaliYparam ={
.p = 2.9f,
.i = 0.0051f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit =3000.0f,
},
.Sick_CaliXparam ={
.p = 2.9f,
.i = 0.0065f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit =3000.0f,
},
.Chassis_AngleAdjust_param={
// .SickCali_WInparam = {
// .p = 3.0f,
// .i = 0.000f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
// .SickCali_WOutparam = {
// .p = 2.6f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_YInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_YOutparam = {
// .p = 4.5f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 00.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_XInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_XOutparam = {
// .p = 4.5f,
// .i = 0.00f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
.SickCali_WInparam = {
.p = 3.0f,
.i = 0.000f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 2000.0f,
},
.SickCali_WOutparam = {
.p = 2.6f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 1000.0f,
},
.SickCali_YInparam = {
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_YOutparam = {
.p = 4.5f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 00.0f,
.out_limit = 1000.0f,
},
.SickCali_XInparam = {
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_XOutparam = {
.p = 4.5f,
.i = 0.00f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 2000.0f,
},
.Chassis_AngleAdjust_param={
.p = 10.0f,
.i = 0.0f,
.d = 0.0f,
@ -141,10 +235,10 @@ static const ConfigParam_t param ={
},
.sickparam={
.w_error=5,
.xy_error=5,
.x_set=600,
.y_set=100,
.w_error=2,
.xy_error=3,
.x_set=10000,
.y_set=2000,
},

356
User/Module/up.c
View File

@ -43,13 +43,19 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
if (!u->Pitch_Cfg .is_init) {
u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
u->Pitch_Cfg .is_init = 1;
if (!u->PitchContext .is_init) {
u->PitchContext .PitchConfig = u->param ->PitchCfg ;//赋值
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
u->PitchContext .is_init = 1;
}
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
if(!u->PassContext.is_init) {
u->PassContext .PassCfg = u->param ->PassCfg ;//赋值
u->PassContext .PassState = PASS_STOP; //状态更新
u->PassContext .is_init = 1;
}
u->LaunchContext.LaunchCfg = u->param->LaunchCfg;
u->MID360Context.MID360Cfg = u->param->MID360Cfg;
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
}
@ -118,8 +124,6 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
//电机控制 ，传进can
@ -131,16 +135,17 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
/*俯仰2006，双环，内使用oid角度环，外电机速度环
*/
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 .Pitch_angle))
u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle)+0.14)
);
GO_SendData(
&u->motorfeedback.go_data,
&u->go_cmd,u->motor_target .go_shoot,
u->Pitch_Cfg .PitchConfig .pull_speed
);//对应的拉动速度
u->motor_target.go_pull_speed
);
for(int i=0;i<4;i++){
out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ;
@ -166,167 +171,260 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
if(c ==NULL) return 0;
/*指针简化*/
PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;
PitchCfg_t *pitch_cfg = &u->PitchContext.PitchConfig;
LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
up_motor_target_t *target=&u->motor_target ;
/*config值限位*/
abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-215.0f,0.0f);
abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.Pitch_angle ,48.0f,67.0f);
abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go_release_pos ,-215.0f,0.0f);
abs_limit_min_max_fp(&u->PitchContext.PitchConfig.Pitch_angle ,48.0f,67.0f);
/*部分数据更新*/
static int is_pitch=1;
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve);
// if (u->Pitch_Cfg.Curve == CURVE_58) {
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve);
// if (u->PitchContext.Curve == CURVE_58) {
// target->Pitch_angle = 58;
// } else {
// target->Pitch_angle = 66;
// }
u->vofa_send [2] = c->pos;
u->vofa_send [3] = LowPassFilter2p_Apply(&u->filled[0],c->pos);
u->vofa_send [4] =1;
switch (c->CMD_CtrlType )
{
case RCcontrol: //在手动模式下
switch (c-> CMD_mode )
{
case Normal :
case Normal : //复位，go位置和俯仰角保持LaunchCfg最后修改后的位置，扳机2006角度复位用于发射
/*投篮*/
if(is_pitch){
target->Shoot_M2006_angle = pitch_cfg->go_init ;
target->Pitch_angle = pitch_cfg->Pitch_angle;
target->go_shoot = LaunchCfg->go_init ;
target->Pitch_angle = LaunchCfg->Pitch_angle;
is_pitch=0;
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
} //初始上电
//LaunchCfg->go_up_speed=15;
target->Pitch_angle =LaunchCfg->Pitch_angle;
target->go_pull_speed=LaunchCfg->go_up_speed;
target->Shoot_M2006_angle=LaunchCfg->m2006_init;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
u->LaunchContext.LaunchState = Launch_Stop;
break;
case Pitch :
if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE)
if (u->PitchContext .PitchState ==PITCH_PREPARE)
{
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
}
Pitch_Process(u,out);
Pitch_Process(u,out,c);
break ;
case UP_Adjust: //测试用
pitch_cfg ->pull_speed=5;
case UP_Adjust: //测试用,手动使用pitch下的cfg
LaunchCfg->go_up_speed=5;
pitch_cfg ->go_release_pos += c->Vx ;
pitch_cfg->Pitch_angle += c->Vy/100;
LaunchCfg->Pitch_angle += c->Vy/100;
target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos;
target->Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
target->go_shoot=pitch_cfg ->go_release_pos;
target->Pitch_angle=LaunchCfg->Pitch_angle;
break ;
default:
break;
}
break;
case AUTO:
/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
if (u->Pitch_Cfg.Curve == CURVE_58) {
target->Pitch_angle = 58;
} else {
target->Pitch_angle = 66;
UP_AUTO_Control(u,out,c);
break ;
case PASS_BALL:
Pass_Process(u,out,c);
break;
case RELAXED:
// target->go_shoot= pitch_cfg .
target->Pitch_angle = 58;
target->go_shoot = -190.0f;
//失控下，最好切手动，手动用pitch
pitch_cfg ->go_release_pos=-190;
// target->Pitch_angle = 58;
break ;
}
return 0;
}
//复用发射，
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
switch(LaunchContext->LaunchState){
case Launch_Stop: break;
case Launch_PREPARE:
u->motor_target.go_shoot = StartPos;
if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&&
is_reached(u->motorfeedback.go_data.W,0,1.0f)){
//根据位置和速度判断是否到达初始位置
LaunchContext->LaunchState = Launch_START;
}break;
case Launch_START:
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos;
if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
LaunchContext->LaunchState = Launch_TRIGGER;
}break;
case Launch_TRIGGER:
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1){ //当2006的总角度小于1，可以认为已经勾上,误差为1
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ;
// if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
// LaunchContext->LaunchState = Launch_SHOOT_WAIT;
} break;
case Launch_SHOOT_WAIT:
if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射
LaunchContext->LaunchState = Launch_DONE;
break;
case Launch_DONE:
break ;
}
}
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{
/*简化*/
PitchCfg_t *cfg = &u->PitchContext.PitchConfig;
PitchState_t *state =&u->PitchContext .PitchState;
up_motor_target_t *target=&u->motor_target ;
LaunchContext_t *LaunchContext = &u->LaunchContext;
// 可根据实际需要传入不同的起始和末位置,起始：当前位置
Pitch_Launch_Sequence(u, LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
switch(*state){
case PITCH_START:
LaunchContext->LaunchState = Launch_PREPARE;
*state=PITCH_WAIT;
break;
case PITCH_WAIT:
if(LaunchContext->LaunchState==Launch_DONE)
*state=PITCH_COMPLETE;
break;
case PITCH_COMPLETE:
break;
default:
break;
}
return 0;
}
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
PassCfg_t *PassCfg = &u->PassContext.PassCfg;
PassState_t *state = &u->PassContext.PassState;
up_motor_target_t *target=&u->motor_target ;
LaunchContext_t *LaunchContext = &u->LaunchContext;
Pass_Sequence_Check(u,c);
PassCfg ->go_release_pos =
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3,4,&u->PassContext.Curve);
switch (*state) { //遥控器按键进行状态切换
case PASS_STOP:
break;
case PASS_IDLE:
LaunchContext->LaunchState = Launch_PREPARE;
break;
case PASS_PREPARE:
target->go_pull_speed=PassCfg->go_up_speed;
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
break;
case PASS_START:
if(LaunchContext->LaunchState == Launch_TRIGGER){
target->go_pull_speed=PassCfg->go_down_speed;
target->go_shoot = PassCfg->go_release_pos ;
}
break ;
case PASS_POS_PREPARE:
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射
break;
case PASS_COMPLETE:
break;
default:
break;
}
return 0;
}
int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
up_motor_target_t *target=&u->motor_target ;
LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3.2,4.3,&u->MID360Context.Curve);
if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58;
} else {
target->Pitch_angle = 66;
}
LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
switch(c-> CMD_mode)
{
case AUTO_MID360_Pitch:
if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE)
{
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
if(MID360Context->IsLaunch==0){
MID360Context->IsLaunch=1;
LaunchContext->LaunchState = Launch_PREPARE;
}
/*根据距离实时计算所需pos*/
pitch_cfg ->go_release_pos=
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve);
Pitch_Process(u,out);
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
break ;
case AUTO_MID360:
target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;
u->Pitch_Cfg .PitchState = PITCH_PREPARE;
target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init;
MID360Context->IsLaunch=0;
break ;
default:
break;
}
break ;
case RELAXED:
// target->go_shoot= pitch_cfg .
target->go_shoot = pitch_cfg->go_init;
target->Pitch_angle = 58;
target->Shoot_M2006_angle =pitch_cfg->m2006_init;
break ;
}
return 0;
}
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
{
PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig;
PitchState_t *state =&u->Pitch_Cfg .PitchState;
up_motor_target_t *target=&u->motor_target ;
switch(*state){
case PITCH_START:
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
cfg->pull_speed=12;
target->go_shoot = cfg->go_pull_pos;
if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0
u->Pitch_Cfg .PitchConfig .pull_speed=6;
*state=PITCH_PULL_TRIGGER;
}//更改标志位
break ;
case PITCH_PULL_TRIGGER:
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{
target->go_shoot=cfg->go_release_pos;
if(is_reached (u->motorfeedback .go_data .Pos ,target->go_shoot ,1.0f))
{
*state=PITCH_LAUNCHING;
}
}
break ;
case PITCH_LAUNCHING: //等待发射
// *state=PITCH_COMPLETE;
break ;
case PITCH_COMPLETE: //发射完成
break ;
}
return 0;
}
}

119
User/Module/up.h
View File

@ -28,26 +28,50 @@
*/
/*共用发射，重复部分*/
typedef struct {
fp32 m2006_init; // M2006初始角度
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
fp32 go_init;
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
fp32 go_up_speed;
fp32 go_down_speed;
fp32 Pitch_angle;
} LaunchCfg_t;
typedef enum {
Launch_Stop,
Launch_PREPARE, //
Launch_START, //启动，从指定位置上升，扣动扳机
Launch_TRIGGER, //拉动到指定位置
Launch_SHOOT_WAIT, // 发射等待
Launch_SHOOT,
Launch_DONE,
}LaunchState_t;
typedef struct {
LaunchCfg_t LaunchCfg;
LaunchState_t LaunchState;
} LaunchContext_t;
/* 投球状态定义 */
typedef enum {
PITCH_PREPARE, // 准备阶段
PITCH_START, //启动，拉扳机
PITCH_PULL_TRIGGER,
PITCH_LAUNCHING, // 发射中
PITCH_START, //启动，拉扳机
PITCH_WAIT, // 发射等待
PITCH_COMPLETE // 完成
} PitchState_t;
/* 投球参数配置 */
typedef struct {
fp32 m2006_init; // M2006初始角度
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
fp32 Pitch_angle; //俯仰角度，以oid为准
fp32 pull_speed;//go速度
fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
fp32 Pitch_angle; //俯仰角度，以oid为准
fp32 go_release_pos;//go松开，发射位置
} PitchCfg_t;
@ -55,19 +79,53 @@ typedef struct {
typedef struct {
PitchState_t PitchState;
PitchCfg_t PitchConfig;
uint8_t is_init ;
} PitchContext_t;
/*传球过程 */
typedef enum {
PASS_STOP, // 停止状态,未进入传球模式
PASS_IDLE, // 空闲状态，进入未开始
PASS_PREPARE, // 传球准备
PASS_START, // 启动传球,拉动到等球位置
PASS_POS_PREPARE, // 传球位置准备，对准篮筐，go的位置
PASS_COMPLETE // 发射
} PassState_t;
typedef struct {
fp32 go_wait; // GO等待位置
fp32 go_release_pos; // GO电机发射位置
fp32 Pitch_angle; // 俯仰角度，以oid为准
fp32 go_up_speed; // GO电机上升速度
fp32 go_down_speed; // GO电机下降速度
} PassCfg_t;
typedef struct {
PassState_t PassState;
PassCfg_t PassCfg;
CurveType Curve; //当前函数，雷达距离->pos
uint8_t is_init ;
} Pitch_Cfg_t;
} PassContext_t;
/*雷达自动*/
typedef struct {
fp32 go_release_pos; // GO电机发射位置
fp32 go_up_speed; // GO电机上升速度
fp32 go_down_speed; // GO电机下降速度
} MID360Cfg_t;
typedef struct {
MID360Cfg_t MID360Cfg;
CurveType Curve; //当前函数，雷达距离->pos
int IsLaunch; //是否在发射过程
} MID360Context_t;
/*运球*/
typedef enum {
DRIBBLE_PREPARE, // 准备阶段
DRIBBLE_START,
DRIBBLE_TRANSLATE, // 平移过程
DRIBBLE_PROCESS_DOWN, // 运球过程，球下落
DRIBBLE_PROCESS_UP, // 运球过程，球上升
DRIBBLE_DONE // 运球结束
} DribbleState_t;
typedef struct {
@ -91,7 +149,10 @@ typedef struct
pid_param_t Pitch_Angle_M2006_speed_param;
LaunchCfg_t LaunchCfg;
PitchCfg_t PitchCfg;
PassCfg_t PassCfg;
MID360Cfg_t MID360Cfg;
GO_MotorCmd_t go_cmd;
}UP_Param_t;
@ -103,7 +164,7 @@ typedef struct{
fp32 Shoot_M2006_angle;
fp32 go_shoot;
fp32 go_pull_speed;
fp32 Pitch_angle; //以oid的角度为准,目标俯仰角
}up_motor_target_t;
@ -128,10 +189,14 @@ typedef struct{
typedef struct{
uint8_t up_task_run;
const UP_Param_t *param;
const UP_Param_t *param;
LaunchContext_t LaunchContext;
/*投篮过程*/
Pitch_Cfg_t Pitch_Cfg;
PitchContext_t PitchContext;
/*传球过程*/
PassContext_t PassContext;
/*自动过程*/
MID360Context_t MID360Context;
CMD_t *cmd;
struct{
@ -184,7 +249,11 @@ 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 Pitch_Process(UP_t *u, CAN_Output_t *out);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c);
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c);
#endif

79
User/Module/up_utils.c
View File

@ -1,5 +1,5 @@
#include "up_utils.h"
#include "up.h"
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
{
@ -62,25 +62,27 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
}
// 计算58度曲线
static float curve_58(float d) {
return 0.448f * d * d + 24.8604f * d - 177.99552f;
}
// 计算66度曲线
// 计算66度曲线（偏上）
// 计算66度曲线（偏上）
static float curve_66(float d) {
return 8.84935f * d * d - 10.5424f * d - 126.791f;
return 4.0310f * d * d + 8.9026f * d -139.5156;
}
// 计算58度曲线（偏下）
// 计算58度曲线（偏下）
static float curve_58(float d) {
return -1.9776f * d * d + 42.8499f * d - 204.2442f;
}
/*
曲线切换,用于距离和pos拟合
迟滞区x-y
曲线x重合区，根据当前函数和变化方向切换
*/
int abdddd=0;
float CurveChange(float d, float x, float y, CurveType *cs)
{
if (d<3.2) abdddd++;
if (*cs == CURVE_66) {
if (d > y) {
*cs = CURVE_58;
@ -99,3 +101,62 @@ float CurveChange(float d, float x, float y, CurveType *cs)
}
}
int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，按需求改
{
PassState_t *state = &u->PassContext.PassState;
static enum {
SEQ_IDLE,
SEQ_MID1,
SEQ_UP,
SEQ_MID2,
SEQ_DOWN
} seq = SEQ_IDLE;
switch (seq) {
case SEQ_IDLE:
if (c->CMD_mode == PB_MID) {
seq = SEQ_MID1;
*state = PASS_IDLE;
}
break;
case SEQ_MID1:
if (c->CMD_mode == PB_UP) {
seq = SEQ_UP;
*state = PASS_PREPARE;
} else if (c->CMD_mode != PB_MID) {
seq = SEQ_IDLE;
*state = PASS_STOP;
}
break;
case SEQ_UP:
if (c->CMD_mode == PB_MID) {
seq = SEQ_MID2;
*state = PASS_START;
} else if (c->CMD_mode != PB_UP) {
seq = SEQ_IDLE;
*state = PASS_STOP;
}
break;
case SEQ_MID2:
if (c->CMD_mode == PB_DOWN) {
seq = SEQ_DOWN;
*state = PASS_POS_PREPARE;
} else if (c->CMD_mode != PB_MID) {
seq = SEQ_IDLE;
*state = PASS_STOP;
}
break;
case SEQ_DOWN:
if (c->CMD_mode == PB_MID) {
seq = SEQ_IDLE;
*state = PASS_IDLE; // 或 PASS_COMPLETE，看你需求
} else if (c->CMD_mode != PB_DOWN) {
seq = SEQ_IDLE;
*state = PASS_STOP;
}
break;
}
return 0;
}

8
User/device/cmd.c
View File

@ -164,10 +164,10 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
}
else if(rc->dr16.sw_l==CMD_SW_DOWN)
{
cmd ->CMD_CtrlType =RCcontrol;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
cmd ->CMD_CtrlType =PASS_BALL;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =PB_UP; //左下，右上，无模式
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = PB_MID; //左下，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =PB_DOWN; //左下，右上，无模式
}

9
User/device/cmd.h
View File

@ -14,6 +14,7 @@
typedef enum{
RCcontrol,//遥控器控制，左按键上，控制上层
AUTO,
PASS_BALL,//传球模式
RELAXED,//异常模式
}CMD_CtrlType_t;
@ -29,11 +30,13 @@ typedef enum{
UP_Adjust,//上层调整
Chassis_Adjust_Sick,
PassBall,
/*传球占三个,逻辑在up.h里，简化就这样写了*/
PB_UP,
PB_MID,
PB_DOWN,
Pitch, //投篮,底盘锁定
Pitch,
Dribbl_transfer

275
User/device/nuc.c
View File

@ -2,176 +2,181 @@
#include "crc16.h"
#include <string.h>
#include "error_detect.h"
static volatile uint32_t drop_message = 0;
volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
uint8_t nucbuf[31];
char SendBuffer[7];
uint8_t nucbuf[25];
char SendBuffer[7];
static void NUC_IdleCallback(void) {
osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
detect_hook(NUC_TOE);
static void NUC_CBLTCallback(void)
{
osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
// detect_hook(NUC_TOE);
}
int8_t NUC_Init(NUC_t *nuc){
if(nuc == NULL) return DEVICE_ERR_NULL;
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_NUC,BSP_UART_IDLE_LINE_CB,
NUC_IdleCallback);
return DEVICE_OK;
static void NUC_ERRORCALLBACK(void)
{
NUC_Restart();
uint32_t error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_NUC));
// osThreadFlagsSet(thread_alert, SIGNAL_NUC_ERROR);
}
int8_t NUC_StartReceiving() {
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
int8_t NUC_Init(NUC_t *nuc)
{
if (nuc == NULL)
return DEVICE_ERR_NULL;
if ((thread_alert = osThreadGetId()) == NULL)
return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_RX_CPLT_CB,
NUC_CBLTCallback);
BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_ERROR_CB,
NUC_ERRORCALLBACK);
return DEVICE_OK;
}
int8_t NUC_StartReceiving()
{
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)nucbuf,
sizeof(nucbuf)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
int8_t NUC_StartSending(fp32 *data) {
int8_t NUC_StartSending(fp32 *data)
{
union
{
float x[1];
char data[4];
}instance;
// for (int i = 0; i < 1; i++) {
instance.x[0] = data[0];
// }
SendBuffer[0] = 0xFC; //帧头
SendBuffer[1] = 0x01; //控制帧
for(int i = 2; i < 6; i++)
union
{
SendBuffer[i] = instance.data[i-2];
}
SendBuffer[6] = 0xFD; //帧尾
float x[1];
char data[4];
} instance;
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)SendBuffer,
sizeof(SendBuffer)) == HAL_OK)
// for (int i = 0; i < 1; i++) {
instance.x[0] = data[0];
// }
SendBuffer[0] = 0xFC; // 帧头
SendBuffer[1] = 0x01; // 控制帧
for (int i = 2; i < 6; i++)
{
SendBuffer[i] = instance.data[i - 2];
}
SendBuffer[6] = 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) {
int8_t NUC_Restart(void)
{
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC));
return DEVICE_OK;
}
bool_t NUC_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,20) ==
bool_t NUC_WaitDmaCplt(void)
{
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 20) ==
SIGNAL_NUC_RAW_REDY);
}
int8_t NUC_RawParse(CMD_NUC_t *n){
if(n ==NULL) return DEVICE_ERR_NULL;
union
{
float x[5];
char data[20];
}instance;
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
else{
n->status_fromnuc =nucbuf[1];
n->ctrl_status =nucbuf[2];
int8_t NUC_RawParse(CMD_NUC_t *n)
{
if (n == NULL)
return DEVICE_ERR_NULL;
union
{
float x[5];
char data[20];
} instance;
if (nucbuf[0] != HEAD)
goto error; // 发送ID不是底盘
else
{
n->status_fromnuc = nucbuf[1];
n->ctrl_status = nucbuf[2];
switch (n->status_fromnuc)
{
case VISION://控制帧0x02
/* 协议格式
0xFF HEAD
0x02 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
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];//
instance.data[3] = nucbuf[3];
instance.data[2] = nucbuf[4];
instance.data[1] = nucbuf[5];
instance.data[0] = nucbuf[6];
n->camera.data1 = instance.x[0]; //
instance.data[7] = nucbuf[7];
instance.data[6] = nucbuf[8];
instance.data[5] = nucbuf[9];
instance.data[4] = nucbuf[10];
n->camera.data2 = instance.x[1]; //
instance.data[11] = nucbuf[11];
instance.data[10] = nucbuf[12];
instance.data[9] = nucbuf[13];
instance.data[8] = nucbuf[14];
n->camera.data3 = instance.x[2]; //
break;
case MID ://控制帧0x08
/* 协议格式
0xFF HEAD
0x09 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
if(nucbuf[24]!=TAIL)goto error;
instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[3];
case MID: // 控制帧0x08
/* 协议格式
0xFF HEAD
0x09 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
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->MID360.vx = instance.x[0]; //
instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7];
n->MID360.vy = instance.x[1];//
instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->MID360.wz = instance.x[2];//
instance.data[15] = nucbuf[18];
instance.data[14] = nucbuf[17];
instance.data[13] = nucbuf[16];
instance.data[12] = nucbuf[15];
n->MID360.pos = instance.x[3];//
instance.data[19] = nucbuf[22];
instance.data[18] = nucbuf[21];
instance.data[17] = nucbuf[20];
instance.data[16] = nucbuf[19];
n->MID360.angle = instance.x[4];//
instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7];
n->MID360.vy = instance.x[1]; //
instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->MID360.wz = instance.x[2]; //
instance.data[15] = nucbuf[18];
instance.data[14] = nucbuf[17];
instance.data[13] = nucbuf[16];
instance.data[12] = nucbuf[15];
n->MID360.pos = instance.x[3]; //
instance.data[19] = nucbuf[22];
instance.data[18] = nucbuf[21];
instance.data[17] = nucbuf[20];
instance.data[16] = nucbuf[19];
n->MID360.angle = instance.x[4]; //
n->MID360.flag = nucbuf[23]; //
n->MID360.flag = nucbuf[23];//
break;
}
return DEVICE_OK;
return DEVICE_OK;
}
error:
drop_message++;
return DEVICE_ERR;
error:
drop_message++;
NUC_Restart();
return DEVICE_ERR;
}
int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
{
if(cmd == NULL) return DEVICE_ERR_NULL;
memset(cmd, 0, sizeof(*cmd));
return 0;
if (cmd == NULL)
return DEVICE_ERR_NULL;
// memset(cmd, 0, sizeof(*cmd));
return 0;
}

2
User/device/nuc.h
View File

@ -33,5 +33,7 @@ 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);
int8_t NUC_Restart(void);
#endif

4
User/device/vofa.c
View File

@ -59,7 +59,7 @@ void vofa_tx_main(float *data)
/*在下面使用对应的串口发送函数*/
// HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet));
// CDC_Transmit_FS( packet, sizeof(packet));
// osDelay(1);
CDC_Transmit_FS( packet, sizeof(packet));
osDelay(1);
}

2
User/task/chassis_task.c
View File

@ -98,7 +98,7 @@ void Task_Chassis(void *argument)
vofa_send[6] = chassis.vofa_send[6];
vofa_send[7] = chassis.vofa_send[7];
vofa_tx_main(vofa_send);
// vofa_tx_main(vofa_send);
tick += delay_tick;
osDelayUntil(tick);

6
User/task/nuc_task.c
View File

@ -18,7 +18,7 @@ int a=0;
void Task_nuc(void *argument){
(void)argument; /**/
// osDelay(TASK_INIT_DELAY_NUC);
osDelay(TASK_INIT_DELAY_NUC);
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;
NUC_Init(&nuc_raw);
@ -35,8 +35,8 @@ void Task_nuc(void *argument){
#endif
a++;
NUC_StartReceiving();
NUC_StartSending(NUC_send.send);
// NUC_StartSending (send);
// NUC_StartSending(NUC_send.send);
// NUC_StartSending (send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc);
}

7
User/task/up_task.c
View File

@ -91,8 +91,11 @@ 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_send [2]=UP.vofa_send [2];
vofa_send [3]=UP.vofa_send [3];
vofa_send [4]=UP.vofa_send [4];
vofa_tx_main (vofa_send);
tick += delay_tick;

2
User/task/user_task.h
View File

@ -18,7 +18,7 @@
#define TASK_FREQ_UP (900u)
#define TASK_FREQ_CTRL_CMD (500u)
#define TASK_FREQ_NUC (50u)
#define TASK_FREQ_NUC (500u)
#define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_RC (1000u)