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

雷达已测，vofa待改，nuc串口数据，因为要把视觉和雷达一块发送，可能还需要改

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-05 22:14:27 +08:00
parent b9f733f116
commit 123d6afefb
9 changed files with 116 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
MDK-ARM/R2_CHASSIS.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel>
<nTsel>3</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@ -114,9 +114,14 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O718 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key>
@ -135,12 +140,12 @@
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<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>
<Name>-U00260035480000034E575152 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -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>
<Key>DLGUARM</Key>
<Name>(105=-1,-1,-1,-1,0)</Name>
<Name></Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
@ -210,6 +215,11 @@
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>a</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
@ -258,7 +268,7 @@
<EnableFlashSeq>0</EnableFlashSeq>
<EnableLog>0</EnableLog>
<Protocol>2</Protocol>
<DbgClock>10000000</DbgClock>
<DbgClock>5000000</DbgClock>
</DebugDescription>
</TargetOption>
</Target>

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

Binary file not shown.

54
User/Module/Chassis.c
View File

@ -71,10 +71,9 @@ 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_PICKWzPID_HIGN),PID_POSITION,&(c->param->chassis_PICKWzPID_HIGN_param));
PID_init(&(c->pid.chassis_PICKWzPID_IN),PID_POSITION,&(c->param->chassis_PICKWzPIDIN_param));
PID_init(&(c->pid.chassis_PICKWzPID_OUT),PID_POSITION,&(c->param->chassis_PICKWzPIDOUT_param));
PID_init(&(c->pid.chassis_PICKWzPID_LOW),PID_POSITION,&(c->param->chassis_PICKWzPID_LOW_param));
PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
@ -113,7 +112,7 @@ void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆
// if(xArrive && yArrive) return true;
// else return false;
//}
fp32 a,b,c,d;
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
{
@ -146,17 +145,23 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
case MID_NAVI: //雷达
// //这套是全向轮的方向,一定要注意这里的xy方向
c->move_vec.Vw =ctrl->C_navi.wz ;
c->move_vec.Vx =ctrl->C_navi.vy ;
c->move_vec.Vy =ctrl->C_navi.vx ;
c->move_vec.Vw =ctrl->C_navi.wz*1000 ;
c->move_vec.Vy =-ctrl->C_navi.vy*1000 ;
c->move_vec.Vx =-ctrl->C_navi.vx*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);
// //电机速度限幅
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);
abs_limit_fp(&c->move_vec.Vx,2000.0f);
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);
abs_limit_fp(&c->move_vec.Vy,2000.0f);
abs_limit_fp(&c->move_vec.Vw,2000.0f);
if(ctrl->status[5] ==1)
{
@ -172,18 +177,27 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
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 = -ctrl->C_pick .posx;
if(abs_value(ctrl ->C_pick .posx )>20)
{
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_IN),-(ctrl ->C_pick .posx) ,0);
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0);
}
else if(abs_value(ctrl ->C_pick .posx )<0.2)
{c->move_vec.Vw =0;
else if(abs_value(ctrl ->C_pick .posx )<0.1)
{
c->move_vec.Vw =0;
}
else
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_OUT),-(ctrl ->C_pick .posx) ,0);
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->C_pick .posx;

16
User/Module/Chassis.h
View File

@ -83,8 +83,8 @@ typedef struct
/*该部分决定PID的参数整定在config中修改*/
pid_param_t M3508_param;
pid_param_t chassis_PICKWzPIDIN_param;
pid_param_t chassis_PICKWzPIDOUT_param;
pid_param_t chassis_PICKWzPID_HIGN_param;
pid_param_t chassis_PICKWzPID_LOW_param;
pid_param_t NaviVx_param;
pid_param_t NaviVy_param;
@ -160,8 +160,13 @@ typedef struct{
struct{
pid_type_def chassis_3508VecPID[4];
pid_type_def chassis_PICKWzPID_IN;
pid_type_def chassis_PICKWzPID_OUT;
/*视觉*/
/*存在较高误差*/
pid_type_def chassis_PICKWzPID_HIGN;
/*存在较低误差*/
pid_type_def chassis_PICKWzPID_LOW;
pid_type_def chassis_NaviWzPID;
pid_type_def chassis_NaviVxPID;
@ -170,9 +175,6 @@ typedef struct{
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;
fp32 vofa_send[8];

81
User/Module/config.c
View File

@ -30,20 +30,23 @@ 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_PICKWzPID_HIGN_param ={ //高响应
.p = 1.0f,
.i = 0.03f,
.d = 0.03f,
.i_limit = 100.0f,
.out_limit =2000.0f,
},
.chassis_PICKWzPIDOUT_param ={
.p = 10.0f,
.i = 0.02f,
.d = 0.00f,
.i_limit = 0.0f,
.out_limit =5000.0f,
.chassis_PICKWzPID_LOW_param ={ //高精度
.p = 0.5f, //1.0 0.5
.i = 0.5f, //0.01 0.04
.d = 0.0f, //0.02 0.02
.i_limit = 50.0f,
.out_limit =1000.0f,
},
// .M3508_param = {
// .p = 10.0f,
// .i = 0.0f,
@ -53,51 +56,51 @@ static const ConfigParam_t param_chassis ={
// },
////高速那一套
// .NaviVx_param ={
// .p = 1.15f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
// .NaviVy_param ={
// .p = 1.15f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
// .NaviVw_param ={
// .p = 1.15f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
//
//低速那一套
.NaviVx_param ={
.p = 1.04f,
.p = 1.15f,
.i = 0.0f,
.d = 0.15f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.NaviVy_param ={
.p = 1.1f,
.p = 1.15f,
.i = 0.0f,
.d = 0.15f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.NaviVw_param ={
.p = 1.5f,
.p = 1.15f,
.i = 0.0f,
.d = 0.15f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
//
//低速那一套
// .NaviVx_param ={
// .p = 1.04f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
// .NaviVy_param ={
// .p = 1.1f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
// .NaviVw_param ={
// .p = 1.5f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
.Sick_CaliYparam ={
.p =2.5f,
.i =0.001f,

4
User/bsp/bsp_usart.c
View File

@ -9,7 +9,7 @@ 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;
return BSP_UART_VOFA;
else if (huart->Instance == USART6)
return BSP_UART_NUC;
/*
@ -94,7 +94,7 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
switch (uart) {
case BSP_UART_REMOTE:
return &huart3;
case BSP_UART_ACTION:
case BSP_UART_VOFA:
return &huart1;
case BSP_UART_NUC:
return &huart6;

2
User/bsp/bsp_usart.h
View File

@ -12,7 +12,7 @@
typedef enum {
BSP_UART_REMOTE,
BSP_UART_ACTION,
BSP_UART_VOFA,
BSP_UART_NUC,
/* BSP_UART_XXX, */
BSP_UART_NUM,

2
User/device/cmd.c
View File

@ -63,7 +63,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
cmd->Vx = rc->ch_r_x;
cmd->Vy = -rc->ch_r_y;
cmd->Vy = rc->ch_r_y;
cmd->Vw = rc->ch_l_x;
cmd->poscamear = rc->ch_l_y;

10
User/device/vofa.c
View File

@ -1,5 +1,5 @@
#include "vofa.h"
//#include "usart.h"
#include "bsp_usart.h"
#include "usbd_cdc_if.h"
#include <cmsis_os2.h>
//用来对需要的数据进行串口绘图 （未添加接收函数）
@ -30,11 +30,11 @@ void vofa_tx_main(float *data)
/*在下面使用对应的串口发送函数*/
// HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1);
// HAL_UART_Transmit_DMA(&huart1, tail, 4);
// osDelay(1);
CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_VOFA), tail, 4);
osDelay(1);
CDC_Transmit_FS( tail, 4);
// CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1);
// CDC_Transmit_FS( tail, 4);
}