PC会覆盖其他输入源命令

CMakeLists.txt

@@ -76,7 +76,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/module/config.c
     User/module/gimbal.c
     User/module/shoot.c
-
+    User/module/cmd.c
     # User/task sources
     User/task/atti_esti.c
     User/task/blink.c

User/module/chassis.c

@@ -1,208 +1,299 @@
+/*
+底盘模组
+*/
 
-#include "chassis.h"
+/*
+底盘模组
+*/
 
-#include "can.h"
+#include "cmsis_os2.h"
-#include "device/at9s_pro.h"
+#include <stdlib.h>
-#include "component/pid.h"
+#include "bsp/mm.h"
+#include "bsp/can.h"
+#include "component/ahrs.h"
 #include "device/motor_rm.h"
-#include <math.h>
+#include "device/motor.h"
-#include "bsp/time.h"
+#include "module/chassis.h"
 
-KPID_t pid_wheel[4];
+/**
-KPID_Params_t pid_wheel_params={1, 0.8, 0.0, 0.0, 0.0, 0.8,-1.0,-1.0};
+ * @brief 底盘小陀螺模式相关参数
-
+ */
-KPID_t pid_lockagl[4];
+#define CHASSIS_ROTOR_WZ_MIN 0.6f       //小陀螺最小速度
-KPID_Params_t pid_lockagl_params={1, 0.8, 0.05, 0.0, 0.1, 0.8,-1.0,M_2PI};
+#define CHASSIS_ROTOR_WZ_MAX 0.8f				//小陀螺最大速度
-
+#define M_7OVER72PI (M_2PI * 7.0f / 72.0f)   //角度偏移量（用在跟随云台35°）
-KPID_t pid_lockomg[4];
+#define CHASSIS_ROTOR_OMEGA 0.001f        //角速度变化频率
-KPID_Params_t pid_lockomg_params={2, 1, 0.0, 0.01, 0.0, 0.8,-1.0,-1.0};
 
 
-KPID_t pid_follow;
-KPID_Params_t pid_follow_params={0.5, 1, 0, 0.0, 0, 0.0, -1.0, -1};
 
-MOTOR_RM_Param_t motor1to4_param[4]={
+/**
-    {.can=BSP_CAN_1,.id=0x201,.module=MOTOR_M3508,.reverse=false,.gear=true,},
+ * @brief 设置底盘模式
-    {.can=BSP_CAN_1,.id=0x202,.module=MOTOR_M3508,.reverse=false,.gear=true,},
+ * @param c 底盘结构体指针
-    {.can=BSP_CAN_1,.id=0x203,.module=MOTOR_M3508,.reverse=false,.gear=true,},
+ * @param mode 目标控制模式
-    {.can=BSP_CAN_1,.id=0x204,.module=MOTOR_M3508,.reverse=false,.gear=true,}
+ * @param now 当前时间戳(ms)
-};
+ * @return CHASSIS_OK:成功   CHASSIS_ERR_NULL:空
+ */
+static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, uint32_t now) {
+    if (!c) 
+			return CHASSIS_ERR_NULL;
+    if (mode == c->mode)
+			return CHASSIS_OK;
+//随机种子，小陀螺模式随机设置旋转方向
+    if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
+        srand(now);
+        c->wz_multi = (rand() % 2) ? -1 : 1;
+    }
+//重置PID和滤波
+    for (uint8_t i = 0; i < c->num_wheel; i++) {
+        PID_Reset(&c->pid.motor[i]);
+        LowPassFilter2p_Reset(&c->filter.in[i], 0.0f);
+        LowPassFilter2p_Reset(&c->filter.out[i], 0.0f);
+    }
 
-MOTOR_Feedback_t motor1to4_measure[4];
+    c->mode = mode;
-float lockagl_out[4];
+    return CHASSIS_OK;
-float chassis_out[4];
-float Wheel_status[4];
-void Inverse_resolve(float vx,float vy,float w)
-{
-    Wheel_status[0]=vx-vy+w;
-    Wheel_status[1]=vx+vy+w;
-    Wheel_status[2]=-vx+vy+w;
-    Wheel_status[3]=-vx-vy+w;
 }
 
-void Motor_Init(float freq)
+/**
-{	for(int i=0;i<4;i++)
+ * @brief 小陀螺模式动态角速度
-	{
+ * @param min 最小速度
-	PID_Init(&pid_wheel[i], KPID_MODE_CALC_D, freq,&pid_wheel_params);
+ * @param max 最大速度
-		PID_Init(&pid_lockagl[i], KPID_MODE_CALC_D, freq,&pid_lockagl_params);
+ * @param now 当前时间戳(ms)
-	PID_Init(&pid_lockomg[i], KPID_MODE_CALC_D, freq,&pid_lockomg_params);
+ * @return 角速度值
+ */
+static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
+    float wz_vary = fabsf(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
+    return (wz_vary > max) ? max : wz_vary;
+}
 
-	}
+/**
-	
+ * @brief 底盘模式初始化
-		
+ * @param c 底盘结构体指针
-	PID_Init(&pid_follow, KPID_MODE_NO_D, freq,&pid_follow_params);
+ * @param param  底盘参数结构体指针
-    BSP_CAN_Init();
+ * @param mech_zero 机械零点欧拉角
-    for(int i=0;i<4;i++){
+ * @param target_freq 控制频率(Hz)
-	MOTOR_RM_Register(&motor1to4_param[i]);
+ * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空 CHASSIS_ERR_TYPE:不支持的模式
+ */
+int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
+                    float target_freq) {
+    if (!c) return CHASSIS_ERR_NULL;
+											
+		//初始化CAN通信									
+		BSP_CAN_Init();
+    c->param = param;
+    c->mode = CHASSIS_MODE_RELAX;
+//根据底盘不同设置模式轮子与混合器
+    Mixer_Mode_t mixer_mode;
+    switch (param->type) {
+        case CHASSIS_TYPE_MECANUM://麦轮
+					c->num_wheel = 4; 
+				mixer_mode = MIXER_MECANUM;
+				break;
+        case CHASSIS_TYPE_PARLFIX4:
+					c->num_wheel = 4; 
+				mixer_mode = MIXER_PARLFIX4;
+				break;
+        case CHASSIS_TYPE_PARLFIX2:
+					c->num_wheel = 2; 
+				mixer_mode = MIXER_PARLFIX2; 
+				break;
+        case CHASSIS_TYPE_OMNI_CROSS:
+					c->num_wheel = 4;
+				mixer_mode = MIXER_OMNICROSS;
+				break;
+        case CHASSIS_TYPE_OMNI_PLUS: //全向轮（老步兵类型）
+					c->num_wheel = 4;
+				mixer_mode = MIXER_OMNIPLUS;
+				break;
+        case CHASSIS_TYPE_SINGLE:
+					c->num_wheel = 1; 
+				mixer_mode = MIXER_SINGLE;
+				break;
+        default: 
+					return CHASSIS_ERR_TYPE;
+    }
+		//初始化时间戳
+				c->last_wakeup = 0;
+				c->dt = 0.0f;
+		//初始化PID和滤波
+    for (uint8_t i = 0; i < c->num_wheel; i++) {
+        PID_Init(&c->pid.motor[i], KPID_MODE_NO_D, target_freq, &param->pid.motor_pid_param);
+        LowPassFilter2p_Init(&c->filter.in[i], target_freq, param->low_pass_cutoff_freq.in);
+        LowPassFilter2p_Init(&c->filter.out[i], target_freq, param->low_pass_cutoff_freq.out);
+			//清零电机反馈
+				c->feedback.motor[i].rotor_speed = 0;
+        c->feedback.motor[i].torque_current = 0;
+        c->feedback.motor[i].rotor_abs_angle = 0;
+        c->feedback.motor[i].temp = 0;
+    }
+     //初始化PID和混合器
+    PID_Init(&c->pid.follow, KPID_MODE_NO_D, target_freq, &param->pid.follow_pid_param);
+    Mixer_Init(&c->mixer, mixer_mode);
+     //清零运动向量和输出
+    c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;
+		for (uint8_t i = 0; i < c->num_wheel; i++) { 
+				c->out.motor[i] = 0.0f;
+		}
+		//注册大疆电机
+		for (int i = 0; i < c->num_wheel; i++) {
+			MOTOR_RM_Register(&(c->param->motor_param[i]));
+			 
+		}
+    return CHASSIS_OK;
+}
+
+/**
+ * @brief 更新电机反馈(IMU+电机状态)
+ * @param c 底盘结构体指针
+ * @param feedback 底盘反馈指针结构体
+ * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
+ */
+int8_t Chassis_UpdateFeedback(Chassis_t *c) {
+
+
+		//更新所有电机反馈
+    for (uint8_t i = 0; i < c->num_wheel; i++) {
+			MOTOR_RM_Update(&(c->param->motor_param[i])); 
+			MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(c->param->motor_param[i]));
+			c->motors[i] = rm_motor;
+			MOTOR_RM_t *rm = c->motors[i];
+         if (rm_motor != NULL) {
+            c->feedback.motor[i] = rm_motor->feedback;
+             }else 
+					{ 
+					return CHASSIS_ERR_NULL; 
+					} 
+		}
+    return CHASSIS_OK;
+}
+
+/**
+ * @brief 底盘电机控制
+ * @param c 底盘结构体指针
+ * @param c_cmd 控制命令
+ * @param now 当前时间戳(ms)
+ * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
+ */
+int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
+    if (!c || !c_cmd) return CHASSIS_ERR_NULL;
+		//计算控制周期
+    c->dt = (float)(now - c->last_wakeup) / 1000.0f; 
+    c->last_wakeup = now;
+		if (!isfinite(c->dt) || c->dt <= 0.0f) {
+				c->dt = 0.001f;            
+		}
+		if (c->dt < 0.0005f) c->dt = 0.0005f;   
+		if (c->dt > 0.050f)  c->dt = 0.050f;    
+    //设置模式
+    Chassis_SetMode(c, c_cmd->mode, now);
+    //不同模式下对应解算（运动向量）
+    switch (c->mode) {
+        case CHASSIS_MODE_BREAK:
+            c->move_vec.vx = c->move_vec.vy = 0.0f;
+            break;
+        case CHASSIS_MODE_INDEPENDENT:
+            c->move_vec.vx = c_cmd->ctrl_vec.vx;
+            c->move_vec.vy = c_cmd->ctrl_vec.vy;
+            break;
+        default: {      //遥控器坐标->机体坐标系
+            float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
+            float cosb = cosf(beta);
+            float sinb = sinf(beta);
+            c->move_vec.vx = cosb * c_cmd->ctrl_vec.vx - sinb * c_cmd->ctrl_vec.vy;
+            c->move_vec.vy = sinb * c_cmd->ctrl_vec.vx + cosb * c_cmd->ctrl_vec.vy;
+            break;
+        }
+    }
+		//根据模式计算底盘角速度
+    switch (c->mode) {
+        case CHASSIS_MODE_RELAX:
+        case CHASSIS_MODE_BREAK:
+        case CHASSIS_MODE_INDEPENDENT:
+            c->move_vec.wz = 0.0f;
+            break;
+        case CHASSIS_MODE_OPEN:
+						c->move_vec.wz = c_cmd->ctrl_vec.wz;
+						break;
+				//云台跟随
+        case CHASSIS_MODE_FOLLOW_GIMBAL:
+            c->move_vec.wz = PID_Calc(&c->pid.follow, c->mech_zero, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
+            break;
+				//云台跟随（偏移）
+        case CHASSIS_MODE_FOLLOW_GIMBAL_35:
+            c->move_vec.wz = PID_Calc(&c->pid.follow,c->mech_zero +M_7OVER72PI, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
+            break;
+				//小陀螺
+        case CHASSIS_MODE_ROTOR:
+            c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
+            break;
+    }
+        //运动学逆解算，运动向量分解为电机转速
+    Mixer_Apply(&c->mixer, &c->move_vec, c->setpoint.motor_rpm, c->num_wheel, 500.0f);
+
+
+    for (uint8_t i = 0; i < c->num_wheel; i++) {
+				float rf = c->setpoint.motor_rpm[i];///目标转速
+        float fb = LowPassFilter2p_Apply(&c->filter.in[i], (float)c->feedback.motor[i].rotor_speed);
+				float out_current;
+        switch (c->mode) {
+            case CHASSIS_MODE_BREAK:
+            case CHASSIS_MODE_FOLLOW_GIMBAL:
+            case CHASSIS_MODE_FOLLOW_GIMBAL_35:
+            case CHASSIS_MODE_ROTOR:
+            case CHASSIS_MODE_INDEPENDENT:
+                out_current = PID_Calc(&c->pid.motor[i], c->setpoint.motor_rpm[i], fb, 0.0f, c->dt);
+                break;
+            case CHASSIS_MODE_OPEN:
+                out_current = c->setpoint.motor_rpm[i] / 7000.0f;
+                break;
+            case CHASSIS_MODE_RELAX:
+                out_current = 0.0f;
+                break;
+        }
+
+       //低通滤波和限幅
+         c->out.motor[i] = LowPassFilter2p_Apply(&c->filter.out[i], out_current);
+				 Clip(&c->out.motor[i], -c->param->limit.max_current, c->param->limit.max_current);
+    }
+
+			
+    return CHASSIS_OK;
+}
+
+/**
+ * @brief 电机输出
+ * @param c 底盘结构体指针
+ */
+void Chassis_Output(Chassis_t *c) {
+    if (!c) 
+			return;
+
+    //每个电机目标输出
+    for (uint8_t i = 0; i < c->num_wheel; i++) {
+        MOTOR_RM_t *rm = c->motors[i];
+        if (!rm) continue;
+        MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
+    }
+
+    //调用ctrl
+    for (uint8_t i = 0; i < c->num_wheel; i++) {
+        MOTOR_RM_t *rm = c->motors[i];
+        if (rm) {
+            MOTOR_RM_Ctrl(&rm->param);
+        }
     }
 }
-int64_t now,last_wakeup;
-float dt;
-float vx,vy,w;float chassis_yaw;
-float speed[4];
-float lockagl[4]={0,0,0,0};
-void chassis_control(chassis_ctrl_eulr_t eulr, COMP_AT9S_CMD_t cmd_rc)//k:ת<><D7AA><EFBFBD>޷<EFBFBD>
-{
-	//float vx,vy,w;
-	now 		=BSP_TIME_Get_us() / 1000000.0f;
-    dt 			=(BSP_TIME_Get_us() - last_wakeup) / 1000000.0f;
-    last_wakeup	=BSP_TIME_Get_us();
-	float delta_angle;
-	float cos_delta_angle;
-	float sin_delta_angle;
-    for(int i=0;i<4;i++){
-	MOTOR_RM_Update(&motor1to4_param[i]);
-    MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&motor1to4_param[i]);
-		if(motor_fed!=NULL)
-		{
-			motor1to4_measure[i]=motor_fed->motor.feedback;
-		}
-    }
-	switch(cmd_rc.mode){
-		case 0:
-	vx=0;vy=0;w=0;
-		break;
-		case 1:
-	delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
-      cos_delta_angle = cosf(delta_angle);
-      sin_delta_angle = sinf(delta_angle);
-      vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
-      vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
-		w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw,
-                  eulr.chassis_encoder_yaw, 0.0f, dt);
-		break;
-		case 2:
-	PID_ResetIntegral(&pid_follow);
-		
-	  delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
-      cos_delta_angle = cosf(delta_angle);
-      sin_delta_angle = sinf(delta_angle);
-      vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
-      vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
-		
-		w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw,
-                  eulr.chassis_encoder_yaw, 0.0f, dt);
-		break;
-		case 3:
-	  delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
-      cos_delta_angle = cosf(delta_angle);
-      sin_delta_angle = sinf(delta_angle);
-      vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
-      vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
-		
-	  w=0.6f; 
-		break;
-		default :
-			break;
-	}
-	
-	
-    Inverse_resolve(vx, vy, w);
-	float abs_max = 0.0f;
-	for (int8_t i = 0; i < 4; i++) {
-		const float abs_val = fabsf(Wheel_status[i]);
-		abs_max = (abs_val > abs_max) ? abs_val : abs_max;
-	}
-	if (abs_max > 1.0f) {
-		for (int8_t i = 0; i < 4; i++) {
-			Wheel_status[i] /= abs_max;
-		}
-	}
-	
-//	if(vx==0&&vy==0)PID_ResetIntegral(&pid_wheel);
-	switch(cmd_rc.mode){
-		case 0:
-			PID_ResetIntegral(&pid_wheel[0]);
-			PID_ResetIntegral(&pid_wheel[1]);
-			PID_ResetIntegral(&pid_wheel[2]);
-			PID_ResetIntegral(&pid_wheel[3]);
-	chassis_out[0] = 0;
-    chassis_out[1] = 0;
-    chassis_out[2] = 0;
-    chassis_out[3] = 0;
-		break;
-		case 1:
-			PID_ResetIntegral(&pid_wheel[0]);
-			PID_ResetIntegral(&pid_wheel[1]);
-			PID_ResetIntegral(&pid_wheel[2]);
-			PID_ResetIntegral(&pid_wheel[3]);
-//	chassis_out[0] = 0;
-//    chassis_out[1] = 0;
-//    chassis_out[2] = 0;
-//    chassis_out[3] = 0;
-		for(int i=0;i<4;i++){
-				speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
-			
-				if(speed[i]>1)speed[i]=1;
-		else if(speed[i]<-1)speed[i]=-1;
-				lockagl_out[i] = PID_Calc(&pid_lockagl[i],lockagl[i],motor1to4_measure[i].rotor_abs_angle, 0,dt);
-				chassis_out[i] = PID_Calc(&pid_lockomg[i],lockagl_out[i],speed[i], 0,dt);
-				if(chassis_out[i]>0.8f)chassis_out[i]=0.8f;
-			}
-		break;
-		case 2:
-			for(int i=0;i<4;i++){
-				
-				lockagl[i]=motor1to4_measure[i].rotor_abs_angle;
-				
-				speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
-				if(speed[i]>1)speed[i]=1;
-		else if(speed[i]<-1)speed[i]=-1;
-				chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt);
-				
-			}
-			
-		break;                                                                            
-		case 3:                                                                           
-		for(int i=0;i<4;i++){
-			
-				lockagl[i]=motor1to4_measure[i].rotor_abs_angle;
-			
-				speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
-				if(speed[i]>1)speed[i]=1;
-		else if(speed[i]<-1)speed[i]=-1;
-				chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt);
-			
-			}
-		break;
-		default :
-			break;
-	}
 
-//	for(int i;i<4;i++){
+/**
-//		chassis_out[i]*=0.6;
+ * @brief 重置底盘输出
-//	}
+ * @param c  底盘结构体指针
-    MOTOR_RM_SetOutput(&motor1to4_param[0], chassis_out[0]);
+ */
-    MOTOR_RM_SetOutput(&motor1to4_param[1], chassis_out[1]);
+void Chassis_ResetOutput(Chassis_t *c) {
-    MOTOR_RM_SetOutput(&motor1to4_param[2], chassis_out[2]);
+    if (!c) return;
-    MOTOR_RM_SetOutput(&motor1to4_param[3], chassis_out[3]);
+    for (uint8_t i = 0; i < c->num_wheel; i++) {
-//MOTOR_RM_SetOutput(&motor1to4_param[1], 0);
+        MOTOR_RM_t *m = c->motors[i];
-//    MOTOR_RM_SetOutput(&motor1to4_param[2], 0);
+        if (m) {
-    MOTOR_RM_Ctrl(&motor1to4_param[0]);
+            MOTOR_RM_Relax(&(m->param));
+        }
+    }
 }
 
-
-
-
-
-

User/module/chassis.h

@@ -1,21 +1,238 @@
+/*
+ 底盘模组
+ */
+#pragma once
 
 
-#ifndef CHASSIS_CONTROL_H
+#ifdef __cplusplus
-#define CHASSIS_CONTROL_H
+extern "C" {
+#endif
 
-#include "main.h"
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+#include "bsp/can.h"
+#include "component\filter.h"
+#include "component\mixer.h"
+#include "component\pid.h"
+#include "component\ahrs.h"
+#include "device/motor_rm.h"
+/* Exported constants ------------------------------------------------------- */
+#define CHASSIS_OK (0)        /* 运行正常 */
+#define CHASSIS_ERR (-1)      /* 运行时出现了一些小错误 */
+#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
+#define CHASSIS_ERR_MODE (-3) /* 运行时出配置了错误的ChassisMode_t */
+#define CHASSIS_ERR_TYPE (-4) /* 运行时出配置了错误的Chassis_Type_t */
+	
+#define MAX_MOTOR_CURRENT 20.0f 
+/* 底盘控制模式 */
+typedef enum {
+  CHASSIS_MODE_RELAX, /* 放松模式,电机不输出。一般情况底盘初始化之后的模式 */
+  CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制静止。用于机器人停止状态 */
+  CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
+  CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35°跟随云台 */
+  CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
+  CHASSIS_MODE_INDEPENDENT, /*独立模式。底盘运行不受云台影响 */
+  CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
+} Chassis_Mode_t;
+	
+/* 小陀螺转动模式 */
+typedef enum {
+  ROTOR_MODE_CW,   /* 顺时针转动 */
+  ROTOR_MODE_CCW,  /* 逆时针转动 */
+  ROTOR_MODE_RAND, /* 随机转动 */
+} Chassis_RotorMode_t;
 
-#include "component/at9s_pro_cmd.h"
+/* 底盘控制命令 */
-/* ŷ<><C5B7><EFBFBD>ǣ<EFBFBD>Euler angle<6C><65> */
 typedef struct {
-	float chassis_mech_zero_yaw;
+  Chassis_Mode_t mode;     /* 底盘运行模式 */
-	float chassis_encoder_yaw;
+  Chassis_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
-} chassis_ctrl_eulr_t;
+  MoveVector_t ctrl_vec;      /* 底盘控制向量 */
+} Chassis_CMD_t;
 
-void Inverse_resolve(float vx,float vy,float w);
+/* 限位 */
-void Motor_Init(float freq);
+typedef struct {
-void chassis_control(chassis_ctrl_eulr_t eulr, COMP_AT9S_CMD_t cmd_rc);
+  float max;
+  float min;
+} Chassis_Limit_t;
+
+	/* 底盘类型（底盘的物理设计） */
+typedef enum {
+  CHASSIS_TYPE_MECANUM,    /* 麦克纳姆轮 */
+  CHASSIS_TYPE_PARLFIX4,   /* 平行摆放的四个驱动轮 */
+  CHASSIS_TYPE_PARLFIX2,   /* 平行摆放的两个驱动轮 */
+  CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆放的四个全向轮 */
+  CHASSIS_TYPE_OMNI_PLUS,  /* 十字型摆设的四个全向轮 */
+  CHASSIS_TYPE_DRONE,      /* 无人机底盘 */
+  CHASSIS_TYPE_SINGLE,     /* 单个摩擦轮 */
+} Chassis_Type_t;
+
+
+/* 底盘参数结构体,ALL初始化参数 */
+typedef struct {
+	MOTOR_RM_Param_t motor_param[4];
+	  struct {
+    KPID_Params_t motor_pid_param;  /* 底盘电机PID参数 */
+    KPID_Params_t follow_pid_param; /* 跟随云台PID参数 */
+  } pid;
+  Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
+
+  /* 低通滤波器截至频率*/
+  struct {
+    float in;  /* 输入 */
+    float out; /* 输出 */
+  } low_pass_cutoff_freq;
+
+  /* 电机反装，应该和云台设置相同*/
+  struct {
+    bool yaw;
+  } reverse;
+	struct {
+        float max_vx, max_vy, max_wz;
+        float max_current; 
+    } limit; 
+} Chassis_Params_t;
+
+typedef struct {
+  AHRS_Gyro_t gyro;
+  AHRS_Eulr_t eulr;
+} Chassis_IMU_t;
+
+typedef struct {
+  MOTOR_Feedback_t motor[4];          // 四个 3508电机 反馈
+	float encoder_gimbalYawMotor;
+} Chassis_Feedback_t;
+
+/* 底盘输出结构体*/
+typedef struct {
+	float motor[4];  
+} Chassis_Output_t;
+
+/*
+ * 运行的主结构体￣
+ * 包含初始化参数,中间变量,输出变量
+ */
+typedef struct {
+  uint64_t last_wakeup;
+  float dt;
+
+  Chassis_Params_t *param; /* 底盘参数,用Chassis_Init设定 */
+
+  /* 模块通用 */
+  Chassis_Mode_t mode; /* 底盘模式 */
+
+
+  /* 底盘设计 */
+  int8_t num_wheel; /* 底盘轮子数量 */
+  Mixer_t mixer;    /* 混合器,移动向量->电机目标值 */
+  MoveVector_t move_vec; /* 底盘实际的运动向量 */
+  MOTOR_RM_t *motors[4];/*指向底盘每个电机参数*/
+	float mech_zero;
+  float wz_multi; /* 小陀螺旋转模式 */
+
+  /* PID计算目标值 */
+  struct {
+    float motor_rpm[4]; /* 电机转速的动态数组,单位:RPM */
+  } setpoint;
+
+  /* 反馈控制用的PID */
+  struct {
+    KPID_t motor[4]; /* 控制轮子电机用的PID的动态数组 */
+    KPID_t follow; /* 跟随云台用的PID */
+  } pid;
+ 
+	struct {
+        Chassis_Limit_t vx, vy, wz; 
+    } limit; 
+	
+  /* 滤波器 */
+  struct {
+    LowPassFilter2p_t in[4];  /* 反馈值滤波器 */
+    LowPassFilter2p_t out[4]; /* 输出值滤波器 */
+  } filter;
+
+  Chassis_Output_t out; /* 电机输出 */
+	Chassis_Feedback_t feedback;
+	//float out_motor[4];
+} Chassis_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * \brief 底盘初始化
+ *
+ * \param c 包含底盘数据的结构体
+ * \param param 包含底盘参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 运行结果
+ */
+int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
+                    float target_freq);
+
+/**
+ * \brief 更新底盘反馈信息
+ *
+ * \param c 包含底盘数据的结构体
+ * \param can CAN设备结构体
+ *
+ * \return 运行结果
+ */
+int8_t Chassis_UpdateFeedback(Chassis_t *c);
+
+/**
+ * \brief 运行底盘控制逻辑
+ *
+ * \param c 包含底盘数据的结构体
+ * \param c_cmd 底盘控制指令
+ * \param dt_sec 两次调用的时间间隔
+ *
+ * \return 运行结果
+ */
+int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd,
+                       uint32_t now);
+
+
+/**
+ * \brief 复制底盘输出值
+ *
+ * \param s 包含底盘数据的结构体
+ * \param out CAN设备底盘输出结构体
+ */
+void Chassis_Output(Chassis_t *c);
+
+/**
+ * \brief 清空Chassis输出数据
+ *
+ * \param out CAN设备底盘输出结构体
+ */
+void Chassis_ResetOutput(Chassis_t *c);
+
+
+/**
+ * @brief 底盘功率限制
+ *
+ * @param c 底盘数据
+ * @param cap 电容数据
+ * @param ref 裁判系统数据
+ * @return 函数运行结果
+ */
+//还没有加入，waiting。。。。。。int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
+//                          const Referee_ForChassis_t *ref);
+
+
+/**
+ * @brief 导出底盘数据
+ *
+ * @param chassis 底盘数据结构体
+ * @param ui UI数据结构体
+ */
+//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
+
+
+#ifdef __cplusplus
+}
 #endif
 
 
 
+

User/module/cmd.c

@@ -0,0 +1,602 @@
+/*
+  控制命令
+*/
+#include "module/cmd.h"
+#include <stdint.h>
+#include <string.h>
+
+/*************************************************************************************************************************************/
+/***************************************************************仲裁器****************************************************************/
+/*************************************************************************************************************************************/
+
+/* Includes ----------------------------------------------------------------- */
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+
+bool isREFOnline(CMD_t *c){return c->inputData.ref.online;}
+bool isNUCOnline(CMD_t *c){return c->inputData.nuc.online;}
+bool isRCOnline(CMD_t *c){return c->inputData.rc.online;}
+bool isPCOnline(CMD_t *c){return c->inputData.pc.online;}
+void PriorityConfigsRanking(CMD_Param_t *param) {
+    static bool init=false;
+    if(init) return;
+    for (int i = 0; i < CMD_SRC_NUM - 1; i++) {
+        for (int j = 0; j < CMD_SRC_NUM - i - 1; j++) {
+            if (param->sourcePriorityConfigs[j].priority < param->sourcePriorityConfigs[j + 1].priority) {
+                CMD_SourcePriorityConfig_t temp = param->sourcePriorityConfigs[j];
+                param->sourcePriorityConfigs[j] = param->sourcePriorityConfigs[j + 1];
+                param->sourcePriorityConfigs[j + 1] = temp;
+            }
+        }
+    }
+    init=!init;
+}
+CMD_InputSource_t getHighestPrioritySource(CMD_t *c) {
+    for (int i = 0; i < CMD_SRC_NUM; i++) {
+        CMD_InputSource_t source = c->params.sourcePriorityConfigs[i].source;
+        switch (source) {
+            case CMD_SRC_REFEREE:
+                if (isREFOnline(c)) {
+                    return CMD_SRC_REFEREE;
+                }
+                break;
+            case CMD_SRC_NUC:
+                if (isNUCOnline(c)) {
+                    return CMD_SRC_NUC;
+                }
+                break;
+            case CMD_SRC_RC:
+                if (isRCOnline(c)) {
+                    return CMD_SRC_RC;
+                }
+                break;
+            case CMD_SRC_PC:
+                if (isPCOnline(c)) {
+                    return CMD_SRC_PC;
+                }
+        }
+    }
+    return CMD_SRC_ERR;
+}
+
+int8_t Cmd_Arbiter(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    PriorityConfigsRanking(&c->params);
+
+    CMD_InputSource_t source = getHighestPrioritySource(c);
+
+    c->outCmd.chassis.source = source;
+    c->outCmd.gimbal.source = source;
+    c->outCmd.shoot.source = source;
+
+    return CMD_OK;
+}
+/* Exported functions ------------------------------------------------------- */
+
+
+
+/*************************************************************************************************************************************/
+/****************************************************************RC*******************************************************************/
+/*************************************************************************************************************************************/
+/* API ---------------------------------------------------------------------- */
+/* Mrobot生成 */
+#define RC_SELECT_Index 1
+/* 扩展接口 */
+#if RC_SELECT_Index == 0
+#define FOR_EACH_RC(X) X(dr16, DR16)
+#elif RC_SELECT_Index == 1
+#define FOR_EACH_RC(X) X(at9s, AT9S)
+#endif
+/* Includes ----------------------------------------------------------------- */
+#if RC_SELECT_Index == 0
+#include "device/dr16.h"
+#elif RC_SELECT_Index == 1
+#include "device/at9s_pro.h"
+#endif
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+#define RC_X_FIELD_MACRO(name, NAME) DEVICE_##NAME##_t name;
+#define RC_X_EXTERN_MACRO(name, NAME) extern DEVICE_##NAME##_t name##_out;
+#define RC_X_COPYDEFINE_MACRO(name, NAME) \
+    static void copy_##name(rc_inputdata_u *dst) { dst->name = name##_out; }
+#define RC_X_COPYREFERENCE_MACRO(name, NAME) copy_##name,
+union rc_inputdata_u{
+    FOR_EACH_RC(RC_X_FIELD_MACRO)
+};
+FOR_EACH_RC(RC_X_EXTERN_MACRO)
+FOR_EACH_RC(RC_X_COPYDEFINE_MACRO)
+/* Private variables -------------------------------------------------------- */
+/*静态缓冲区*/
+static rc_inputdata_u rc_buffer;
+/* Private function  -------------------------------------------------------- */
+int8_t Cmd_Get_rc(CMD_InputData_RC_t *rc)
+{
+    FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer); 
+    rc->inputData = &rc_buffer;                     
+    rc->type = RC_SELECT_Index;
+    return CMD_OK;
+}
+
+
+int8_t Cmd_BuildChassisCommandFromInput_rc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+#if RC_SELECT_Index == 0
+    c->input_rc.inputData->dr16.data =
+#elif RC_SELECT_Index == 1
+    switch (c->inputData.rc.inputData->at9s.data.key_E) {
+    case AT9S_CMD_SW_DOWN:
+      c->outCmd.chassis.cmd.mode = CHASSIS_MODE_RELAX;
+      break;
+    case AT9S_CMD_SW_MID:
+      c->outCmd.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
+      break;
+    case AT9S_CMD_SW_UP:
+      c->outCmd.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
+      break;
+    default:
+      c->outCmd.chassis.cmd.mode = CHASSIS_MODE_RELAX;
+      break;
+    }
+    c->outCmd.chassis.cmd.ctrl_vec.vx = c->inputData.rc.inputData->at9s.data.ch_l_y;
+    c->outCmd.chassis.cmd.ctrl_vec.vy = c->inputData.rc.inputData->at9s.data.ch_l_x;
+    c->outCmd.chassis.cmd.ctrl_vec.wz = c->inputData.rc.inputData->at9s.data.ch_r_x;
+#endif
+    return CMD_OK;
+}
+
+int8_t Cmd_BuildGimbalCommandFromInput_rc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+#if RC_SELECT_Index == 0
+    c->input_rc.inputData->dr16.data =
+#elif RC_SELECT_Index == 1
+	switch (c->inputData.rc.inputData->at9s.data.key_G) {
+    case AT9S_CMD_SW_DOWN:
+      c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
+      c->outCmd.gimbal.cmd.delta_yaw = 0.0f;
+      c->outCmd.gimbal.cmd.delta_pit = 0.0f;
+      break;
+    case AT9S_CMD_SW_MID:
+      c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
+      c->outCmd.gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
+      c->outCmd.gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
+      break;
+    case AT9S_CMD_SW_UP:
+      c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
+      c->outCmd.gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
+      c->outCmd.gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
+      break;
+    default:
+      c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
+      c->outCmd.gimbal.cmd.delta_yaw = 0.0f;
+      c->outCmd.gimbal.cmd.delta_pit = 0.0f;
+      break;
+    }
+#endif
+    return CMD_OK;
+}
+
+
+int8_t Cmd_BuildShootCommandFromInput_rc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+#if RC_SELECT_Index == 0
+    c->input_rc.inputData->dr16.data =
+#elif RC_SELECT_Index == 1
+    switch (c->inputData.rc.inputData->at9s.data.key_C) {
+    case AT9S_CMD_SW_DOWN:
+	  c->outCmd.shoot.cmd.online = at9s_out.online;
+      c->outCmd.shoot.cmd.ready    = true;
+      c->outCmd.shoot.cmd.firecmd  = true;
+      break;
+    case AT9S_CMD_SW_MID:
+	  c->outCmd.shoot.cmd.online = at9s_out.online;
+      c->outCmd.shoot.cmd.ready    = true;
+      c->outCmd.shoot.cmd.firecmd  = false;
+      break;
+    case AT9S_CMD_SW_UP:
+	  c->outCmd.shoot.cmd.online = at9s_out.online;
+      c->outCmd.shoot.cmd.ready    = false;
+      c->outCmd.shoot.cmd.firecmd  = false;
+      break;
+    default:
+	  c->outCmd.shoot.cmd.online = at9s_out.online;
+      c->outCmd.shoot.cmd.ready    = false;
+      c->outCmd.shoot.cmd.firecmd  = false;
+      break;
+    }
+#endif
+    return CMD_OK;
+}
+/* Exported functions ------------------------------------------------------- */
+
+
+/*************************************************************************************************************************************/
+/*****************************************************************PC******************************************************************/
+/*************************************************************************************************************************************/
+
+/* Includes ----------------------------------------------------------------- */
+/* Private typedef ---------------------------------------------------------- */
+typedef int8_t (*CMD_BehaviorFunc)(CMD_t *c);
+typedef struct {
+    CMD_Behavior_t behavior;
+    CMD_BehaviorFunc func;
+} CMD_BehaviorHandlerFunc_t;
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+#define PC_MACRO_BEHAVIOR_TABLE(X) \
+    X(FORE)                 \
+    X(BACK)                 \
+    X(LEFT)                 \
+    X(RIGHT)                \
+    X(ACCELERATE)           \
+    X(DECELEBRATE)          \
+    X(FIRE)                 \
+    X(FIRE_MODE)            \
+    X(BUFF)                 \
+    X(AUTOAIM)              \
+    X(OPENCOVER)            \
+    X(ROTOR)                \
+    X(REVTRIG)               \
+    X(FOLLOWGIMBAL35)       
+#define PC_MACRO_FOR_DECLARE_HANDLER_FUNCTION(BEHAVIOR) \
+    static int8_t Cmd_PC_HandleBehavior##BEHAVIOR(CMD_t *c);
+
+#define PC_MACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY(BEHAVIOR) \
+    {CMD_BEHAVIOR_##BEHAVIOR, Cmd_PC_HandleBehavior##BEHAVIOR},
+
+PC_MACRO_BEHAVIOR_TABLE(PC_MACRO_FOR_DECLARE_HANDLER_FUNCTION)
+/* Private variables -------------------------------------------------------- */
+CMD_BehaviorHandlerFunc_t behaviorHandlerFunc[] = {
+    PC_MACRO_BEHAVIOR_TABLE(PC_MACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY)
+};
+/* Private function  -------------------------------------------------------- */
+static int8_t Cmd_PC_HandleBehaviorFORE(CMD_t *c){
+    c->outCmd.chassis.cmd.ctrl_vec.vy += c->params.pc.sensitivity.move_sense;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorBACK(CMD_t *c){
+    c->outCmd.chassis.cmd.ctrl_vec.vy -= c->params.pc.sensitivity.move_sense;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorLEFT(CMD_t *c){
+    c->outCmd.chassis.cmd.ctrl_vec.vx -= c->params.pc.sensitivity.move_sense;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorRIGHT(CMD_t *c){
+    c->outCmd.chassis.cmd.ctrl_vec.vx += c->params.pc.sensitivity.move_sense;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorACCELERATE(CMD_t *c){
+    c->outCmd.chassis.cmd.ctrl_vec.vx *= c->params.pc.sensitivity.move_fast_sense;
+    c->outCmd.chassis.cmd.ctrl_vec.vy *= c->params.pc.sensitivity.move_fast_sense;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorDECELEBRATE(CMD_t *c){
+    c->outCmd.chassis.cmd.ctrl_vec.vx *= c->params.pc.sensitivity.move_slow_sense;
+    c->outCmd.chassis.cmd.ctrl_vec.vy *= c->params.pc.sensitivity.move_slow_sense;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorFIRE(CMD_t *c){
+    c->outCmd.shoot.cmd.firecmd = true;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorFIRE_MODE(CMD_t *c){
+    c->outCmd.shoot.cmd.mode++;
+    c->outCmd.shoot.cmd.mode %= SHOOT_MODE_NUM;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorBUFF(CMD_t *c){
+//    if (cmd->ai_status == AI_STATUS_HITSWITCH) {
+//        CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
+//        cmd->host_overwrite = false;
+//        cmd->ai_status = AI_STATUS_STOP;
+//    } else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+//        // 自瞄模式中切换失败提醒
+//    } else {
+//        CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
+//        cmd->ai_status = AI_STATUS_HITSWITCH;
+//        cmd->host_overwrite = true;
+//    }
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorAUTOAIM(CMD_t *c){
+//    if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+//        cmd->host_overwrite = false;
+//        cmd->ai_status = AI_STATUS_STOP;
+//        CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
+//    } else {
+//        cmd->ai_status = AI_STATUS_AUTOAIM;
+//        cmd->host_overwrite = true;
+//        CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
+//    }
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorOPENCOVER(CMD_t *c){
+//    c->shoot.cover_open = !c->shoot.cover_open;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorROTOR(CMD_t *c){
+    c->outCmd.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
+    c->outCmd.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorREVTRIG(CMD_t *c){
+//    c->outCmd.shoot.cmd.reverse_trig = true;
+    return CMD_OK;
+}
+static int8_t Cmd_PC_HandleBehaviorFOLLOWGIMBAL35(CMD_t *c){
+    c->outCmd.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
+    return CMD_OK;
+}
+
+static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *c,
+                                               CMD_Behavior_t behavior) {
+  return c->params.pc.map.key_map[behavior].key;
+}
+
+static inline CMD_TriggerType_t CMD_BehaviorToActive(CMD_t *c,
+                                                    CMD_Behavior_t behavior) {
+  return c->params.pc.map.key_map[behavior].trigger_type;
+}
+
+int8_t Cmd_Get_pc(CMD_InputData_PC_t *pc)
+{
+    pc->online = false;
+//    pc->data=
+    return CMD_OK;
+}
+
+static bool CMD_BehaviorOccurredRc(CMD_t *c, CMD_Behavior_t behavior) {
+  CMD_KeyValue_t key = CMD_BehaviorToKey(c, behavior);
+  CMD_TriggerType_t active = CMD_BehaviorToActive(c, behavior);
+
+  bool now_key_pressed, last_key_pressed;
+
+  /* 按下按键为鼠标左、右键 */
+  if (key == CMD_L_CLICK) {
+    now_key_pressed = c->inputData.pc.data.mouse.l_click;
+    last_key_pressed = c->inputData.pc.lastData.mouse.l_click;
+  } else if (key == CMD_R_CLICK) {
+    now_key_pressed = c->inputData.pc.data.mouse.r_click;
+    last_key_pressed = c->inputData.pc.lastData.mouse.r_click;
+  } else {
+    now_key_pressed = c->inputData.pc.data.key & (1u << key);
+    last_key_pressed = c->inputData.pc.lastData.key & (1u << key);
+  }
+
+  switch (active) {
+    case CMD_ACTIVE_RISING_EDGE:
+      return !now_key_pressed && last_key_pressed;
+    case CMD_ACTIVE_FALLING_EDGE:
+      return now_key_pressed && !last_key_pressed;
+    case CMD_ACTIVE_PRESSED:
+      return now_key_pressed;
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+
+
+static void CMD_PcLogic(CMD_t *c, float dt_sec) {
+    c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
+
+    c->outCmd.gimbal.cmd.delta_yaw = (float)c->inputData.pc.data.mouse.x * dt_sec * c->params.pc.sensitivity.sens_mouse;
+    c->outCmd.gimbal.cmd.delta_pit = (float)c->inputData.pc.data.mouse.y * dt_sec * c->params.pc.sensitivity.sens_mouse;
+    c->outCmd.chassis.cmd.ctrl_vec.vx = c->outCmd.chassis.cmd.ctrl_vec.vy = 0.0f;
+
+    c->outCmd.shoot.cmd.firecmd = false;
+
+    for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) {
+        if (CMD_BehaviorOccurredRc(&c, behaviorHandlerFunc[i].behavior)) {
+            behaviorHandlerFunc[i].func(c);
+        }
+    }
+
+    c->inputData.pc.lastData.key = c->inputData.pc.data.key;
+    c->inputData.pc.lastData.mouse.l_click = c->inputData.pc.data.mouse.l_click;
+    c->inputData.pc.lastData.mouse.r_click = c->inputData.pc.data.mouse.r_click;
+}
+
+int8_t Cmd_BuildChassisCommandFromInput_pc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    return CMD_OK;
+}
+
+int8_t Cmd_BuildGimbalCommandFromInput_pc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    return CMD_OK;
+}
+int8_t Cmd_BuildShootCommandFromInput_pc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+     c->outCmd.shoot.cmd.firecmd = false;
+
+    for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) {
+        if (CMD_BehaviorOccurredRc(&c, behaviorHandlerFunc[i].behavior)) {
+            behaviorHandlerFunc[i].func(c);
+        }
+    }
+
+    c->inputData.pc.lastData.key = c->inputData.pc.data.key;
+    c->inputData.pc.lastData.mouse.l_click = c->inputData.pc.data.mouse.l_click;
+    c->inputData.pc.lastData.mouse.r_click = c->inputData.pc.data.mouse.r_click;
+
+    return CMD_OK;
+}
+/*************************************************************************************************************************************/
+/****************************************************************NUC******************************************************************/
+/*************************************************************************************************************************************/
+
+/* Includes ----------------------------------------------------------------- */
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+int8_t Cmd_BuildChassisCommandFromInput_nuc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    return CMD_OK;
+}
+
+int8_t Cmd_BuildGimbalCommandFromInput_nuc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    return CMD_OK;
+}
+int8_t Cmd_BuildShootCommandFromInput_nuc(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    return CMD_OK;
+}
+
+/*************************************************************************************************************************************/
+/***************************************************************REF*******************************************************************/
+/*************************************************************************************************************************************/
+
+/* Includes ----------------------------------------------------------------- */
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+int8_t Cmd_BuildChassisCommandFromInput_referee(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    return CMD_OK;
+}
+
+int8_t Cmd_BuildGimbalCommandFromInput_referee(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    return CMD_OK;
+}
+int8_t Cmd_BuildShootCommandFromInput_referee(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    return CMD_OK;
+}
+
+/*************************************************************************************************************************************/
+/***************************************************************分发命令***************************************************************/
+/*************************************************************************************************************************************/
+
+/* Includes ----------------------------------------------------------------- */
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+typedef int8_t (*CMD_BuildCommandFunc)(CMD_t *c);
+
+typedef struct {
+    CMD_InputSource_t source;
+    CMD_BuildCommandFunc chassisFunc;
+    CMD_BuildCommandFunc gimbalFunc;
+    CMD_BuildCommandFunc shootFunc;
+} CMD_SourceHandler_t;
+
+CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
+    {CMD_SRC_RC, Cmd_BuildChassisCommandFromInput_rc, Cmd_BuildGimbalCommandFromInput_rc, Cmd_BuildShootCommandFromInput_rc},
+    {CMD_SRC_PC, Cmd_BuildChassisCommandFromInput_pc, Cmd_BuildGimbalCommandFromInput_pc, Cmd_BuildShootCommandFromInput_pc},
+    {CMD_SRC_NUC, Cmd_BuildChassisCommandFromInput_nuc, Cmd_BuildGimbalCommandFromInput_nuc, Cmd_BuildShootCommandFromInput_nuc},
+    {CMD_SRC_REFEREE, Cmd_BuildChassisCommandFromInput_referee, Cmd_BuildGimbalCommandFromInput_referee, Cmd_BuildShootCommandFromInput_referee},
+};
+
+int8_t Cmd_GenerateCommand(CMD_t *c) {
+    if (c == NULL) {
+        return CMD_ERR_NULL; // 参数错误
+    }
+
+    Cmd_Arbiter(c);
+    switch (c->outCmd.chassis.source) {
+        case CMD_SRC_RC:
+            sourceHandlers[CMD_SRC_RC].chassisFunc(c);
+            break;
+        case CMD_SRC_PC:
+            sourceHandlers[CMD_SRC_PC].chassisFunc(c);
+            break;
+        case CMD_SRC_NUC:
+            sourceHandlers[CMD_SRC_NUC].chassisFunc(c);
+            break;
+        case CMD_SRC_REFEREE:
+            sourceHandlers[CMD_SRC_REFEREE].chassisFunc(c);
+            break;
+        default:
+            break;
+    }
+    switch (c->outCmd.gimbal.source) {
+        case CMD_SRC_RC:
+            sourceHandlers[CMD_SRC_RC].gimbalFunc(c);
+            break;
+        case CMD_SRC_PC:
+            sourceHandlers[CMD_SRC_PC].gimbalFunc(c);
+            break;
+        case CMD_SRC_NUC:
+            sourceHandlers[CMD_SRC_NUC].gimbalFunc(c);
+            break;
+        case CMD_SRC_REFEREE:
+            sourceHandlers[CMD_SRC_REFEREE].gimbalFunc(c);
+            break;
+        default:
+            break;
+    }
+    switch (c->outCmd.shoot.source) {
+        case CMD_SRC_RC:
+            sourceHandlers[CMD_SRC_RC].shootFunc(c);
+            break;
+        case CMD_SRC_PC:
+            sourceHandlers[CMD_SRC_PC].shootFunc(c);
+            break;
+        case CMD_SRC_NUC:
+            sourceHandlers[CMD_SRC_NUC].shootFunc(c);
+            break;
+        case CMD_SRC_REFEREE:
+            sourceHandlers[CMD_SRC_REFEREE].shootFunc(c);
+            break;
+        default:
+            break;
+    }
+
+    return CMD_OK;
+}

User/module/cmd.h

@@ -0,0 +1,222 @@
+/*
+  控制命令
+*/
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "module/chassis.h"
+#include "module/gimbal.h"
+#include "module/shoot.h"
+
+
+/* Exported constants ------------------------------------------------------- */
+#define CMD_OK (0)        /* 运行正常 */
+#define CMD_ERR_NULL (-1) /* 运行时发现NULL指针 */    
+#define CMD_ERR_ERR (-2)  /* 运行时发现了其他错误 */
+#define CMD_ERR_MODE (-3) /* 运行时配置了错误的Mode */
+#define CMD_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */
+#define CMD_ERR_MALLOC (-5) /* 内存分配失败 */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef union rc_inputdata_u rc_inputdata_u; 
+
+#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */
+
+/* 输入源枚举 */
+typedef enum {
+  CMD_SRC_RC=0,      /* 遥控器 */
+  CMD_SRC_PC,      /* 键盘鼠标 */
+  CMD_SRC_NUC,     /* 上位机 */
+  CMD_SRC_REFEREE, /* 裁判系统 */
+  CMD_SRC_NUM,
+  CMD_SRC_ERR
+} CMD_InputSource_t;
+
+typedef enum {
+  CMD_MODULE_CHASSIS=0,
+  CMD_MODULE_GIMBAL,
+  CMD_MODULE_SHOOT,
+  CMD_MODULE_NUM /* 模块数量 */
+} CMD_MODULE_t;
+
+/* RC part begin-------------------------------------- */
+typedef struct {
+  bool online;
+  enum {DR16, AT9S} type;
+  rc_inputdata_u *inputData;               
+} CMD_InputData_RC_t;
+
+/* RC part end---------------------------------------- */
+
+/* PC part begin-------------------------------------- */
+/* 键盘按键值 */
+typedef enum {
+  CMD_KEY_W = 0,
+  CMD_KEY_S,
+  CMD_KEY_A,
+  CMD_KEY_D,
+  CMD_KEY_SHIFT,
+  CMD_KEY_CTRL,
+  CMD_KEY_Q,
+  CMD_KEY_E,
+  CMD_KEY_R,
+  CMD_KEY_F,
+  CMD_KEY_G,
+  CMD_KEY_Z,
+  CMD_KEY_X,
+  CMD_KEY_C,
+  CMD_KEY_V,
+  CMD_KEY_B,
+  CMD_L_CLICK,
+  CMD_R_CLICK,
+  CMD_KEY_NUM,
+} CMD_KeyValue_t;
+
+typedef enum {
+  CMD_ACTIVE_RISING_EDGE, /* 按下时触发 */
+  CMD_ACTIVE_FALLING_EDGE,   /* 抬起时触发 */
+  CMD_ACTIVE_PRESSED,  /* 按住时触发 */
+} CMD_TriggerType_t;
+
+/* 行为值序列 */
+typedef enum {
+  CMD_BEHAVIOR_FORE = 0,       /* 向前 */
+  CMD_BEHAVIOR_BACK,           /* 向后 */
+  CMD_BEHAVIOR_LEFT,           /* 向左 */
+  CMD_BEHAVIOR_RIGHT,          /* 向右 */
+  CMD_BEHAVIOR_ACCELERATE,     /* 加速 */
+  CMD_BEHAVIOR_DECELEBRATE,    /* 减速 */
+  CMD_BEHAVIOR_FIRE,           /* 开火 */
+  CMD_BEHAVIOR_FIRE_MODE,      /* 切换开火模式 */
+  CMD_BEHAVIOR_BUFF,           /* 打符模式 */
+  CMD_BEHAVIOR_AUTOAIM,        /* 自瞄模式 */
+  CMD_BEHAVIOR_OPENCOVER,      /* 弹舱盖开关 */
+  CMD_BEHAVIOR_ROTOR,          /* 小陀螺模式 */
+  CMD_BEHAVIOR_REVTRIG,        /* 反转拨弹 */
+  CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */
+  CMD_BEHAVIOR_NUM,
+} CMD_Behavior_t;
+typedef struct {
+  CMD_KeyValue_t key;
+  CMD_TriggerType_t trigger_type;
+} CMD_KeyMapItem_t;
+
+/* 行为映射的对应按键数组 */
+typedef struct {
+  CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM];
+} CMD_KeyMap_Params_t;
+
+typedef struct {
+  float sens_mouse;       /* 鼠标灵敏度 */
+  float move_sense;        /* 移动灵敏度 */
+  float move_fast_sense;   /* 快速移动灵敏度 */
+  float move_slow_sense;   /* 慢速移动灵敏度 */
+} CMD_PC_Sensitivity_t;
+
+typedef struct {
+  struct {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+    bool l_click; /* 左键 */
+    bool r_click; /* 右键 */
+  } mouse;        /* 鼠标值 */
+
+  uint16_t key; /* 按键值 */
+
+  uint16_t res; /* 保留，未启用 */
+} CMD_PC_Data_t;
+
+typedef struct {
+  struct {
+    bool l_click; /* 左键 */
+    bool r_click; /* 右键 */
+  } mouse;        /* 鼠标值 */
+  uint16_t key; /* 按键值 */
+} CMD_PC_LastData_t;
+
+typedef struct {
+  bool online;
+  CMD_PC_Data_t data;
+  CMD_PC_LastData_t lastData;
+}CMD_InputData_PC_t;
+
+typedef struct {
+  CMD_KeyMap_Params_t map; /* 按键映射行为命令 */
+  CMD_PC_Sensitivity_t sensitivity; /* PC灵敏度设置 */
+}CMD_PCParams_t;
+
+/* PC part end---------------------------------------- */
+
+/* NUC part begin------------------------------------- */
+typedef struct {
+  bool online;
+}CMD_InputData_NUC_t;
+
+/* NUC part end--------------------------------------- */
+
+/* REF part begin------------------------------------- */
+typedef struct {
+  bool online;
+  CMD_PC_Data_t data;
+}CMD_InputData_REF_t;
+
+/* REF part begin------------------------------------- */
+
+/* 底盘控制命令 */
+typedef struct {
+  CMD_InputSource_t source;
+  Chassis_CMD_t cmd;
+} CMD_OutputCmd_CHASSIS_t;
+
+/* 云台控制命令 */
+typedef struct {
+  CMD_InputSource_t source;
+  Gimbal_CMD_t cmd;
+} CMD_OutputCmd_GIMBAL_t;
+
+/* 射击控制命令 */
+typedef struct {
+  CMD_InputSource_t source;
+  Shoot_CMD_t cmd;
+} CMD_OutputCmd_SHOOT_t;
+
+typedef struct {
+  CMD_InputData_RC_t rc;
+  CMD_InputData_PC_t pc;
+  CMD_InputData_NUC_t nuc;
+  CMD_InputData_REF_t ref;
+} CMD_InputData_t;
+typedef struct {
+  CMD_OutputCmd_CHASSIS_t chassis;
+  CMD_OutputCmd_GIMBAL_t  gimbal;
+  CMD_OutputCmd_SHOOT_t   shoot;
+} CMD_OutputCmd_t;
+typedef struct {
+    CMD_InputSource_t source;
+    uint8_t priority;
+} CMD_SourcePriorityConfig_t;
+
+typedef struct {
+  CMD_SourcePriorityConfig_t sourcePriorityConfigs[CMD_SRC_NUM];/* 输入源优先级配置 */
+  CMD_PCParams_t pc;
+} CMD_Param_t;
+
+typedef struct {
+
+  CMD_Param_t params;
+
+  CMD_InputData_t inputData;
+  CMD_OutputCmd_t outCmd;
+  
+
+} CMD_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+#ifdef __cplusplus
+}
+#endif

User/module/config.c

@@ -16,6 +16,84 @@
 
 // 机器人参数配置
 Config_RobotParam_t robot_config = {
+    
+  .chassis_param = {
+
+    .motor_param = {
+      { 
+			.can = BSP_CAN_1, 
+			.id = 0x201, 
+			.module = MOTOR_M3508, 
+			.reverse = false,
+			.gear =  true
+			},
+      { 
+			.can = BSP_CAN_1,
+			.id = 0x202, 
+			.module = MOTOR_M3508, 
+			.reverse = false,
+			.gear =  true
+			},
+      { 
+			.can = BSP_CAN_1, 
+			.id = 0x203, 
+			.module = MOTOR_M3508, 
+			.reverse = false,
+			.gear =  true				
+			},
+      { 
+			.can = BSP_CAN_1,
+			.id = 0x204, 
+			.module = MOTOR_M3508, 
+			.reverse = false,
+			.gear =  true	
+			},
+    },
+		.type = CHASSIS_TYPE_OMNI_PLUS,
+
+    .pid = {
+
+      .motor_pid_param = {
+        .k = 0.001f,
+        .p = 0.01f,
+        .i = 0.01f,
+        .d = 0.0f,
+        .i_limit = 1.0f,
+        .out_limit = 1.0f,
+        .d_cutoff_freq = -1.0f,
+        .range = -1.0f,
+      },
+
+     
+      .follow_pid_param = {
+        .k = 0.5f,
+        .p = 1.0f,
+        .i = 0.0f,
+        .d = 0.0f,
+        .i_limit = 1.0f,
+        .out_limit = 1.0f,
+        .d_cutoff_freq = -1.0f,
+        .range = M_2PI,
+      },
+    },
+
+   
+    .low_pass_cutoff_freq = {
+      .in = 50.0f,
+      .out = 50.0f,
+    },
+
+   
+    .reverse = {
+      .yaw = true,
+    },
+		.limit = {
+      .max_vx = 3.0f,     
+      .max_vy = 3.0f,
+      .max_wz = 2.0f,    
+      .max_current = 16000.0f 
+    },
+  },
 
     
     .gimbal_param = {

User/module/config.h

@@ -18,7 +18,7 @@ extern "C" {
 typedef struct {
     Shoot_Params_t shoot_param;
 	Gimbal_Params_t gimbal_param;
-//    Chassis_Params_t chassis_param;
+    Chassis_Params_t chassis_param;
 } Config_RobotParam_t;
 
 /* Exported functions prototypes -------------------------------------------- */

User/module/shoot.h

@@ -42,7 +42,8 @@ typedef enum {
     SHOOT_MODE_SAFE = 0,       /* 安全模式 */
     SHOOT_MODE_SINGLE,         /* 单发模式 */
     SHOOT_MODE_BURST,          /* 多发模式 */
-    SHOOT_MODE_CONTINUE        /* 连发模式 */
+    SHOOT_MODE_CONTINUE,       /* 连发模式 */
+    SHOOT_MODE_NUM
 }Shoot_Mode_t;
 
 typedef enum {
@@ -52,7 +53,7 @@ typedef enum {
 
 typedef struct {
   bool online;	                 /* 遥控器在线 */
-  bool mode;                     /* 射击模式 */
+  Shoot_Mode_t mode;             /* 射击模式 */
   bool ready;                    /* 准备射击 */
   bool firecmd;                  /* 射击指令 */
 }Shoot_CMD_t;

User/task/cmd.c

@@ -20,7 +20,7 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-DEVICE_AT9S_t at9s_from_rc;
+DEVICE_AT9S_t at9s_out;
 Shoot_CMD_t cmd_for_shoot;
 //Chassis_Cmd_t cmd_for_chassis;
 Gimbal_CMD_t cmd_for_gimbal;
@@ -45,9 +45,9 @@ void Task_cmd(void *argument) {
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-	osMessageQueueGet(task_runtime.msgq.cmd.rc, &at9s_from_rc, NULL, 0);
+	osMessageQueueGet(task_runtime.msgq.cmd.rc, &at9s_out, NULL, 0);
 	 	  
-	switch (at9s_from_rc.data.key_G) {
+	switch (at9s_out.data.key_G) {
     case AT9S_CMD_SW_DOWN:
       cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
       cmd_for_gimbal.delta_yaw = 0.0f;
@@ -55,13 +55,13 @@ void Task_cmd(void *argument) {
       break;
     case AT9S_CMD_SW_MID:
       cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
-      cmd_for_gimbal.delta_yaw = -at9s_from_rc.data.ch_l_x * 2.0f;
+      cmd_for_gimbal.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
-      cmd_for_gimbal.delta_pit = -at9s_from_rc.data.ch_l_y * 1.5f;
+      cmd_for_gimbal.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
       break;
     case AT9S_CMD_SW_UP:
       cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
-      cmd_for_gimbal.delta_yaw = -at9s_from_rc.data.ch_l_x * 2.0f;
+      cmd_for_gimbal.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
-      cmd_for_gimbal.delta_pit = -at9s_from_rc.data.ch_l_y * 1.5f;
+      cmd_for_gimbal.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
       break;
     default:
       cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
@@ -70,24 +70,24 @@ void Task_cmd(void *argument) {
       break;
     }
 
-	switch (at9s_from_rc.data.key_C) {
+	switch (at9s_out.data.key_C) {
     case AT9S_CMD_SW_DOWN:
-		  cmd_for_shoot.online = at9s_from_rc.online;
+		  cmd_for_shoot.online = at9s_out.online;
       cmd_for_shoot.ready = true;
       cmd_for_shoot.firecmd = true;
       break;
     case AT9S_CMD_SW_MID:
-		  cmd_for_shoot.online = at9s_from_rc.online;
+		  cmd_for_shoot.online = at9s_out.online;
       cmd_for_shoot.ready = true;
       cmd_for_shoot.firecmd = false;
       break;
     case AT9S_CMD_SW_UP:
-		  cmd_for_shoot.online = at9s_from_rc.online;
+		  cmd_for_shoot.online = at9s_out.online;
       cmd_for_shoot.ready = false;
       cmd_for_shoot.firecmd = false;
       break;
     default:
-		  cmd_for_shoot.online = at9s_from_rc.online;
+		  cmd_for_shoot.online = at9s_out.online;
       cmd_for_shoot.ready = false;
       cmd_for_shoot.firecmd = false;
       break;

User/task/ctrl_chassis.c

@@ -4,11 +4,10 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
-#include "task/user_task.h"
+	#include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-#include <can.h>
 #include "module/chassis.h"
-#include "component/at9s_pro_cmd.h"
+#include "module/config.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -16,8 +15,10 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-chassis_ctrl_eulr_t chassis_ctrl_eulr;
+Chassis_t chassis;
-COMP_AT9S_CMD_t chassis_ctrl_cmd_rc;
+Chassis_CMD_t chassis_cmd;   
+Chassis_IMU_t chassis_imu;
+static AHRS_Eulr_t mech_zero = {0};  
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -33,18 +34,20 @@ void Task_ctrl_chassis(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
-Motor_Init(CTRL_CHASSIS_FREQ);
+  Config_RobotParam_t *cfg = Config_GetRobotParam();
+  Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ);
+  chassis.mech_zero=4.13f;
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_ctrl_cmd_rc, NULL, 0); 
+    osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis.feedback.encoder_gimbalYawMotor, NULL, 0);
-	  osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis_ctrl_eulr.chassis_encoder_yaw, NULL, 0); 
+		osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);//遥控器
 
-	  chassis_ctrl_eulr.chassis_mech_zero_yaw=4.13f;
+	  Chassis_UpdateFeedback(&chassis);
-////	  chassis_ctrl_eulr.chassis_encoder_yaw=motor_gimbal_yaw.total_angle;
+    Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
-	  chassis_control (chassis_ctrl_eulr,chassis_ctrl_cmd_rc);
+    Chassis_Output(&chassis);
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

ozone/hero.jdebug

@@ -5,7 +5,7 @@
 **********************************************************************
 
 File          : 
-Created       : 14. Oct 2025 21:13
+Created       : 02. Nov 2025 13:12
 Ozone Version : V3.40b
 */
 
@@ -22,8 +22,8 @@ void OnProjectLoad (void) {
   //
   // Dialog-generated settings
   //
-  Project.AddPathSubstitute ("D:/CUBEMX/hero/ozone", "$(ProjectDir)");
+  Project.AddPathSubstitute ("D:/CUBEMX/hero/god-yuan-hero/ozone", "$(ProjectDir)");
-  Project.AddPathSubstitute ("d:/cubemx/hero/ozone", "$(ProjectDir)");
+  Project.AddPathSubstitute ("d:/cubemx/hero/god-yuan-hero/ozone", "$(ProjectDir)");
   Project.SetDevice ("STM32F407IG");
   Project.SetHostIF ("USB", "");
   Project.SetTargetIF ("SWD");
@@ -32,7 +32,7 @@ void OnProjectLoad (void) {
   //
   // User settings
   //
-  File.Open ("D:/CUBEMX/hero/MDK-ARM/hero/hero.axf");
+  File.Open ("D:/CUBEMX/hero/god-yuan-hero/build/Debug/hero.elf");
 }
 
 /*********************************************************************

ozone/hero.jdebug.user

@@ -1,42 +1,26 @@
 
 
-GraphedExpression="((shoot).target_variable).target_rpm", Color=#e56a6f, Show=0
+OpenDocument="ctrl_shoot.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_shoot.c", Line=0
-GraphedExpression="(shoot).errtosee", Color=#35792b, Show=0
+OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_gimbal.c", Line=14
-GraphedExpression="chassis_out[2]", Color=#769dda
+OpenDocument="ctrl_chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_chassis.c", Line=7
-OpenDocument="startup_stm32f407xx.s", FilePath="D:/CUBEMX/hero/MDK-ARM/startup_stm32f407xx.s", Line=161
+OpenDocument="user_task.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/user_task.c", Line=0
-OpenDocument="chassis.h", FilePath="D:/CUBEMX/hero/User/module/chassis.h", Line=0
+OpenDocument="main.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/main.c", Line=60
-OpenDocument="ctrl_chassis.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_chassis.c", Line=4
-OpenDocument="pid.h", FilePath="D:/CUBEMX/hero/User/component/pid.h", Line=0
-OpenDocument="shoot.h", FilePath="D:/CUBEMX/hero/User/module/shoot.h", Line=166
-OpenDocument="bmi088.c", FilePath="D:/CUBEMX/hero/User/device/bmi088.c", Line=159
-OpenDocument="stm32f4xx_it.c", FilePath="D:/CUBEMX/hero/Core/Src/stm32f4xx_it.c", Line=87
-OpenDocument="shoot.c", FilePath="D:/CUBEMX/hero/User/module/shoot.c", Line=364
-OpenDocument="ctrl_shoot.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_shoot.c", Line=0
-OpenDocument="can.c", FilePath="D:/CUBEMX/hero/User/bsp/can.c", Line=485
-OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_gimbal.c", Line=0
-OpenDocument="chassis.c", FilePath="D:/CUBEMX/hero/User/module/chassis.c", Line=20
-OpenDocument="main.c", FilePath="D:/CUBEMX/hero/Core/Src/main.c", Line=48
 OpenToolbar="Debug", Floating=0, x=0, y=0
-OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=609, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
+OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=599, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=309, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=319, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=609, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=599, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1279, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
+OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1276, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=BOTTOM, x=0, y=0, w=1280, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="1 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="758;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1053;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="1004;0"
+OpenWindow="Timeline", DockArea=BOTTOM, x=0, y=0, w=1283, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1056;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1056;-69", CodeGraphLegendShown=1, CodeGraphLegendPosition="1007;0"
-OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=1279, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=1276, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
-TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;482]
 TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100]
+TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
+TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;100;100;100;100;110;126;422]
 TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
+TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;294]
+TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;103]
 TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
-TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;726]
+TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;366]
-TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;100]
-TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((shoot).target_variable).target_rpm";" (shoot).errtosee";" chassis_out[2]"], ColWidths=[100;100;100;100;100]
-TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;144;134;124;134;110;134;281]
-TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
-WatchedExpression="gimbal", RefreshRate=5, Window=Watched Data 1
-WatchedExpression="shoot", RefreshRate=5, Window=Watched Data 1
-WatchedExpression="shoot.feedback.fric[2]", RefreshRate=2, Window=Watched Data 1
-WatchedExpression="chassis_out", RefreshRate=5, Window=Watched Data 1
-WatchedExpression="shoot_cmd", RefreshRate=5, Window=Watched Data 1