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

3.30，测的底盘，雷达串口数据有问题，但视觉数据接收没问题

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-02 17:10:39 +08:00
parent ced203a924
commit b9f733f116
12 changed files with 236 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
MDK-ARM/.vscode/c_cpp_properties.json vendored
View File

@ -307,6 +307,114 @@
"__vfp_status(x,y)=0"
],
"intelliSenseMode": "${default}"
},
{
"name": "R2_CHASSIS",
"includePath": [
"d:\\Desktop\\r2\\R2_CHASSIS\\Core\\Inc",
"d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\CMSIS\\Include",
"d:\\Desktop\\r2\\R2_CHASSIS\\User",
"d:\\Desktop\\r2\\R2_CHASSIS\\User\\bsp",
"d:\\Desktop\\r2\\R2_CHASSIS\\User\\device",
"d:\\Desktop\\r2\\R2_CHASSIS\\User\\task",
"d:\\Desktop\\r2\\R2_CHASSIS\\User\\Algorithm",
"d:\\Desktop\\r2\\R2_CHASSIS\\User\\Module",
"d:\\Desktop\\r2\\R2_CHASSIS\\MDK-ARM",
"d:\\Desktop\\r2\\R2_CHASSIS\\USB_DEVICE\\App",
"d:\\Desktop\\r2\\R2_CHASSIS\\USB_DEVICE\\Target",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc",
"d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\CMSIS\\Lib\\ARM",
"d:\\Desktop\\r2\\R2_CHASSIS\\Core\\Src",
"d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src",
"d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src"
],
"defines": [
"USE_HAL_DRIVER",
"STM32F407xx",
"__CC_ARM",
"__arm__",
"__align(x)=",
"__ALIGNOF__(x)=",
"__alignof__(x)=",
"__asm(x)=",
"__forceinline=",
"__restrict=",
"__global_reg(n)=",
"__inline=",
"__int64=long long",
"__INTADDR__(expr)=0",
"__irq=",
"__packed=",
"__pure=",
"__smc(n)=",
"__svc(n)=",
"__svc_indirect(n)=",
"__svc_indirect_r7(n)=",
"__value_in_regs=",
"__weak=",
"__writeonly=",
"__declspec(x)=",
"__attribute__(x)=",
"__nonnull__(x)=",
"__register=",
"__breakpoint(x)=",
"__cdp(x,y,z)=",
"__clrex()=",
"__clz(x)=0U",
"__current_pc()=0U",
"__current_sp()=0U",
"__disable_fiq()=",
"__disable_irq()=",
"__dmb(x)=",
"__dsb(x)=",
"__enable_fiq()=",
"__enable_irq()=",
"__fabs(x)=0.0",
"__fabsf(x)=0.0f",
"__force_loads()=",
"__force_stores()=",
"__isb(x)=",
"__ldrex(x)=0U",
"__ldrexd(x)=0U",
"__ldrt(x)=0U",
"__memory_changed()=",
"__nop()=",
"__pld(...)=",
"__pli(...)=",
"__qadd(x,y)=0",
"__qdbl(x)=0",
"__qsub(x,y)=0",
"__rbit(x)=0U",
"__rev(x)=0U",
"__return_address()=0U",
"__ror(x,y)=0U",
"__schedule_barrier()=",
"__semihost(x,y)=0",
"__sev()=",
"__sqrt(x)=0.0",
"__sqrtf(x)=0.0f",
"__ssat(x,y)=0",
"__strex(x,y)=0U",
"__strexd(x,y)=0",
"__strt(x,y)=",
"__swp(x,y)=0U",
"__usat(x,y)=0U",
"__wfe()=",
"__wfi()=",
"__yield()=",
"__vfp_status(x,y)=0"
],
"intelliSenseMode": "${default}"
}
],
"version": 4

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

@ -449,3 +449,5 @@
[info] Log at : 2025/1/13|22:53:23|GMT+0800
[info] Log at : 2025/3/28|21:46:16|GMT+0800

72
MDK-ARM/R2_CHASSIS.uvoptx
View File

@ -135,7 +135,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U-O206 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
<Name>-U00260035480000034E575152 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8000 -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>
@ -160,13 +160,63 @@
<WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>out,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>can_out,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
</Tracepoint>
<DebugFlag>
<trace>0</trace>
<periodic>0</periodic>
<periodic>1</periodic>
<aLwin>1</aLwin>
<aCover>0</aCover>
<aSer1>0</aSer1>
@ -247,7 +297,7 @@
<Group>
<GroupName>Application/User/Core</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -659,7 +709,7 @@
<Group>
<GroupName>Drivers/CMSIS</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -679,7 +729,7 @@
<Group>
<GroupName>Middlewares/FreeRTOS</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -839,7 +889,7 @@
<Group>
<GroupName>User/bsp</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1131,7 +1181,7 @@
<Group>
<GroupName>User/Algorithm</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1211,7 +1261,7 @@
<Group>
<GroupName>User/Device</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1303,7 +1353,7 @@
<Group>
<GroupName>Application/User/USB_DEVICE/App</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1347,7 +1397,7 @@
<Group>
<GroupName>Application/User/USB_DEVICE/Target</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1367,7 +1417,7 @@
<Group>
<GroupName>Middlewares/USB_Device_Library</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>

3
MDK-ARM/R2_CHASSIS.uvprojx
View File

@ -10,6 +10,7 @@
<TargetName>R2_CHASSIS</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<pArmCC>5060960::V5.06 update 7 (build 960)::.\ARMCC</pArmCC>
<pCCUsed>5060960::V5.06 update 7 (build 960)::.\ARMCC</pCCUsed>
<uAC6>0</uAC6>
<TargetOption>
@ -81,7 +82,7 @@
</BeforeMake>
<AfterMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>1</RunUserProg2>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>

BIN
MDK-ARM/R2_CHASSIS/R2_CHASSIS.axf Normal file
View File

Binary file not shown.

9
User/Algorithm/user_math.c
View File

@ -309,3 +309,12 @@ uint8_t average(uint8_t arr[], uint8_t n) {
}
return (float) sum / n;
}
int abs_value(int num) {
if (num >= 0) {
return num; // 如果数是非负的，直接返回该数
} else {
return -num; // 如果数是负的，返回它的相反数
}
}

3
User/Algorithm/user_math.h
View File

@ -157,4 +157,7 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2);
/// @param n 元素数
/// @return 平均值
uint8_t average(uint8_t arr[], uint8_t n);
int abs_value(int num);
#endif

38
User/Module/Chassis.c
View File

@ -71,23 +71,11 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param)); //带D项滤波
}
PID_init((&c->pid.chassis_pitAngle6020),PID_POSITION,&(c->param->C6020pitAngle_param));//尝试位置控制角度
PID_init((&c->pid.chassis_pitOmega6020),PID_POSITION,&(c->param->C6020pitOmega_param));
PID_init(&(c->pid.chassis_PICKWzPID_IN),PID_POSITION,&(c->param->chassis_PICKWzPIDIN_param));
PID_init((&c->pid.chassis_gimbal_yawAnglePID),PID_POSITION,&(c->param->Gimbal_yawAngle_param));//尝试位置控制角度
PID_init(&(c->pid.chassis_PICKWzPID_OUT),PID_POSITION,&(c->param->chassis_PICKWzPIDOUT_param));
PID_init((&c->pid.chassis_gimbal_yawOmegaPID),PID_POSITION,&(c->param->Gimbal_yawOmega_param));
PID_init((&c->pid.chassis_gimbal_pitchAnglePID),PID_POSITION,&(c->param->Gimbal_pitchAngle_param));//尝试位置控制角度
PID_init((&c->pid.chassis_gimbal_pitchOmegaPID),PID_POSITION,&(c->param->Gimbal_pitchOmega_param));
PID_init(&(c->pid.chassis_NaviVxPID),PID_POSITION,&(c->param->NaviVx_param));
PID_init(&(c->pid.chassis_NaviVyPID),PID_POSITION,&(c->param->NaviVy_param));
PID_init(&(c->pid.chassis_NaviWzPID),PID_POSITION,&(c->param->NaviVw_param));
PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
@ -180,10 +168,24 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
break;
case PICK:
c->move_vec.Vw =ctrl->C_navi.wz ;
c->move_vec.Vx =ctrl->C_navi.vy ;
c->move_vec.Vy =ctrl->C_navi.vx ;
case PICK_t:
c->move_vec.Vx =ctrl->Vx*6000 ;
c->move_vec.Vy =ctrl->Vy *6000;
c->move_vec .Vw = ctrl->C_pick .posx ;
// c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
if(abs_value(ctrl ->C_pick .posx )>10)
{
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_IN),-(ctrl ->C_pick .posx) ,0);
}
else if(abs_value(ctrl ->C_pick .posx )<0.2)
{c->move_vec.Vw =0;
}
else
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_OUT),-(ctrl ->C_pick .posx) ,0);

36
User/Module/Chassis.h
View File

@ -82,16 +82,10 @@ typedef struct
/*该部分决定PID的参数整定在config中修改*/
pid_param_t M3508_param;
pid_param_t AngleCor_param;
pid_param_t OmegaCor_param;
pid_param_t DisCamera_param;
pid_param_t ImuCor_param;
pid_param_t C6020pitAngle_param;
pid_param_t C6020pitOmega_param;
pid_param_t Gimbal_yawAngle_param;
pid_param_t Gimbal_yawOmega_param;
pid_param_t Gimbal_pitchAngle_param;
pid_param_t Gimbal_pitchOmega_param;
pid_param_t chassis_PICKWzPIDIN_param;
pid_param_t chassis_PICKWzPIDOUT_param;
pid_param_t NaviVx_param;
pid_param_t NaviVy_param;
pid_param_t NaviVw_param;
@ -166,21 +160,19 @@ typedef struct{
struct{
pid_type_def chassis_3508VecPID[4];
pid_type_def chassis_pitAngle6020;
pid_type_def chassis_pitOmega6020;
pid_type_def chassis_gimbal_yawAnglePID;
pid_type_def chassis_gimbal_yawOmegaPID;
pid_type_def chassis_gimbal_pitchAnglePID;
pid_type_def chassis_gimbal_pitchOmegaPID;
pid_type_def chassis_NaviVxPID;
pid_type_def chassis_NaviVyPID;
pid_type_def chassis_NaviWzPID;
pid_type_def chassis_PICKWzPID_IN;
pid_type_def chassis_PICKWzPID_OUT;
pid_type_def chassis_NaviWzPID;
pid_type_def chassis_NaviVxPID;
pid_type_def chassis_NaviVyPID;
pid_type_def sick_CaliforYPID;
pid_type_def sick_CaliforXPID;
pid_type_def Action_VxPID;
pid_type_def Action_VyPID;
pid_type_def Action_WzPID;
// pid_type_def Action_VxPID;
// pid_type_def Action_VyPID;
// pid_type_def Action_WzPID;
}pid;
fp32 vofa_send[8];

105
User/Module/config.c
View File

@ -8,21 +8,6 @@
#define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
/*导航地图*/
#define point_num 4
point_t map_point[point_num] ={
{0.0, 0, DEG_TO_RAD(90.0), 5.0},
{0, 0, DEG_TO_RAD(180.0), 5.0},
//
{0.0, 0.0, DEG_TO_RAD(270.0), 5.0},
{0.0, 0.0, DEG_TO_RAD(360.0), 5.0},
};
#ifdef DEBUG
@ -36,83 +21,7 @@ static const ConfigParam_t param_chassis ={
#endif
.chassis = {/**/
.C6020pitAngle_param = {
.p = 15.0f,
.i = 0.3f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f,
},
.C6020pitOmega_param = {
.p =30.0f,
.i =0.3f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.Gimbal_yawAngle_param = {
.p =8.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.Gimbal_yawOmega_param = {
.p =18.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.Gimbal_pitchAngle_param = {
.p =8.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.Gimbal_pitchOmega_param = {
.p =18.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.AngleCor_param = {
.p =0.8f,
.i =0.0f,
.d =1.0f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.OmegaCor_param = {
.p =23.5f,
.i =0.0f,
.d =0.05f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.ImuCor_param = {
.p =95.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 0.0f,
.out_limit =200.0f,
},
.DisCamera_param = {
.p =80.0f,
.i =0.1f,
.d =0.0f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.M3508_param = {
.p = 15.1f,
@ -121,6 +30,20 @@ static const ConfigParam_t param_chassis ={
.i_limit = 200.0f,
.out_limit =6000.0f,
},
.chassis_PICKWzPIDIN_param ={
.p = 5.0f,
.i = 0.0f,
.d = 0.15f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.chassis_PICKWzPIDOUT_param ={
.p = 10.0f,
.i = 0.02f,
.d = 0.00f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
// .M3508_param = {
// .p = 10.0f,
// .i = 0.0f,

2
User/device/cmd.c
View File

@ -187,7 +187,7 @@ int8_t CMD_CtrlSet(CMD_t *cmd) {
{
if(cmd->key_ctrl_r==CMD_SW_UP)
{
cmd->C_cmd.type = RC;
cmd->C_cmd.type = PICK_t;
cmd->C_cmd.mode = RC_MODE3;
}
if(cmd->key_ctrl_r==CMD_SW_MID)

4
User/device/nuc.c
View File

@ -6,7 +6,7 @@ static volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
uint8_t nucbuf[32];
uint8_t nucbuf[31];
static void NUC_IdleCallback(void) {
osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
@ -54,7 +54,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
case MID://控制帧0x09
/* 协议格式
0xFF HEAD
0x0X 控制帧
0x09 控制帧
0x01 相机帧
vx fp32
vy fp32