diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log
index 4555934..19d8611 100644
--- a/MDK-ARM/.vscode/keil-assistant.log
+++ b/MDK-ARM/.vscode/keil-assistant.log
@@ -463,3 +463,7 @@
[info] Log at : 2025/6/14|15:14:16|GMT+0800
+[info] Log at : 2025/6/16|23:35:32|GMT+0800
+
+[info] Log at : 2025/6/18|17:47:04|GMT+0800
+
diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index b1b816f..8aacd56 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -175,6 +175,86 @@
1
NUC_send,0x0A
+
+ 4
+ 1
+ pid,0x0A
+
+
+ 5
+ 1
+ pid_param,0x0A
+
+
+ 6
+ 1
+ cmd_rc,0x0A
+
+
+ 7
+ 1
+ cbuf,0x10
+
+
+ 8
+ 1
+ cmd
+
+
+ 9
+ 1
+ rc_ctrl,0x0A
+
+
+ 10
+ 1
+ valid_check,0x0A
+
+
+ 11
+ 1
+ nuc_raw,0x0A
+
+
+ 12
+ 1
+ cmd_fromnuc,0x0A
+
+
+ 13
+ 1
+ nucbuf,0x0A
+
+
+ 14
+ 1
+ LD_raw,0x0A
+
+
+ 15
+ 1
+ aeeaa,0x0A
+
+
+ 16
+ 1
+ aaaaaaaaaaaa,0x0A
+
+
+ 17
+ 1
+ valid_check,0x0A
+
+
+ 18
+ 1
+ aaaaaaaaaaaaaaaaaaaaaaaaaa,0x0A
+
+
+ 19
+ 1
+ aaaaaaaaaaaaaaaaaaa,0x0A
+
0
@@ -182,7 +262,7 @@
0
1
- 1
+ 0
0
0
0
@@ -1181,8 +1261,8 @@
0
0
0
- ..\User\Algorithm\kalman.c
- kalman.c
+ ..\User\Algorithm\user_cmsis_os2.c
+ user_cmsis_os2.c
0
0
@@ -1193,8 +1273,8 @@
0
0
0
- ..\User\Algorithm\user_cmsis_os2.c
- user_cmsis_os2.c
+ ..\User\Algorithm\kalman.c
+ kalman.c
0
0
diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx
index 2e3293a..42309e5 100644
--- a/MDK-ARM/R2.uvprojx
+++ b/MDK-ARM/R2.uvprojx
@@ -1248,16 +1248,16 @@
1
..\User\Algorithm\ahrs.c
-
- kalman.c
- 1
- ..\User\Algorithm\kalman.c
-
user_cmsis_os2.c
1
..\User\Algorithm\user_cmsis_os2.c
+
+ kalman.c
+ 1
+ ..\User\Algorithm\kalman.c
+
user_math.c
1
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index 45fd09e..7fb4434 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 1924905..529a6e1 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -62,22 +62,24 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
fp32 gyro_kp = 1.0f;
fp32 PIAN = 0;
-
+fp32 pian_yaw;
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
- 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; // 左前
+ c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前
+ c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后
+ c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后
+ c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +pian_yaw; // 左前
+
+ Chassis_AngleCompensate(c);
+
}
int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL;
Chassis_SetCtrl(c, ctrl);
-
// IMU加滤波修正
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
@@ -92,12 +94,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
case RCcontrol:
switch (c->chassis_ctrl.mode) {
case Normal:
- c->move_vec.Vw = ctrl->Vw * 6000;
- c->move_vec.Vx = ctrl->Vy * 6000;
- c->move_vec.Vy = ctrl->Vx * 6000;
+ c->move_vec.Vw = ctrl->Vw * 8000;
+ c->move_vec.Vx = ctrl->Vy * 8000;
+ c->move_vec.Vy = ctrl->Vx * 8000;
break;
case Pitch:
-
+ c->move_vec.Vw = ctrl->Vw * 6000;
+ c->move_vec.Vx = ctrl->Vy * 6000;
+ c->move_vec.Vy = ctrl->Vx * 6000;
break;
case UP_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
@@ -159,6 +163,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
}
c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
+
return CHASSIS_OK;
@@ -214,5 +219,36 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
}
+pid_type_def pid;
+pid_param_t pid_param={
+ .p = 10.0f,
+ .i = 0.00f,
+ .d = 0.00f,
+ .i_limit = 2000.0f,
+ .out_limit =5000.0f,
+};
+ fp32 cur_angle,pian_angel;
+int8_t Chassis_AngleCompensate(Chassis_t *c)
+{
+ if (c == NULL) return CHASSIS_ERR_NULL;
+// int is_in;//是否初始化
+// if(is_in==0){
+ PID_init(&pid,PID_POSITION,&pid_param);
+// is_in=1;
+// }
+
+
+
+ if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
+ {
+ pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
+ }
+
+ else {
+ cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
+ pian_angel=0;
+ }
+ pian_yaw = PID_calc(&pid,pian_angel,0);
+}
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index dd09723..e2e2e4a 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -189,6 +189,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) ;
+int8_t Chassis_AngleCompensate(Chassis_t *c) ;
#endif
diff --git a/User/Module/config.c b/User/Module/config.c
index 134e59f..54bbfa0 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -110,7 +110,7 @@ static const ConfigParam_t param ={
},
/*投球*/
.PitchCfg = {
- .m2006_init =-100, //释放的角度
+ .m2006_init =-150, //释放的角度
.m2006_trig =0, //可以拉住发射的角度
.go_init = -50, //仅用在go上电，初始位置
.go_pull_pos =-210,
diff --git a/User/device/can_use.c b/User/device/can_use.c
index 0f2730c..eb86332 100644
--- a/User/device/can_use.c
+++ b/User/device/can_use.c
@@ -374,9 +374,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index);
break;
- case CAN_SICK_ID:
- CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
- break;
+
default:
break;
@@ -407,7 +405,9 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data);
break;
-
+ case CAN_SICK_ID:
+ CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
+ break;
case CAN_Encoder_ID:
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
can->recive_flag |= 1 << 10;
diff --git a/User/device/cmd.c b/User/device/cmd.c
index f5fa1bf..7b9d47c 100644
--- a/User/device/cmd.c
+++ b/User/device/cmd.c
@@ -146,18 +146,20 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
- if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，手动调整
+ if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust; //左上，右上，手动调整
+
+ if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust;
}
else if(rc->dr16.sw_l==CMD_SW_MID)
{
cmd ->CMD_CtrlType =AUTO;
- if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360; //左中，右中，雷达
+ if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Pitch; //左中，右中，雷达
- if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
+ if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =AUTO_MID360; //左中，右中，无模式
- if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_MID360_Pitch; //左中，右下，视觉
+ if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉
}
else if(rc->dr16.sw_l==CMD_SW_DOWN)
diff --git a/User/device/device.h b/User/device/device.h
index 86a547a..897ba4f 100644
--- a/User/device/device.h
+++ b/User/device/device.h
@@ -28,8 +28,7 @@ extern "C" {
#define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
#define SIGNAL_ACTION_RAW_REDY (1u << 10)
#define SIGNAL_OPSTIMER_REDY (1u << 11)
-#define SIGNAL_R12DS_BUF1_REDY (1u << 12)
-#define SIGNAL_DR16_RAW_REDY (1u <<13)
+#define SIGNAL_RC_RAW_REDY (1u <<13)
#define SIGNAL_AI_RAW_REDY (1u << 15)
#define SIGNAL_KEY_REDY (1u << 16)
diff --git a/User/device/nuc.c b/User/device/nuc.c
index e309f3f..a1c723f 100644
--- a/User/device/nuc.c
+++ b/User/device/nuc.c
@@ -6,7 +6,7 @@ static volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
-static uint8_t nucbuf[31];
+ uint8_t nucbuf[31];
static char SendBuffer[19];
diff --git a/User/device/rc.c b/User/device/rc.c
index bebd452..84c5658 100644
--- a/User/device/rc.c
+++ b/User/device/rc.c
@@ -12,8 +12,8 @@
#include "error_detect.h"
-//#define DR16
-#define LD_t
+#define DR16
+//#define LD_t
#ifdef DR16
#define FRAME_LEN 36
@@ -38,14 +38,14 @@ int8_t RC_SBUS_Init(void )
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
- RC_SBUS_RxCpltCallback);
+ RC_RxCpltCallback);
memset(cbuf, 0, sizeof(cbuf));//初始化清空数据包
return DEVICE_OK;
}
-static void RC_SBUS_RxCpltCallback(void) {
- osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
+static void RC_RxCpltCallback(void) {
+ osThreadFlagsSet(thread_alert, SIGNAL_RC_RAW_REDY);
// detect_hook(DR16_TOE);
}
int8_t RC_SBUS_Restart(void) {
@@ -53,7 +53,7 @@ int8_t RC_SBUS_Restart(void) {
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
return DEVICE_OK;
}
-int8_t RC_SBUS_StartDmaRecv(void) {
+int8_t RC_StartDmaRecv(void) {
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_REMOTE),
(uint8_t *)cbuf,
@@ -61,9 +61,9 @@ int8_t RC_SBUS_StartDmaRecv(void) {
return DEVICE_OK;
return DEVICE_ERR;
}
-bool RC_SBUS_WaitDmaCplt(uint32_t timeout) {
- return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
- SIGNAL_DR16_RAW_REDY);
+bool RC_WaitDmaCplt(uint32_t timeout) {
+ return (osThreadFlagsWait(SIGNAL_RC_RAW_REDY, osFlagsWaitAll, timeout) ==
+ SIGNAL_RC_RAW_REDY);
}
/*乐迪数据解析 */
int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
@@ -197,7 +197,7 @@ int8_t DR16_ParseRaw(DR16_t *dr16)
dr16->data.ch_l_y = ((cbuf[4] >>1) | (cbuf[5] <<7));
dr16->data.sw_r = ((cbuf[5] >>4));
dr16->data.sw_l = ((cbuf[5] >> 4) & 0x000C) >> 2;
-
+ dr16->data.res= (fp32)((cbuf[16]|cbuf[17] << 8));
return 1;
}
int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
@@ -220,11 +220,11 @@ 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_t)dr16->data.sw_l;
+ rc->dr16.sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;
rc->dr16.key = dr16->data.key;
-
+ rc->dr16.res = ((float)dr16->data.res - RC_CH_VALUE_MID) ;
// rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;
diff --git a/User/device/rc.h b/User/device/rc.h
index e21aaa5..86adf4b 100644
--- a/User/device/rc.h
+++ b/User/device/rc.h
@@ -11,6 +11,10 @@
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
+
+
+
+
typedef __packed struct
{
uint16_t ch_r_x : 11;
@@ -69,10 +73,10 @@ typedef __packed struct
int8_t RC_SBUS_Init(void );
-static void RC_SBUS_RxCpltCallback(void) ;
-int8_t RC_SBUS_Restart(void) ;
-int8_t RC_SBUS_StartDmaRecv(void) ;
-bool RC_SBUS_WaitDmaCplt(uint32_t timeout) ;
+static void RC_RxCpltCallback(void) ;
+int8_t RC_Restart(void) ;
+int8_t RC_StartDmaRecv(void) ;
+bool RC_WaitDmaCplt(uint32_t timeout) ;
int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD);
static bool DR16_DataCorrupted(const DR16_t *dr16) ;
int8_t DR16_ParseRaw(DR16_t *dr16);
diff --git a/User/task/rc_task.c b/User/task/rc_task.c
index b34b109..15586ef 100644
--- a/User/task/rc_task.c
+++ b/User/task/rc_task.c
@@ -20,7 +20,9 @@ LD_Data_t LD;
#else
static DR16_t dr16;
static CMD_RC_t cmd_rc;
- static RC_ctrl_t cc;
+static LD_raw_t LD_raw;
+static LD_Data_t LD;
+
#endif
@@ -31,10 +33,12 @@ static CMD_RC_t cmd_rc;
* \brief dr16接收机
*
* \param argument 未使用
- */
+ */int aaaaaaaaaaaaaaaaaaa=0;
void Task_rc(void *argument) {
(void)argument; /* 未使用，消除警告 */
+ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC;
+ uint32_t tick = osKernelGetTickCount();
RC_SBUS_Init(); /* 初始化 */
while (1) {
@@ -43,17 +47,26 @@ void Task_rc(void *argument) {
task_runtime.stack_water_mark.rc = osThreadGetStackSpace(osThreadGetId());
#endif
/* 开启DMA */
- RC_SBUS_StartDmaRecv();
-
- if (RC_SBUS_WaitDmaCplt(30)) {
+ RC_StartDmaRecv();
+aaaaaaaaaaaaaaaaaaa++;
+ if (RC_WaitDmaCplt(30)) {
RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);
} else {
- /* 处理dr16遥控器离线 ,乐迪不可用*/
- DR16_HandleOffline(&dr16, &cmd_rc);
- LD_HandleOffline(&LD,&cmd_rc);
+ /* 处理遥控器离线*/
+
+
+
+ DR16_HandleOffline(&dr16, &cmd_rc);
+ LD_HandleOffline(&LD,&cmd_rc);
+
+
+
}
osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
+ tick += delay_tick;
+ osDelayUntil(tick);
+
}
}
diff --git a/User/task/user_task.h b/User/task/user_task.h
index 958b2c3..81c836e 100644
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@@ -20,7 +20,6 @@
#define TASK_FREQ_CTRL_CMD (500u)
#define TASK_FREQ_NUC (500u)
#define TASK_FREQ_CAN (1000u)
-#define TASK_FREQ_R12DS (1000u)
#define TASK_FREQ_RC (1000u)
#define TASK_FREQ_ERROR_DTC (3u)