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> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel> <nTsel>3</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@ -114,9 +114,14 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon> <pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <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> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key> <Key>ARMRTXEVENTFLAGS</Key>
@ -135,12 +140,12 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key> <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>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name>(105=-1,-1,-1,-1,0)</Name> <Name></Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -210,6 +215,11 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText> <ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>a</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -258,7 +268,7 @@
<EnableFlashSeq>0</EnableFlashSeq> <EnableFlashSeq>0</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>10000000</DbgClock> <DbgClock>5000000</DbgClock>
</DebugDescription> </DebugDescription>
</TargetOption> </TargetOption>
</Target> </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_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_LOW),PID_POSITION,&(c->param->chassis_PICKWzPID_LOW_param));
PID_init(&(c->pid.chassis_PICKWzPID_OUT),PID_POSITION,&(c->param->chassis_PICKWzPIDOUT_param));
PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam)); 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; // if(xArrive && yArrive) return true;
// else return false; // else return false;
//} //}
fp32 a,b,c,d;
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) 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: //雷达 case MID_NAVI: //雷达
// //这套是全向轮的方向,一定要注意这里的xy方向 // //这套是全向轮的方向,一定要注意这里的xy方向
c->move_vec.Vw =ctrl->C_navi.wz ; c->move_vec.Vw =ctrl->C_navi.wz*1000 ;
c->move_vec.Vx =ctrl->C_navi.vy ; c->move_vec.Vy =-ctrl->C_navi.vy*1000 ;
c->move_vec.Vy =ctrl->C_navi.vx ; 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); abs_limit_fp(&c->move_vec.Vx,2000.0f);
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); abs_limit_fp(&c->move_vec.Vy,2000.0f);
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.Vw,2000.0f);
if(ctrl->status[5] ==1) 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.Vx =ctrl->Vx*6000 ;
c->move_vec.Vy =ctrl->Vy *6000; c->move_vec.Vy =ctrl->Vy *6000;
c->move_vec .Vw = ctrl->C_pick .posx ; 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)
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) else if(abs_value(ctrl ->C_pick .posx )<0.1)
{c->move_vec.Vw =0; {
c->move_vec.Vw =0;
} }
else 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的参数整定在config中修改*/
pid_param_t M3508_param; pid_param_t M3508_param;
pid_param_t chassis_PICKWzPIDIN_param; pid_param_t chassis_PICKWzPID_HIGN_param;
pid_param_t chassis_PICKWzPIDOUT_param; pid_param_t chassis_PICKWzPID_LOW_param;
pid_param_t NaviVx_param; pid_param_t NaviVx_param;
pid_param_t NaviVy_param; pid_param_t NaviVy_param;
@ -160,8 +160,13 @@ typedef struct{
struct{ struct{
pid_type_def chassis_3508VecPID[4]; 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_NaviWzPID;
pid_type_def chassis_NaviVxPID; pid_type_def chassis_NaviVxPID;
@ -170,9 +175,6 @@ typedef struct{
pid_type_def sick_CaliforYPID; pid_type_def sick_CaliforYPID;
pid_type_def sick_CaliforXPID; pid_type_def sick_CaliforXPID;
// pid_type_def Action_VxPID;
// pid_type_def Action_VyPID;
// pid_type_def Action_WzPID;
}pid; }pid;
fp32 vofa_send[8]; 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, .i_limit = 200.0f,
.out_limit =6000.0f, .out_limit =6000.0f,
}, },
.chassis_PICKWzPIDIN_param ={ /*视觉*/
.p = 5.0f, .chassis_PICKWzPID_HIGN_param ={ //高响应
.i = 0.0f, .p = 1.0f,
.d = 0.15f, .i = 0.03f,
.i_limit = 0.0f, .d = 0.03f,
.out_limit =5000.0f, .i_limit = 100.0f,
.out_limit =2000.0f,
}, },
.chassis_PICKWzPIDOUT_param ={ .chassis_PICKWzPID_LOW_param ={ //高精度
.p = 10.0f, .p = 0.5f, //1.0 0.5
.i = 0.02f, .i = 0.5f, //0.01 0.04
.d = 0.00f, .d = 0.0f, //0.02 0.02
.i_limit = 0.0f, .i_limit = 50.0f,
.out_limit =5000.0f, .out_limit =1000.0f,
}, },
// .M3508_param = { // .M3508_param = {
// .p = 10.0f, // .p = 10.0f,
// .i = 0.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 ={ .NaviVx_param ={
.p = 1.04f, .p = 1.15f,
.i = 0.0f, .i = 0.0f,
.d = 0.15f, .d = 0.15f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit =5000.0f, .out_limit =5000.0f,
}, },
.NaviVy_param ={ .NaviVy_param ={
.p = 1.1f, .p = 1.15f,
.i = 0.0f, .i = 0.0f,
.d = 0.15f, .d = 0.15f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit =5000.0f, .out_limit =5000.0f,
}, },
.NaviVw_param ={ .NaviVw_param ={
.p = 1.5f, .p = 1.15f,
.i = 0.0f, .i = 0.0f,
.d = 0.15f, .d = 0.15f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit =5000.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 ={ .Sick_CaliYparam ={
.p =2.5f, .p =2.5f,
.i =0.001f, .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) if (huart->Instance == USART3)
return BSP_UART_REMOTE; return BSP_UART_REMOTE;
else if (huart->Instance == USART1) else if (huart->Instance == USART1)
return BSP_UART_NUC; return BSP_UART_VOFA;
else if (huart->Instance == USART6) else if (huart->Instance == USART6)
return BSP_UART_NUC; return BSP_UART_NUC;
/* /*
@ -94,7 +94,7 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
switch (uart) { switch (uart) {
case BSP_UART_REMOTE: case BSP_UART_REMOTE:
return &huart3; return &huart3;
case BSP_UART_ACTION: case BSP_UART_VOFA:
return &huart1; return &huart1;
case BSP_UART_NUC: case BSP_UART_NUC:
return &huart6; return &huart6;

2
User/bsp/bsp_usart.h
View File

@ -12,7 +12,7 @@
typedef enum { typedef enum {
BSP_UART_REMOTE, BSP_UART_REMOTE,
BSP_UART_ACTION, BSP_UART_VOFA,
BSP_UART_NUC, BSP_UART_NUC,
/* BSP_UART_XXX, */ /* BSP_UART_XXX, */
BSP_UART_NUM, 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->Vx = rc->ch_r_x;
cmd->Vy = -rc->ch_r_y; cmd->Vy = rc->ch_r_y;
cmd->Vw = rc->ch_l_x; cmd->Vw = rc->ch_l_x;
cmd->poscamear = rc->ch_l_y; cmd->poscamear = rc->ch_l_y;

10
User/device/vofa.c
View File

@ -1,5 +1,5 @@
#include "vofa.h" #include "vofa.h"
//#include "usart.h" #include "bsp_usart.h"
#include "usbd_cdc_if.h" #include "usbd_cdc_if.h"
#include <cmsis_os2.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)); // HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1); // osDelay(1);
// HAL_UART_Transmit_DMA(&huart1, tail, 4); HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_VOFA), tail, 4);
// osDelay(1);
CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
osDelay(1); osDelay(1);
CDC_Transmit_FS( tail, 4); // CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1);
// CDC_Transmit_FS( tail, 4);
} }