diff --git a/MDK-ARM/R2_CHASSIS.uvoptx b/MDK-ARM/R2_CHASSIS.uvoptx
index d3de66b..fd90927 100644
--- a/MDK-ARM/R2_CHASSIS.uvoptx
+++ b/MDK-ARM/R2_CHASSIS.uvoptx
@@ -103,7 +103,7 @@
1
0
0
- 6
+ 3
@@ -114,9 +114,14 @@
- STLink\ST-LINKIII-KEIL_SWO.dll
+ BIN\CMSIS_AGDI.dll
+
+ 0
+ CMSIS_AGDI
+ -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)
+
0
ARMRTXEVENTFLAGS
@@ -135,12 +140,12 @@
0
ST-LINKIII-KEIL_SWO
- -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)
+ -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)
0
DLGUARM
- (105=-1,-1,-1,-1,0)
+
0
@@ -210,6 +215,11 @@
1
cmd_fromnuc,0x0A
+
+ 12
+ 1
+ a
+
0
@@ -258,7 +268,7 @@
0
0
2
- 10000000
+ 5000000
diff --git a/MDK-ARM/R2_CHASSIS/R2_CHASSIS.axf b/MDK-ARM/R2_CHASSIS/R2_CHASSIS.axf
index 3aea999..0b8b1c2 100644
Binary files a/MDK-ARM/R2_CHASSIS/R2_CHASSIS.axf and b/MDK-ARM/R2_CHASSIS/R2_CHASSIS.axf differ
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index 051c4e8..d14fa45 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -70,11 +70,10 @@ 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,18 +145,24 @@ 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 =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 =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);
+// //电机速度限幅
+
+ abs_limit_fp(&c->move_vec.Vx,2000.0f);
+
+ abs_limit_fp(&c->move_vec.Vy,2000.0f);
+
+ abs_limit_fp(&c->move_vec.Vw,2000.0f);
+
if(ctrl->status[5] ==1)
{
c->move_vec.Vw = c->move_vec.Vw * 0.8f;
@@ -172,21 +177,30 @@ 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;
+
+
-
-
break ;
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index 7f4f6c8..d5131e3 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -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];
diff --git a/User/Module/config.c b/User/Module/config.c
index 8c82fd0..fa243e0 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -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,
diff --git a/User/bsp/bsp_usart.c b/User/bsp/bsp_usart.c
index b22edd5..369a7d7 100644
--- a/User/bsp/bsp_usart.c
+++ b/User/bsp/bsp_usart.c
@@ -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;
diff --git a/User/bsp/bsp_usart.h b/User/bsp/bsp_usart.h
index ac2af10..cc8d13f 100644
--- a/User/bsp/bsp_usart.h
+++ b/User/bsp/bsp_usart.h
@@ -12,7 +12,7 @@
typedef enum {
BSP_UART_REMOTE,
- BSP_UART_ACTION,
+ BSP_UART_VOFA,
BSP_UART_NUC,
/* BSP_UART_XXX, */
BSP_UART_NUM,
diff --git a/User/device/cmd.c b/User/device/cmd.c
index 76b1653..39938b1 100644
--- a/User/device/cmd.c
+++ b/User/device/cmd.c
@@ -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;
diff --git a/User/device/vofa.c b/User/device/vofa.c
index 87da64f..0d584cc 100644
--- a/User/device/vofa.c
+++ b/User/device/vofa.c
@@ -1,5 +1,5 @@
#include "vofa.h"
-//#include "usart.h"
+#include "bsp_usart.h"
#include "usbd_cdc_if.h"
#include
//用来对需要的数据进行串口绘图 （未添加接收函数）
@@ -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));
- osDelay(1);
- CDC_Transmit_FS( 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);
+// CDC_Transmit_FS( tail, 4);
}