diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index a71f241..56ffa23 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -103,7 +103,7 @@
1
0
0
- 6
+ 3
@@ -114,7 +114,7 @@
- STLink\ST-LINKIII-KEIL_SWO.dll
+ BIN\CMSIS_AGDI.dll
@@ -140,7 +140,7 @@
0
DLGUARM
- (105=-1,-1,-1,-1,0)
+
0
@@ -158,27 +158,27 @@
0
1
- rc_ctrl,0x0A
+ Nor_Vx
1
1
- LD_raw,0x0A
+ Nor_Vy
2
1
- can,0x0A
+ count,0x0A
3
1
- UP,0x0A
+ count
4
1
- cmd_rc,0x0A
+ NUC_StartSending,0x0A
5
@@ -188,127 +188,32 @@
6
1
- nuc_raw,0x0A
+ PIAN
7
1
- nucbuf
+ shang_yaw
8
1
- cmd_fromnuc
+ pian_yaw
9
1
- up_cmd,0x0A
+ pid,0x0A
10
1
- can_out
+ gyro_kp
11
1
- Nor_Vx
-
-
- 12
- 1
- Nor_Vy
-
-
- 13
- 1
- b
-
-
- 14
- 1
- count,0x0A
-
-
- 15
- 1
- count
-
-
- 16
- 1
- raw_rx1,0x0A
-
-
- 17
- 1
- gyro_kp,0x0A
-
-
- 18
- 1
- nucbuf,0x0A
-
-
- 19
- 1
- vofa_send,0x0A
-
-
- 20
- 1
- SendBuffer,0x10
-
-
- 21
- 1
- NUC_StartSending,0x0A
-
-
- 22
- 1
- PIAN
-
-
- 23
- 1
- pid
-
-
- 24
- 1
- bmi088
-
-
- 25
- 1
- ist8310
-
-
- 26
- 1
- imu_eulr,0x0A
-
-
- 27
- 1
- imu_temp,0x0A
-
-
- 28
- 1
- htim10,0x0A
-
-
- 29
- 1
- pulse
-
-
- 30
- 1
- imu_temp
+ rc_ctrl,0x0A
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index 60f8a97..6de7a1a 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index d1ea42d..434ac0f 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -83,6 +83,9 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
fp32 gyro_kp=1.0f;
fp32 PIAN=0;
+fp32 shang_yaw=0;
+fp32 pian_yaw=0;
+
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算,
{
fp64 Nor_Vx,Nor_Vy;//归一化后的数据
@@ -92,23 +95,41 @@ void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆
// c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
// c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
// c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
-// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
-// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
-// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
-// c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左前
- c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw ;//右前
+ if(Vw)
+ {
+ c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw ;//右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw ;//右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw ;//左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw ;//左前
-
-// if(!Vw){
-// PIAN = jiuzheng(c->pos088 .imu_eulr .yaw );
+ }
+ else {
+ c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z*gyro_kp;//右前
+ c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z*gyro_kp;//右后
+ c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z*gyro_kp;//左后
+ c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z*gyro_kp;//左前
+ }
+//
+// if(Vx || Vy){
+// PIAN = jiuzheng(c->pos088 .imu_eulr .yaw );
+// c->hopemotorout.OmniSpeedOut[0] -= PIAN ;//右前
+// c->hopemotorout.OmniSpeedOut[1] -= PIAN ;//右后
+// c->hopemotorout.OmniSpeedOut[2] -= PIAN ;//左后
+// c->hopemotorout.OmniSpeedOut[3] -= PIAN ;//左前
+// }
+// else
+// {
+// PIAN = 0;
+// shang_yaw=c->pos088 .imu_eulr .yaw ;
+// pian_yaw=0;
// }
-// c->hopemotorout.OmniSpeedOut[0] += PIAN ;//右前
-// c->hopemotorout.OmniSpeedOut[1] += PIAN ;//右后
-// c->hopemotorout.OmniSpeedOut[2] += PIAN ;//左后
-// c->hopemotorout.OmniSpeedOut[3] += PIAN ;//左前
+//
+ c->vofa_send[0]=c->pos088.bmi088.gyro.z;
+
+ c->vofa_send[1]=PIAN;
+ c->vofa_send[2]=c->pos088.imu_eulr.yaw;
+// c->vofa_send[3]=
+// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [1];
}
@@ -278,8 +299,6 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
};
fp32 jiuzheng(fp32 yaw)
{
- static fp32 pian_yaw=0;
- static fp32 shang_yaw=0;
static int is=0;
@@ -289,9 +308,9 @@ fp32 jiuzheng(fp32 yaw)
is=1;
}
- pian_yaw+= (yaw - shang_yaw);
+ pian_yaw+= (yaw - shang_yaw)*1000;
shang_yaw=yaw ;
- return PID_calc(&pid,pian_yaw,0);
+ return PID_calc (&pid,pian_yaw,0);
}
diff --git a/User/Module/config.c b/User/Module/config.c
index 65a65cd..42b4668 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -1,4 +1,4 @@
-#include "config.h"
+ #include "config.h"
#include "flash.h"
#include "string.h"
#define DEBUG
diff --git a/User/device/rc.c b/User/device/rc.c
index bebd452..c69c6b2 100644
--- a/User/device/rc.c
+++ b/User/device/rc.c
@@ -5,15 +5,15 @@
/* Includes ----------------------------------------------------------------- */
#include "rc.h"
-
+#include "cmd.h"
#include
#include "bsp_usart.h"
#include "error_detect.h"
-//#define DR16
-#define LD_t
+#define DR16
+//#define LD_t
#ifdef DR16
#define FRAME_LEN 36
@@ -100,15 +100,15 @@ int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
if (raw->ch[0] < 0)
- raw->map_ch[0] = map_fp32(raw->ch[0], -696, 2, -1, 0);
+ raw->map_ch[0] = map_fp32(raw->ch[0], -700, -2, -1, 0);
else
- raw->map_ch[0] = map_fp32(raw->ch[0], 2, 704, 0, 1);
+ raw->map_ch[0] = map_fp32(raw->ch[0], -2, 700, 0, 1);
// ch[1]
if (raw->ch[1] < 0)
- raw->map_ch[1] = map_fp32(raw->ch[1], -638, 5, -1, 0);
+ raw->map_ch[1] = map_fp32(raw->ch[1], -700, 4, -1, 0);
else
- raw->map_ch[1] = map_fp32(raw->ch[1], 5, 762, 0, 1);
+ raw->map_ch[1] = map_fp32(raw->ch[1], 4, 700, 0, 1);
raw->map_ch[2] = map_fp32(raw->ch[2], 2, 1377, 0, 1);
@@ -220,8 +220,10 @@ int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
rc->dr16.ch_l_x = 2 * ((float)dr16->data.ch_l_x - RC_CH_VALUE_MID) / full_range;
rc->dr16.ch_l_y = 2 * ((float)dr16->data.ch_l_y - RC_CH_VALUE_MID) / full_range;
- rc->dr16.sw_l = (CMD_SwitchPos_DR16_t)dr16->data.sw_l;
- rc->dr16.sw_r = (CMD_SwitchPos_DR16_t)dr16->data.sw_r;
+// rc->dr16.sw_l = (CMD_SwitchPos_DR16_t)dr16->data.sw_l;
+// rc->dr16.sw_r = (CMD_SwitchPos_DR16_t)dr16->data.sw_r;
+ rc->dr16.sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
+ rc->dr16.sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;
rc->dr16.key = dr16->data.key;
diff --git a/User/device/vofa.c b/User/device/vofa.c
index d1f8b7f..026144b 100644
--- a/User/device/vofa.c
+++ b/User/device/vofa.c
@@ -28,13 +28,11 @@ void vofa_tx_main(float *data)
/*在下面使用对应的串口发送函数*/
-// CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
-// osDelay(1);
-// CDC_Transmit_FS( tail, 4);
-// HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
-// osDelay(1);
-// HAL_UART_Transmit_DMA(&huart1, tail, 4);
-// osDelay(1);
+
+ HAL_UART_Transmit_DMA(&huart6, ( uint8_t *)fdata, sizeof(fdata));
+ osDelay(1);
+ HAL_UART_Transmit_DMA(&huart6, tail, 4);
+ osDelay(1);
}
diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c
index cbe6d3d..03ece18 100644
--- a/User/task/nuc_task.c
+++ b/User/task/nuc_task.c
@@ -29,7 +29,7 @@ void Task_nuc(void *argument){
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
#endif
NUC_StartReceiving();
- NUC_StartSending(send);
+// NUC_StartSending(send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc);
}