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

协议修改

Browse Source
This commit is contained in:
ZHAISHUI04 2025-07-08 07:44:04 +08:00
parent 2403b3bf52
commit de7321138f
16 changed files with 114 additions and 234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -485,3 +485,5 @@
[info] Log at : 2025/7/1|14:01:22|GMT+0800
[info] Log at : 2025/7/7|02:36:22|GMT+0800

55
MDK-ARM/R2.uvoptx
View File

@ -134,13 +134,13 @@
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>DLGUARM</Key>
<Name></Name>
<Key>CMSIS_AGDI</Key>
<Name>-X"" -O206 -S8 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
<Key>DLGUARM</Key>
<Name></Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
@ -198,42 +198,7 @@
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>NUC_send</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>ang_58</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>ang_66</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>Nuc,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>rec_flag</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl</ItemText>
<ItemText>\\R2\../User/task/up_task.c\UP.MID360Context.IsLaunch</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
@ -242,7 +207,7 @@
<DebugFlag>
<trace>0</trace>
<periodic>1</periodic>
<aLwin>1</aLwin>
<aLwin>0</aLwin>
<aCover>0</aCover>
<aSer1>0</aSer1>
<aSer2>0</aSer2>
@ -938,7 +903,7 @@
<Group>
<GroupName>User/bsp</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1218,7 +1183,7 @@
<Group>
<GroupName>User/Algorithm</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1390,7 +1355,7 @@
<Group>
<GroupName>Application/User/USB_DEVICE/App</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1434,7 +1399,7 @@
<Group>
<GroupName>Application/User/USB_DEVICE/Target</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>

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

Binary file not shown.

1
README.md
View File

@ -0,0 +1 @@
4.16 把整车用一个c板控制，添加主要代码，后续增加上层

48
User/Module/Chassis.c
View File

@ -35,10 +35,6 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
c->vofa_send[7] =c->sick_cali.sick_dis[1]-c->sick_cali.sick_dis[2];
c->vofa_send[6] = c->sick_cali.sick_dis[0];
c->vofa_send[5] =c->sick_cali.sick_dis[1];
c->vofa_send[4] =c->sick_cali.sick_dis[2];
return CHASSIS_OK;
}
@ -58,6 +54,12 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam));
PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam));
PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam));
PID_init(&(c->pid.Mid360AngleInPID), PID_POSITION, &(c->param->Mid360AngleInPID_param));
PID_init(&(c->pid.Mid360AngleOutPID), PID_POSITION, &(c->param->Mid360AngleOutPID_param));
/*修正 */
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
@ -98,16 +100,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
/*初始数据*/
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->move_vec.Vw = 0;
c->to_nuc.send = 0;
c->sick_cali.is_reach = 0;
c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]);
c->move_vec.Vw=0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
@ -131,9 +131,9 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break;
case Chassis_Adjust_Sick:
// sick_calibration(c, ctrl, out);
sick_calibration(c, ctrl, out);
// c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0;
break;
break;
}
break;
@ -141,22 +141,13 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
case AUTO:
switch (c->chassis_ctrl.mode) {
case AUTO_MID360:
// c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
// c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
// c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
c->move_vec.Vw =-ctrl->cmd_MID360.posw * 3000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break;
case AUTO_MID360_Pitch:
// c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
// c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
// c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break;
case REPOSITION:
@ -178,9 +169,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break ;
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
@ -314,3 +303,4 @@ int8_t Chassis_AngleCompensate(Chassis_t *c)
}

10
User/Module/Chassis.h
View File

@ -90,6 +90,12 @@ typedef struct
pid_param_t Chassis_AngleAdjust_param;
pid_param_t Mid360AngleInPID_param;
pid_param_t Mid360AngleOutPID_param;
sickparam_t sickparam;
}Chassis_Param_t;
@ -163,6 +169,10 @@ typedef struct{
pid_type_def SickCaliVyInPID;
pid_type_def SickCaliVyOutPID;
pid_type_def Mid360AngleInPID;
pid_type_def Mid360AngleOutPID;
}pid;

21
User/Module/config.c
View File

@ -235,6 +235,27 @@ static const ConfigParam_t param ={
.out_limit =1000.0f,
},
.Mid360AngleInPID_param={
.p = 0.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =1000.0f,
},
.Mid360AngleOutPID_param={
.p = 10.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =1000.0f,
},
.sickparam={
.w_error=2,
.xy_error=3,

16
User/Module/up.c
View File

@ -181,10 +181,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
/*部分数据更新*/
static int is_pitch=1;
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve);
u->vofa_send[0] = u->MID360Context.MID360Cfg.go_release_pos;
u->vofa_send[1] = u->motorfeedback.go_data.Pos;
u->vofa_send[2] = c->pos;
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.4,4.2,&u->MID360Context.Curve);
switch (c->CMD_CtrlType )
{
@ -364,7 +361,7 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
PassCfg ->go_release_pos =
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve);
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3,4,&u->PassContext.Curve);
switch (*state) { //遥控器按键进行状态切换
case PASS_STOP:
@ -403,14 +400,7 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
if(u->MID360Context.Curve==CURVE_58){
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.25,3.2,4.3,&u->MID360Context.Curve);
}
else {
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve);
}
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.2,4.3,&u->MID360Context.Curve);
if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58;
} else {

12
User/Module/up_utils.c
View File

@ -65,7 +65,7 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
}
// 计算66度曲线（偏上）
// 计算66度曲线（偏上）
static float curve_66(float d) {
return 3.7028f * d * d + 11.2126f * d -142.9446f;
@ -81,16 +81,12 @@ static float curve_58(float d) {
迟滞区x-y
曲线x重合区，根据当前函数和变化方向切换
*/
int abdddd=0;
fp32 ang_58;
fp32 ang_66;
float CurveChange(float d, float x, float y, CurveType *cs)
{
if (d<3.2) abdddd++;
if (*cs == CURVE_66) {
if (d > y) {
*cs = CURVE_58;
*cs = CURVE_58;
}
} else { // CURVE_58
if (d < x) {
@ -100,10 +96,8 @@ float CurveChange(float d, float x, float y, CurveType *cs)
// 根据当前曲线返回结果
if (*cs == CURVE_58) {
ang_58=d;
return curve_58(d);
} else {
ang_66=d;
return curve_66(d);
}
}

21
User/bsp/protocol.h
View File

@ -37,19 +37,11 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) {
Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确
uint8_t status; /* 控制命令 */
struct __attribute__((packed)) {
float posy; /*pitch轴*/
float posx; /*yaw轴*/
char cmd;
float dis; /*距离*/
}pick;
struct __attribute__((packed)) {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
float dis;
}navi;
} Protocol_DownDataChassis_t;
@ -83,17 +75,17 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed))
{
fp32 status;
uint8_t status;
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 上层机构数据结构体*/
typedef struct __attribute__((packed)) {
uint8_t status; /* 控制命令 */
float angle; // 角度信息
float dis; // 位移信息
} Protocol_DownDataUpper_t;
typedef struct __attribute__((packed)) {
Protocol_UpDataMCU_t data;
} Protocol_UpPackageMCU_t;
@ -106,6 +98,7 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) {
Protocol_DownDataUpper_t data;
} Protocol_DownPackageUpper_t;
#ifdef __cplusplus
}
#endif

31
User/device/cmd.c
View File

@ -87,44 +87,25 @@ return 0;
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
if(cmd == NULL) return -1;
if(n == NULL) return -1;
if(n == NULL) return -1;
cmd->cmd_status = n->status_fromnuc;
cmd->raw_status = n->ctrl_status;
for (int i = 0; i < 7; ++i)
{ // 从最低位到最高位遍历
cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
}
switch(cmd->cmd_status){
case MID:
cmd->cmd_MID360.posx = n->MID360.vx;
cmd->cmd_MID360.posy = n->MID360.vy;
cmd->cmd_MID360.posw = n->MID360.wz;
cmd->pos =n->MID360 .pos ;
break;
case VISION :
cmd ->cmd_pick .posx =n->camera.data1 ;
cmd ->cmd_pick .posy =n->camera.data2 ;
cmd ->cmd_pick .posw =n->camera.data3 ;
cmd->cmd_MID360.angle = n->MID360.angle;
cmd->cmd_MID360.dis = n->MID360.dis;
break;
default:
cmd->cmd_MID360.posx = 0;
cmd->cmd_MID360.posy = 0;
cmd->cmd_MID360.posw = 0;
cmd->pos =0 ;
break ;
}
return 0;
}

19
User/device/cmd.h
View File

@ -42,19 +42,12 @@ typedef enum{
Dribbl_transfer
}CMD_mode_t;
typedef struct {
uint8_t status_fromnuc;
uint8_t ctrl_status;
struct
{
fp32 vx;
fp32 vy;
fp32 wz;
fp32 pos;
fp32 angle;
char flag;
fp32 dis;
}MID360;
struct
@ -149,9 +142,9 @@ typedef struct {
}cmd_pick;
struct
{
fp32 posx;
fp32 posy;
fp32 posw;
fp32 angle;
fp32 dis;
}cmd_MID360;

89
User/device/nuc.c
View File

@ -6,7 +6,7 @@ volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
uint8_t nucbuf[25];
uint8_t nucbuf[10];
char SendBuffer[7];
static void NUC_CBLTCallback(void)
@ -58,86 +58,29 @@ int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
return DEVICE_ERR_NULL;
union
{
float x[5];
char data[20];
float x[2];
char data[8];
} instance;
if (nucbuf[0] != HEAD)
goto error; // 发送ID不是底盘
else
{
n->status_fromnuc = nucbuf[1];
n->ctrl_status = nucbuf[2];
switch (n->status_fromnuc)
{
case VISION: // 控制帧0x02
/* 协议格式
0xFF HEAD
0x02 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
instance.data[3] = nucbuf[3];
instance.data[2] = nucbuf[4];
instance.data[1] = nucbuf[5];
instance.data[0] = nucbuf[6];
n->camera.data1 = instance.x[0]; //
instance.data[7] = nucbuf[7];
instance.data[6] = nucbuf[8];
instance.data[5] = nucbuf[9];
instance.data[4] = nucbuf[10];
n->camera.data2 = instance.x[1]; //
instance.data[11] = nucbuf[11];
instance.data[10] = nucbuf[12];
instance.data[9] = nucbuf[13];
instance.data[8] = nucbuf[14];
n->camera.data3 = instance.x[2]; //
break;
case MID: // 控制帧0x08
/* 协议格式
0xFF HEAD
0x09 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
if (nucbuf[24] != TAIL)
if (nucbuf[9] != TAIL)
goto error;
instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[3];
n->MID360.vx = instance.x[0]; //
instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7];
n->MID360.vy = instance.x[1]; //
instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->MID360.wz = instance.x[2]; //
instance.data[15] = nucbuf[18];
instance.data[14] = nucbuf[17];
instance.data[13] = nucbuf[16];
instance.data[12] = nucbuf[15];
n->MID360.pos = instance.x[3]; //
instance.data[19] = nucbuf[22];
instance.data[18] = nucbuf[21];
instance.data[17] = nucbuf[20];
instance.data[16] = nucbuf[19];
n->MID360.angle = instance.x[4]; //
instance.data[3] = nucbuf[4];
instance.data[2] = nucbuf[3];
instance.data[1] = nucbuf[2];
instance.data[0] = nucbuf[1];
n->MID360.angle = instance.x[0]; //
instance.data[7] = nucbuf[8];
instance.data[6] = nucbuf[7];
instance.data[5] = nucbuf[6];
instance.data[4] = nucbuf[5];
n->MID360.dis = instance.x[1]; //
n->MID360.flag = nucbuf[23]; //
break;
}
nuc->unc_online = true; // 设置为在线状态
return DEVICE_OK;
}

18
User/task/cmd_task.c
View File

@ -16,7 +16,7 @@ static CMD_NUC_t Nuc;
#endif
int rec_flag=0;
void Task_cmd(void *argument){
(void)argument; /*未使用传递参数 消除警告*/
@ -38,16 +38,14 @@ void Task_cmd(void *argument){
osKernelLock(); /*锁住RTOS内核调度*/
/*将各任务接收到的原始数据解析为通用的控制命令*/
if((osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)){ //nuc
}
else
{
memset(&Nuc, 0, sizeof(CMD_NUC_t));
}
CMD_ParseNuc(&cmd,&Nuc);
/*注意，不能将nuc和码盘导航一块使用*/
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
CMD_ParseNuc(&cmd,&Nuc);
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
CMD_ParseRc(&cmd, &rc_ctrl);

1
User/task/nuc_task.c
View File

@ -40,7 +40,6 @@ void Task_nuc(void *argument){
#ifdef DEBUG
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
#endif
a++;
NUC_StartReceiving();
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc, &nuc_raw);

4
User/task/user_task.h
View File

@ -17,10 +17,10 @@
#define TASK_FREQ_CHASSIS (900u)
#define TASK_FREQ_UP (900u)
#define TASK_FREQ_CTRL_CMD (100u)
#define TASK_FREQ_CTRL_CMD (500u)
#define TASK_FREQ_NUC (100u)
#define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_RC (1000u)
#define TASK_FREQ_RC (100u)
#define TASK_FREQ_ERROR_DTC (3u)