From e0b108b4af9ec66e50559eaddfc9da90ccdef52b Mon Sep 17 00:00:00 2001
From: xxxxm <2389287465@qq.com>
Date: Fri, 20 Mar 2026 04:08:29 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=A4=9A=E8=BD=AE=E5=BA=95?=
 =?UTF-8?q?=E7=9B=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/module/chassis.c          | 716 ++++++++++++++++++++-------------
 User/module/chassis.h          | 431 ++++++++++----------
 User/module/cmd/cmd.c          |  40 +-
 User/module/cmd/cmd_behavior.c |  18 +-
 User/module/config.c           | 118 ++----
 User/module/config.h           |   2 +-
 User/module/struct_typedef.h   |  14 +
 User/task/ctrl_chassis.c       |  21 +-
 User/task/init.c               |   2 +-
 ozone/arm.jdebug.user          |   4 +-
 10 files changed, 746 insertions(+), 620 deletions(-)
 create mode 100644 User/module/struct_typedef.h

diff --git a/User/module/chassis.c b/User/module/chassis.c
index f055b7f..2265134 100644
--- a/User/module/chassis.c
+++ b/User/module/chassis.c
@@ -1,314 +1,460 @@
+
 /*
-µ×ÅÌÄ£×é
+ 底盘任务
+*/
+/*
+舵轮底盘模组
+example：
+Chassis_t chassis;
+Chassis_CMD_t cmd_chassis;
+
+chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);
+
+ osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
+ osMessageQueueGet(task_runtime.msgq.imu.gyro, &chassis.pos088.bmi088.gyro, NULL, 0);
+ 这个是用来调防翻的，可以不用，而且需要c板放在地盘不能在云台上面
+
+ osMessageQueueGet(task_runtime.msgq.gimbal.yaw6020,&chassis.motorfeedback.gimbal_yaw_encoder,NULL,0);
+    
+    if(osMessageQueueGet(task_runtime.msgq.cmd.chassis, &cmd_chassis, NULL, 0)==osOK)
+    {
+      Chassis_update(&chassis);
+      Chassis_Control(&chassis, &cmd_chassis,tick);
+    }else
+{
+	 Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
+    Chassis_Control(&chassis, &safe_cmd);
+}
+    Chassis_Setoutput(&chassis);
+
+
 */
 
-#include "cmsis_os2.h"
-#include <stdlib.h>
-#include "bsp/mm.h"
-#include "bsp/can.h"
-#include "component/ahrs.h"
+
+
+/* Includes ----------------------------------------------------------------- */
+#include "chassis.h"
 #include "device/motor_rm.h"
-#include "device/motor.h"
-#include "module/chassis.h"
-#include "component/PowerControl.h"
-#include "config.h"
+#include "bsp/time.h"
+#include "bsp/can.h"
 #include "math.h"
-/**
- * @brief µ×ÅÌСÍÓÂÝģʽÏà¹Ø²ÎÊý
- */
-#define CHASSIS_ROTOR_WZ_MIN 0.6f       //СÍÓÂÝ×îСËÙ¶È
-#define CHASSIS_ROTOR_WZ_MAX 0.8f				//СÍÓÂÝ×î´óËÙ¶È
-#define M_7OVER72PI (M_2PI * 7.0f / 72.0f)   //½Ç¶ÈÆ«ÒÆÁ¿£¨ÓÃÔÚ¸úËæÔÆ̨35¡ã£©
-#define CHASSIS_ROTOR_OMEGA 0.001f        //½ÇËٶȱ仯ƵÂÊ
+#include "component/pid.h"
+#include "component/filter.h"
+#include "stdlib.h"
+/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处(define.h里有快捷方法)，
+进debug将四个轮子编码器朝右（左右无所谓，可能会导致5065方向反，在解算里加个负号就行）
+查看6020反馈值，将6020反馈值放入motor_offset中*/
 
+/*chassis_t结构体中的内容现在有 move_vec (最终输出速度)
+hopemotorout(期望的底盘输出值)舵轮解算出的各个电机的期望输出值 包括四个6020 和四个3508
+final_out;(经PID计算后的实际发送给电机的实时输出值)  四个6020 四个3508
+motorfeedback(电机反馈数据) 四个3508 四个6020
+pid 各个电机的pid参数
+ChassisImu_t pos088;  088的实时姿态
+ MotorOffset_t motoroffset;	      //6020校准数据
+const	Chassis_Param_t *param;     //一些固定的参数
+fp32 vofa_send[8];                //vofa输出数据
+  LowPassFilter2p_t filled[9];      //低通滤波器
+  float keep_angle[4];              // 保持的 6020 角度
+*/
 
+/* Private typedef ---------------------------------------------------------- */
+#define CHASSIS_ROTOR_OMEGA 0.001
+#define CHASSIS_ROTOR_WZ_MIN 0.6f          /* 小陀螺旋转位移下界 */
+#define CHASSIS_ROTOR_WZ_MAX 0.8f          /* 小陀螺旋转位移上界 */
+#define ONE_MINUTE 60.0f          /* 一分钟时间 */
 
-/**
- * @brief ÉèÖõ×ÅÌģʽ
- * @param c µ×Å̽ṹÌåÖ¸Õë
- * @param mode Ä¿±ê¿ØÖÆģʽ
- * @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);
-    }
+static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode ,uint32_t now)
+{
+  if (c == NULL)
+    return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
+  if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
 
-    c->mode = mode;
-    return CHASSIS_OK;
+ if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
+    srand(now);
+    c->wz_multi =1;
+    // c->wz_multi = (rand() % 2) ? -1 : 1;
+  }
+  for (int i = 0; i < 4; i++)
+  {
+    PID_Reset(&c->pid.Radder_DIR_omega[i]);
+    PID_Reset(&c->pid.Radder_DIR_angle[i]);
+    PID_Reset(&c->pid.Wheel_DIR_omega[i]);
+    PID_Reset(&c->pid.Radder_DIR_omega[i]);		
+  }
+  c->mode = mode;
+  return CHASSIS_OK;
 }
 
 /**
- * @brief СÍÓÂÝģʽ¶¯Ì¬½ÇËÙ¶È
- * @param min ×îСËÙ¶È
- * @param max ×î´óËÙ¶È
- * @param now µ±Ç°Ê±¼ä´Á(ms)
- * @return ½ÇËÙ¶ÈÖµ
+ * @brief 产生小陀螺wz随机速度
+ *
+ * @param min wz产生最小速度
+ * @param max wz产生最大速度
+ * @param now ctrl_chassis的tick数
+ * @return float
  */
 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;
+  /* wz在min和max之间，上限0.6f */
+  float wz_vary = fabs(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
+  return wz_vary > 0.8f ? max : wz_vary;
 }
 
-/**
- * @brief µ×ÅÌģʽ³õʼ»¯
- * @param c µ×Å̽ṹÌåÖ¸Õë
- * @param param  µ×Å̲ÎÊý½á¹¹ÌåÖ¸Õë
- * @param mech_zero »úеÁãµãÅ·À­½Ç
- * @param target_freq ¿ØÖÆƵÂÊ(Hz)
- * @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_CALC_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 = c_cmd->ctrl_vec.wz;
-            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;
-}
-
-
-
-void Chassis_Power_Control(Chassis_t *c,float max_power)
+int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
 {
-    float* rpm=(float[4]){c->motors[0]->feedback.rotor_speed,c->motors[1]->feedback.rotor_speed,c->motors[2]->feedback.rotor_speed,c->motors[3]->feedback.rotor_speed};
-    power_model_t* param = (c->mode == CHASSIS_MODE_ROTOR) ? &cha2 : &cha;
-    power_calu(param,(float[4]){c->out.motor[0],c->out.motor[1],c->out.motor[2],c->out.motor[3]},rpm);
-    float scale = power_scale_calu(&param,1,max_power);
-    power_out_calu(param,scale,(float[4]){c->out.motor[0],c->out.motor[1],c->out.motor[2],c->out.motor[3]},rpm,c->out.motor);
+  if (c == NULL || param == NULL || target_freq <= 0.0f)
+  {
+    return CHASSIS_ERR; // 参数错误
+  }
+  c->param = param;
+  c->mode = CHASSIS_MODE_RELAX; // 默认模式为停止锁死底盘
+  c->mech_zero = c->param->mech_zero;/*云台6020的机械中点*/
+  c->travel = c->param->travel ;/*云台6020的机械行程*/
+//  c->mech_zero_4310 = c->param->mech_zero_4310;/*云台4310的机械中点*/
+  /*初始化can*/
+  BSP_CAN_Init();
+  /*注册轮向电机*/
+  for (int i = 0; i < 4; i++)
+  {
+    MOTOR_RM_Register(&(c->param->motor.Wheel_DIR[i]));
+  }
+  /*注册舵向电机*/
+  for (int i = 0; i < 4; i++)
+  {
+    MOTOR_RM_Register(&(c->param->motor.Radder_DIR[i]));
+  }
+
+  // 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
+  MotorOffset_t motor_offset = {{5.24851561/M_PI * 180.0f, 1.00092244/ M_PI * 180.0f, 
+                                 5.8199234/ M_PI * 180.0f, 4.74536991/ M_PI * 180.0f}}; // 右右右右
+  c->motoroffset = motor_offset;
+																 
+																 
+																 
+  /*对3508的速度环和6020的角速度以及位置环pid进行初始化*/
+  for (int i = 0; i < 4; i++)
+  {
+    PID_Init(&c->pid.Wheel_DIR_omega[i], KPID_MODE_NO_D, target_freq, &c->param->pid.Wheel_DIR_Omega);
+  }
+  for (int i = 0; i < 4; i++)
+  {
+    PID_Init(&c->pid.Radder_DIR_omega[i], KPID_MODE_CALC_D, target_freq, &c->param->pid.Radder_DIR_Omega);
+    PID_Init(&c->pid.Radder_DIR_angle[i], KPID_MODE_CALC_D, target_freq, &c->param->pid.Radder_DIR_Angle);
+  }
+
+  LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
+  LowPassFilter2p_Init(&c->filled[1], target_freq, 20.0f); // vy
+  LowPassFilter2p_Init(&c->filled[2], target_freq, 20.0f); // vw
+
+  LowPassFilter2p_Init(&c->filled[3], target_freq, 20.0f); // 3508-1
+  LowPassFilter2p_Init(&c->filled[4], target_freq, 20.0f); // 3508-2
+  LowPassFilter2p_Init(&c->filled[5], target_freq, 20.0f); // 3508-3  
+  LowPassFilter2p_Init(&c->filled[6], target_freq, 20.0f); // 3508-4
+
+  LowPassFilter2p_Init(&c->filled[7], target_freq, 20.0f); // 6020-1
+  LowPassFilter2p_Init(&c->filled[8], target_freq, 20.0f); // 6020-2
+  LowPassFilter2p_Init(&c->filled[9], target_freq, 20.0f); // 6020-3
+  LowPassFilter2p_Init(&c->filled[10], target_freq, 20.0f); // 6020-4 
+	
+  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]);
+// 底盘解算
+void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
+{
+
+  // RC模式下松开遥控器防止6020回到默认位置导致侧翻
+  if (c->mode == RC && fabs(c->move_vec.Vx) < 0.1 && fabs(c->move_vec.Vy) < 0.1 && fabs(c->move_vec.Vw) < 0.1)
+  {
+    // 如果之前不处于保持模式，则记录当前角度
+    if (!c->keeping_angle_flag)
+    {
+      c->keeping_angle_flag = 1; // 进入保持模式
     }
-
-    //µ÷ÓÃctrl
-    // for (uint8_t i = 0; i < c->num_wheel; i++) {
-    //     MOTOR_RM_t *rm = c->motors[0];
-    //     if (rm) {
-    //         MOTOR_RM_Ctrl(&rm->param);
-    //     }
-    // }
-
-    MOTOR_RM_t *rm = c->motors[0];
-    if (rm) {
-        MOTOR_RM_Ctrl(&rm->param);
+    // 使用保持的角度，而不是实时反馈值,速度为0
+    for (uint8_t i = 0; i < 4; i++)
+    {
+      c->hopemotorout.Radder_DIR_Solving_1[i] = c->keep_angle[i];
+      c->hopemotorout.Wheel_DIR_Solving_1[i] = 0;
     }
+      // c->hopemotorout.Radder_DIR_Solving_1[0] = 315;
+      // c->hopemotorout.Radder_DIR_Solving_1[1] = 45;
+      // c->hopemotorout.Radder_DIR_Solving_1[2] = 315;
+      // c->hopemotorout.Radder_DIR_Solving_1[3] = 45;
+  }
+  else
+  {
+    // 如果有速度输入，则退出保持模式
+    c->keeping_angle_flag = 0;
+    // 让保持角度实时等于进入保持阈值前的最后一次角度值
+    for (uint8_t i = 0; i < 4; i++)
+    {
+     c->keep_angle[i] = c->hopemotorout.Radder_DIR_Solving_1[i];
+    }
+		
+    switch (c->mode)
+    {
+    case RC:
+    case CHASSIS_MODE_ROTOR:
+    case CHASSIS_MODE_FOLLOW_GIMBAL:
+      
+      // const double radians = atan(1.0f * 330 / 330);
+
+      c->hopemotorout.Wheel_DIR_Solving_1[0] = sqrt(
+          (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy + c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy + c->move_vec.Vw * cos(radians)));
+
+      c->hopemotorout.Wheel_DIR_Solving_1[1] = sqrt(
+          (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy - c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy - c->move_vec.Vw * cos(radians)));
+
+      c->hopemotorout.Wheel_DIR_Solving_1[2] = sqrt(
+          (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy - c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy - c->move_vec.Vw * cos(radians)));
+
+      c->hopemotorout.Wheel_DIR_Solving_1[3] = sqrt(
+          (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy + c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy + c->move_vec.Vw * cos(radians)));
+
+      c->hopemotorout.Radder_DIR_Solving_1[0] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
+                                                     (c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
+                                               (180.0f / M_PI);
+      c->hopemotorout.Radder_DIR_Solving_1[1] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
+                                                     (c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
+                                               (180.0f / M_PI);
+      c->hopemotorout.Radder_DIR_Solving_1[2] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
+                                                     (c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
+                                               (180.0f / M_PI);
+      c->hopemotorout.Radder_DIR_Solving_1[3] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
+                                                     (c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
+                                               (180.0f / M_PI);
+      break;
+
+    case CHASSIS_MODE_BREAK:
+    case STOP:
+    case CHASSIS_MODE_RELAX:
+
+      for (int i = 0; i < 4; i++)
+      {
+        c->hopemotorout.Wheel_DIR_Solving_1[i] = 0.0f;
+      }
+      //      c->hopemotorout.Radder_DIR_Solving_1[0] = 6.26554489/M_PI*180.0f;
+      //      c->hopemotorout.Radder_DIR_Solving_1[1] = 2.1099906/M_PI*180.0f;
+      //      c->hopemotorout.Radder_DIR_Solving_1[2] = 2.08391285/M_PI*180.0f;
+      //      c->hopemotorout.Radder_DIR_Solving_1[3] = 5.26845694/M_PI*180.0f;
+      // c->hopemotorout.Radder_DIR_Solving_1[0] = 315;
+      // c->hopemotorout.Radder_DIR_Solving_1[1] = 45;
+      // c->hopemotorout.Radder_DIR_Solving_1[2] = 315;
+      // c->hopemotorout.Radder_DIR_Solving_1[3] = 45;
+
+      c->hopemotorout.Radder_DIR_Solving_1[0] = 0;
+      c->hopemotorout.Radder_DIR_Solving_1[1] = 0;
+      c->hopemotorout.Radder_DIR_Solving_1[2] = 0;
+      c->hopemotorout.Radder_DIR_Solving_1[3] = 0;
+      break;
+
+  
+    }
+  }
+  // 角度归化到0°——360°
+
+  for (uint8_t i = 0; i < 4; i++)
+  {
+    if (c->hopemotorout.Radder_DIR_Solving_1[i] < 0)
+    {
+      c->hopemotorout.Radder_DIR_Solving_1[i] += 360;
+    }
+  }
+
+  for (uint8_t i = 0; i < 4; i++)
+  {
+    float angle_error[4]; // 角度误差
+     angle_error[i] = c->hopemotorout.Radder_DIR_Solving_1[i] - c->feedback.motor_transformation.Radder_DIR_Angle[i];
+    // 误差角度归化到-180°——+180°
+    while (angle_error[i] > 180)
+      angle_error[i] -= 360;
+    while (angle_error[i] < -180)
+      angle_error[i] += 360;
+    /*这里发现如果下面的c->motorfeedback.rotor_angle6020[i]+angle_error[i]变为
+        c->hopemotorout.Radder_DIR_Solving_1[i]会导致6020出现故障*/
+    if (angle_error[i] > 90 && angle_error[i] <= 180)
+    {
+      c->hopemotorout.Wheel_DIR_Solving_2[i] = c->hopemotorout.Wheel_DIR_Solving_1[i];
+      c->hopemotorout.Radder_DIR_Solving_2[i] = c->feedback.motor_transformation.Radder_DIR_Angle[i] + angle_error[i] - 180;
+    }
+    else if (angle_error[i] < -90 && angle_error[i] >= -180)
+    {
+      c->hopemotorout.Wheel_DIR_Solving_2[i] = c->hopemotorout.Wheel_DIR_Solving_1[i];
+      c->hopemotorout.Radder_DIR_Solving_2[i] = c->feedback.motor_transformation.Radder_DIR_Angle[i] + angle_error[i] + 180;
+    }
+    else
+    {
+      c->hopemotorout.Wheel_DIR_Solving_2[i] = -c->hopemotorout.Wheel_DIR_Solving_1[i];
+      c->hopemotorout.Radder_DIR_Solving_2[i] = c->feedback.motor_transformation.Radder_DIR_Angle[i] + angle_error[i];
+    }
+  }
 }
 
-/**
- * @brief ÖØÖõ×ÅÌÊä³ö
- * @param c  µ×Å̽ṹÌåÖ¸Õë
- */
-void Chassis_ResetOutput(Chassis_t *c) {
-    if (!c) return;
-    for (uint8_t i = 0; i < c->num_wheel; i++) {
-        MOTOR_RM_t *m = c->motors[i];
-        if (m) {
-            MOTOR_RM_Relax(&(m->param));
-        }
-    }
+/*
+更新底盘电机反馈，并且进行归一化处理，但是这里的归一化是我自己测的电机最大转速
+近似于一进行处理，并不是准确的-1到1
+*/
+
+int8_t Chassis_update(Chassis_t *c)
+{
+  if (c == NULL)
+  {
+    return CHASSIS_ERR_NULL; // 参数错误
+  }
+
+	MOTOR_RM_t *Wheel_DIR_MOTOR[4];
+	MOTOR_RM_t *Radder_DIR_MOTOR[4];
+  /*更新电机反馈*/
+  for (int i = 0; i < 4; i++)
+  {
+		/* 轮向电机更新 */		
+	MOTOR_RM_Update(&(c->param->motor.Wheel_DIR[i]));
+	Wheel_DIR_MOTOR[i]= MOTOR_RM_GetMotor(&(c->param->motor.Wheel_DIR[i]));
+	c->feedback.motor.Wheel_DIR[i] = Wheel_DIR_MOTOR[i]->feedback;
+
+		/* 舵向电机 */		
+	MOTOR_RM_Update(&(c->param->motor.Radder_DIR[i]));
+	Radder_DIR_MOTOR[i]= MOTOR_RM_GetMotor(&(c->param->motor.Radder_DIR[i]));
+	c->feedback.motor.Radder_DIR[i] = Radder_DIR_MOTOR[i]->feedback;
+		
+		
+		/* 单位转换 */
+	c->feedback.motor_transformation.Radder_DIR_Angle[i]=c->feedback.motor.Radder_DIR[i].rotor_abs_angle/ M_PI * 180.0f;
+	c->feedback.motor_transformation.Radder_DIR_Omega[i]=	c->feedback.motor.Radder_DIR[i].rotor_speed/320;	
+	c->feedback.motor_transformation.Wheel_DIR_Omega[i]=	c->feedback.motor.Wheel_DIR[i].rotor_speed/10000;
+	c->feedback.motor_transformation.Radder_DIR_Angle[i] = fmod(c->feedback.motor_transformation.Radder_DIR_Angle[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0);
+		
+		if (c->feedback.motor_transformation.Radder_DIR_Angle[i] < 0)
+		{
+			c->feedback.motor_transformation.Radder_DIR_Angle[i] += 360;
+		}		
+		 
+	}
+
+
+  return CHASSIS_OK;
 }
 
+// 防止侧翻
+/*具体开始上车实验之后在进行编写修改，先暂时不使用*/
+void ChassisrolPrevent(Chassis_t *c)
+{
+}
+
+int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
+{
+
+  if (c == NULL || c_cmd == NULL)
+  {
+    return CHASSIS_ERR_NULL; // 参数错误
+  }
+
+  // c->dt = (BSP_TIME_Get_us() - c->last_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
+  // c->last_wakeup = BSP_TIME_Get_us();
+  c->dt = (float)(now - c->last_wakeup) / 1000.0f; /* 计算两次调用的时间间隔，单位秒 */
+  c->last_wakeup = now;
+
+  /*设置底盘模式*/
+  if (Chassis_SetMode(c, c_cmd->mode,now) != CHASSIS_OK)
+  {
+    return CHASSIS_ERR_MODE; /* 设置模式失败 */
+  }
+	
+	float beta;
+  
+  /*根据底盘模式进行不同的控制*/
+  switch (c->mode)
+  {
+  case CHASSIS_MODE_BREAK:
+  case CHASSIS_MODE_RELAX:
+  case STOP:
+    // 停止模式
+    c->move_vec.Vx = 0.0f;
+    c->move_vec.Vy = 0.0f;
+    c->move_vec.Vw = 0.0f;
+    break;
+
+  case RC:
+    // 遥控模式
+    // c->move_vec.Vx = c_cmd->Vx * c_cmd->throttle * 1000.0f;
+    // c->move_vec.Vy = c_cmd->Vy * c_cmd->throttle * 1000.0f;
+
+    c->move_vec.Vx = c_cmd->Vx * c_cmd->throttle;
+    c->move_vec.Vy = c_cmd->Vy * c_cmd->throttle;
+    c->move_vec.Vw = c_cmd->Vw * c_cmd->throttle;
+    c->move_vec.mul = c_cmd->throttle;
+    break;
+
+    case CHASSIS_MODE_ROTOR:
+    // 小陀螺模式
+
+    beta =  (c->feedback.motor.gimbal_yaw.rotor_abs_angle / 180.0f * M_PI) - c->mech_zero; // 云台当前角度转弧度
+    float cos_beta = cosf(beta);
+    float sin_beta = sinf(beta);
+
+    c->move_vec.Vx =cos_beta * c_cmd->x_l - sin_beta * c_cmd->y_l;
+    c->move_vec.Vy =sin_beta* c_cmd->x_l + cos_beta * c_cmd->y_l;
+    c->move_vec.Vw =c->wz_multi* Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
+    break;
+
+    case CHASSIS_MODE_FOLLOW_GIMBAL:
+    // 跟随云台模式
+    c->move_vec.Vx =-c_cmd->y_l;
+    c->move_vec.Vy =-c_cmd->x_l;
+    c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 2.06f ,c->feedback.motor.gimbal_yaw.rotor_abs_angle, 0.0f, c->dt);
+
+ //    c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
+    break;
+
+
+
+  default:
+    return CHASSIS_ERR_MODE; /* 未知模式 */
+  }
+
+  /*给输出的Vx，Vy，Vw进行滤波*/
+  c->move_vec.Vx = LowPassFilter2p_Apply(&c->filled[0], c->move_vec.Vx);
+  c->move_vec.Vy = LowPassFilter2p_Apply(&c->filled[1], c->move_vec.Vy);
+  c->move_vec.Vw = LowPassFilter2p_Apply(&c->filled[2], c->move_vec.Vw);
+
+  Chassis_speed_calculate(c, c_cmd);
+
+  for (int i = 0; i < 4; i++)
+  {
+    float chassis6020_detangle[4]; // 6020解算出的角度                  
+    c->hopemotorout.Radder_DIR_target[i] = c->hopemotorout.Radder_DIR_Solving_2[i];
+    chassis6020_detangle[i] = PID_Calc(&(c->pid.Radder_DIR_angle[i]), c->hopemotorout.Radder_DIR_target[i],
+																				c->feedback.motor_transformation.Radder_DIR_Angle[i], 0.0f, c->dt);
+    // c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个
+		c->chassis6020_detangle[i]=chassis6020_detangle[i];
+    c->final_out.final_Radder_DIR[i] = PID_Calc(&(c->pid.Radder_DIR_omega[i]), chassis6020_detangle[i],
+                                             c->feedback.motor_transformation.Radder_DIR_Omega[i], 0.0f, c->dt);
+
+    c->out.Radder_DIR[i] = LowPassFilter2p_Apply(&c->filled[7+i], c->final_out.final_Radder_DIR[i]);
+    c->hopemotorout.Wheel_DIR_target[i] = c->hopemotorout.Wheel_DIR_Solving_2[i];
+    c->final_out.final_Wheel_DIR[i] = PID_Calc(&(c->pid.Wheel_DIR_omega[i]), c->hopemotorout.Wheel_DIR_target[i],
+                                             c->feedback.motor_transformation.Wheel_DIR_Omega[i], 0.0f, c->dt);
+    c->out.Wheel_DIR[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_Wheel_DIR[i]);
+		
+  }
+  return CHASSIS_OK;
+}
+
+/*电机输出设定和发送*/
+void Chassis_Setoutput(Chassis_t *c)
+{
+    for (uint8_t i = 0; i < 4; i++) {		
+		MOTOR_RM_SetOutput(&(c->param->motor.Wheel_DIR[i]), c->out.Wheel_DIR[i]);	
+		MOTOR_RM_SetOutput(&(c->param->motor.Radder_DIR[i]), c->out.Radder_DIR[i]);
+    }
+		MOTOR_RM_Ctrl(&(c->param->motor.Wheel_DIR[0]));	
+		MOTOR_RM_Ctrl(&(c->param->motor.Radder_DIR[0]));			
+
+
+}
 
diff --git a/User/module/chassis.h b/User/module/chassis.h
index ef3c5ca..c11a772 100644
--- a/User/module/chassis.h
+++ b/User/module/chassis.h
@@ -1,239 +1,240 @@
-/*
- µ×ÅÌÄ£×é
- */
 #pragma once
 
-
 #ifdef __cplusplus
-extern "C" {
+extern "C"
+{
 #endif
 
-/* Includes ----------------------------------------------------------------- */
-#include <cmsis_os2.h>
-#include "bsp/can.h"
+#include "struct_typedef.h"
 #include "component/filter.h"
-#include "component/mixer.h"
 #include "component/pid.h"
 #include "component/ahrs.h"
+//#include "device/buzzer.h"
+#include "component/user_math.h"
+#include "component/filter.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;
 
-/* µ×ÅÌ¿ØÖÆÃüÁî */
 typedef struct {
-  Chassis_Mode_t mode;     /* µ×ÅÌÔËÐÐģʽ */
-  Chassis_RotorMode_t mode_rotor; /* СÍÓÂÝת¶¯Ä£Ê½ */
-  MoveVector_t ctrl_vec;      /* µ×ÅÌ¿ØÖÆÏòÁ¿ */
+    float max_velocity;
+    float acceleration;
+    float deceleration;
+} TrapezoidalProfile;
+
+#define CHASSIS_OK (0)
+#define CHASSIS_ERR (-1)
+#define CHASSIS_ERR_NULL (-2)
+#define CHASSIS_ERR_MODE (-3) /*CMD_ChassisMode_t */
+#define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */
+
+
+//     纵向/横向
+#define radians atan(1.0f * 390 /390) // 角度制
+
+
+/*底盘姿态模式*/
+typedef enum
+{
+	STOP_MODE,/* 急停模式 */
+	EXPAND_MODE,/* 展开模式 */
+  REDUCE_MODE,/* 收缩模式 */
+} CHASSIS_ATTITUDE_MODE_t;
+
+/*底盘状态*/
+typedef enum
+{
+	STOP_STATE,/* 急停状态 */
+	EXPAND_STATE,/* 展开状态 */
+  REDUCE_STATE,/* 收缩状态 */
+} CHASSIS_STATE_t;
+
+/*底盘模式*/
+typedef enum
+{
+    STOP, // 底盘平行
+    RC,   // 遥控模式
+    CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
+     CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
+     CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
+     CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
+    
+} Chassis_Mode_t;
+
+typedef struct
+{
+    int cmd_power_on_safe; // 上电安全标志
+    Chassis_Mode_t mode;
+		CHASSIS_ATTITUDE_MODE_t attitude_mode;
+    // 遥控器输出值
+    fp32 Vx;      
+    fp32 Vy;
+    fp32 Vw;
+    fp32 throttle;
+
+    fp32 x_l;
+    fp32 y_l;
 } Chassis_CMD_t;
 
-/* ÏÞλ */
-typedef struct {
-  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; 
+	// 四个舵轮的安装误差
+	typedef struct
+	{
+		fp32 MOTOR_OFFSET[4];
+	} MotorOffset_t;
 	
-  /* Â˲¨Æ÷ */
-  struct {
-    LowPassFilter2p_t in[4];  /* ·´À¡ÖµÂ˲¨Æ÷ */
-    LowPassFilter2p_t out[4]; /* Êä³öÖµÂ˲¨Æ÷ */
-  } filter;
+	typedef struct
+	{
+		fp32 TELESCOPE_MOTOR_OFFSET[2];
+	} TelescopeMotorOffset_t;
+	
+	typedef struct
+	{
+		/*可通过该枚举类型来决定Imu的数据量纲*/
+		enum
+		{
+			IMU_DEGREE, // 角度值（0-360）
+			IMU_RADIAN	// 弧度制（0-2pi)
+		} angle_mode;
 
-  Chassis_Output_t out; /* µç»úÊä³ö */
-	Chassis_Feedback_t feedback;
-	//float out_motor[4];
-} Chassis_t;
+//		AHRS_Eulr_t imu_eulr; // 解算后存放欧拉角（弧度制）
+	} ChassisImu_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);
+	/*底盘参数结构体*/
+	typedef struct
+	{
+		
+	
 
 
-/**
- * \brief ¸´ÖƵ×ÅÌÊä³öÖµ
- *
- * \param s °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
- * \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå
- */
-void Chassis_Output(Chassis_t *c);
+		struct{
+		/*该部分决定PID的参数整定在config中修改*/
+		KPID_Params_t Radder_DIR_Omega;
+		KPID_Params_t Radder_DIR_Angle;
+		KPID_Params_t Wheel_DIR_Omega;
+		KPID_Params_t chassis_follow_gimbal;
+		}pid;
+		
+		struct{
+		MOTOR_RM_Param_t Wheel_DIR[4]; // 四个轮向电机
+		MOTOR_RM_Param_t Radder_DIR[4]; // 四个舵向电机
+		MOTOR_RM_Param_t chassis_follow_gimbal; // 底盘跟随云台
 
-/**
- * \brief Çå¿ÕChassisÊä³öÊý¾Ý
- *
- * \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå
- */
-void Chassis_ResetOutput(Chassis_t *c);
-
-
-void Chassis_Power_Control(Chassis_t *c,float max_power);
-/**
- * @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);
+		}motor;
+		float mech_zero;/*云台6020的机械中点*/
+		float travel ;/*云台6020的机械行程*/
+		float mech_zero_4310;/*云台4310的机械中点*/
+	} Chassis_Param_t;
+
+	typedef struct
+	{
+		fp32 Vx;
+		fp32 Vy;
+		fp32 Vw;
+		fp32 mul; // 油门倍率
+	} ChassisMove_Vec;
+
+	typedef struct
+	{
+		float Wheel_DIR[4];
+		float Radder_DIR[4];
+	} Chassis_out_t;
+
+	typedef struct
+	{
+		
+		uint32_t last_wakeup;
+		float dt;
+		float chassis6020_detangle[4];
+		Chassis_Mode_t mode;
+		ChassisMove_Vec move_vec; // 最终输入速度
+		TrapezoidalProfile telescope_profile; 		
+		/* 设立角度和跟随角度 */
+		float Set_TelescopeAngle;
+		float Last_Set_TelescopeAngle;		
+		float Follow_TelescopeAngle;		
+		float feedfrward;
+		float Telescope_err;
+		/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
+		struct{
+		uint32_t last_wakeup;
+		float dt;
+		}accl_time;
+		
+		struct
+		{
+			fp32 Wheel_DIR_Solving_1[4];
+			fp32 Wheel_DIR_Solving_2[4];
+			fp32 Radder_DIR_Solving_1[4];
+			fp32 Radder_DIR_Solving_2[4];
+			// fp32 rotor6020_elur_yaw;
+
+			fp32 Radder_DIR_target[4];
+			fp32 Wheel_DIR_target[4];
+
+		} hopemotorout;
+
+		struct
+		{
+			fp32 final_Telescope;
+			fp32 final_Radder_DIR[4];
+			fp32 final_Wheel_DIR[4];
+		} final_out;
+		/* 原始数据 */
+		struct
+		{
+			struct{				
+
+				MOTOR_Feedback_t Wheel_DIR[4]; // 四个轮向电机
+				MOTOR_Feedback_t Radder_DIR[4];
+				MOTOR_Feedback_t gimbal_yaw;
+			}motor;
+			/* 转化数据 */
+			struct{	
+			
+				float Radder_DIR_Angle[4];				
+				float Radder_DIR_Omega[4];				
+				float Wheel_DIR_Angle[4];				
+				float Wheel_DIR_Omega[4];		
+			}motor_transformation;			
+			
+		} feedback;
+
+		struct
+		{	
+			KPID_t Radder_DIR_angle[4];
+			KPID_t Radder_DIR_omega[4];
+			KPID_t Wheel_DIR_omega[4];
+			KPID_t chassis_follow_gimbal_pid;
+		} pid;
+
+		uint8_t keeping_angle_flag; // 是否处于保持角度模式
+
+		// AHRS_Eulr_t set_point; // 底盘纠正目标角
+		// fp32 angle_current;	   // 当前角度
+		// fp32 angle_piancha;	   // 偏差角度
+		// fp32 yaw_out;		   // 角度pid输出值
+
+		ChassisImu_t pos088;		 // 088的实时姿态
+		MotorOffset_t motoroffset;	 // 5065校准数据
+		TelescopeMotorOffset_t telescope_motor_offset;
+		
+		
+		Chassis_Param_t *param;		 // 一些固定的参数
+		LowPassFilter2p_t filled[11]; // 低通滤波器
+		LowPassFilter2p_t accl_filled[2]; // 加速度滤波器		
+		float keep_angle[4];		 // 保持的 舵向 角度
+
+		Chassis_out_t out;
+
+		float wz_multi; /* 小陀螺模式旋转方向 */
+		float mech_zero;/*云台6020的机械中点*/
+		float travel ;/*云台6020的机械行程*/
+//		float mech_zero_4310;/*云台4310的机械中点*/
+	} Chassis_t;
 
 
+	int8_t chassis_init(Chassis_t *c,  Chassis_Param_t *param, float target_freq);
+	int8_t Chassis_update(Chassis_t *c);
+	int8_t Chassis_Control(Chassis_t *c,  Chassis_CMD_t *c_cmd,uint32_t now);
+	void Chassis_Setoutput(Chassis_t *c);
 #ifdef __cplusplus
 }
 #endif
-
-
-
-
diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c
index 0e5b980..fdaa7a2 100644
--- a/User/module/cmd/cmd.c
+++ b/User/module/cmd/cmd.c
@@ -16,15 +16,16 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
   CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
   
   /* 根据左拨杆位置选择模式 */
-  switch (ctx->input.rc.sw[0]) {
+  switch (ctx->input.rc.sw[1]) {
       case CMD_SW_UP:
           ctx->output.chassis.cmd.mode = map->sw_left_up;
           break;
       case CMD_SW_MID:
+           ctx->output.chassis.cmd.throttle = 1;
           ctx->output.chassis.cmd.mode = map->sw_left_mid;
           break;
       case CMD_SW_DOWN:
-          ctx->output.chassis.cmd.mode = map->sw_left_down;
+
           break;
       default:
           ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
@@ -32,9 +33,9 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
   }
   
   /* 摇杆控制移动 */
-  ctx->output.chassis.cmd.ctrl_vec.vx = ctx->input.rc.joy_right.x;
-  ctx->output.chassis.cmd.ctrl_vec.vy = ctx->input.rc.joy_right.y;
-  ctx->output.chassis.cmd.ctrl_vec.wz = ctx->input.rc.joy_left.x;
+   ctx->output.chassis.cmd.Vx =- ctx->input.rc.joy_right.y;
+   ctx->output.chassis.cmd.Vy = -ctx->input.rc.joy_right.x;
+
 }
 #endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS */
 
@@ -76,19 +77,19 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
   
   /* 根据右拨杆控制射击 */
   switch (ctx->input.rc.sw[1]) {
-      case CMD_SW_DOWN:
-          ctx->output.shoot.cmd.ready = true;
-          ctx->output.shoot.cmd.firecmd = true;
-          break;
-      case CMD_SW_MID:
-          ctx->output.shoot.cmd.ready = true;
-          ctx->output.shoot.cmd.firecmd = false;
-          break;
-      case CMD_SW_UP:
-      default:
-          ctx->output.shoot.cmd.ready = false;
-          ctx->output.shoot.cmd.firecmd = false;
-          break;
+      // case CMD_SW_DOWN:
+      //     ctx->output.shoot.cmd.ready = true;
+      //     ctx->output.shoot.cmd.firecmd = true;
+      //     break;
+      // case CMD_SW_MID:
+      //     ctx->output.shoot.cmd.ready = true;
+      //     ctx->output.shoot.cmd.firecmd = false;
+      //     break;
+      // case CMD_SW_UP:
+      // default:
+      //     ctx->output.shoot.cmd.ready = false;
+      //     ctx->output.shoot.cmd.firecmd = false;
+      //     break;
   }
 }
 #endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT */
@@ -140,8 +141,7 @@ static void CMD_PC_BuildChassisCmd(CMD_t *ctx) {
   ctx->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
   
   /* WASD控制移动 */
-  ctx->output.chassis.cmd.ctrl_vec.vx = 0.0f;
-  ctx->output.chassis.cmd.ctrl_vec.vy = 0.0f;
+
   CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_CHASSIS);
 }
 #endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS */
diff --git a/User/module/cmd/cmd_behavior.c b/User/module/cmd/cmd_behavior.c
index c7cdb46..6cfb64a 100644
--- a/User/module/cmd/cmd_behavior.c
+++ b/User/module/cmd/cmd_behavior.c
@@ -15,44 +15,44 @@
 /* 行为处理函数实现 */
 int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_CHASSIS
-    ctx->output.chassis.cmd.ctrl_vec.vy += ctx->config->sensitivity.move_sens;
+    ctx->output.chassis.cmd.Vy += ctx->config->sensitivity.move_sens;
 #endif
     return CMD_OK;
 }
 
 int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_CHASSIS
-    ctx->output.chassis.cmd.ctrl_vec.vy -= ctx->config->sensitivity.move_sens;
+    ctx->output.chassis.cmd.Vy -= ctx->config->sensitivity.move_sens;
 #endif
     return CMD_OK;
 }
 
 int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_CHASSIS
-    ctx->output.chassis.cmd.ctrl_vec.vx -= ctx->config->sensitivity.move_sens;
+    ctx->output.chassis.cmd.Vx -= ctx->config->sensitivity.move_sens;
 #endif
     return CMD_OK;
 }
 
 int8_t CMD_Behavior_Handle_RIGHT(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_CHASSIS
-    ctx->output.chassis.cmd.ctrl_vec.vx += ctx->config->sensitivity.move_sens;
+    ctx->output.chassis.cmd.Vx += ctx->config->sensitivity.move_sens;
 #endif
     return CMD_OK;
 }
 
 int8_t CMD_Behavior_Handle_ACCELERATE(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_CHASSIS
-    ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_fast_mult;
-    ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_fast_mult;
+    ctx->output.chassis.cmd.Vx *= ctx->config->sensitivity.move_fast_mult;
+    ctx->output.chassis.cmd.Vy *= ctx->config->sensitivity.move_fast_mult;
 #endif
     return CMD_OK;
 }
 
 int8_t CMD_Behavior_Handle_DECELERATE(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_CHASSIS
-    ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_slow_mult;
-    ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_slow_mult;
+    ctx->output.chassis.cmd.Vx *= ctx->config->sensitivity.move_slow_mult;
+    ctx->output.chassis.cmd.Vy *= ctx->config->sensitivity.move_slow_mult;
 #endif
     return CMD_OK;
 }
@@ -74,7 +74,7 @@ int8_t CMD_Behavior_Handle_FIRE_MODE(CMD_t *ctx) {
 int8_t CMD_Behavior_Handle_ROTOR(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_CHASSIS
   ctx->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
-  ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
+//  ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
 #endif
 #if CMD_ENABLE_MODULE_GIMBAL
   ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;
diff --git a/User/module/config.c b/User/module/config.c
index 46b987b..fdd4782 100644
--- a/User/module/config.c
+++ b/User/module/config.c
@@ -19,81 +19,51 @@
 // theta(变量), d(mm), a(mm), alpha(rad), theta_offset(rad)
 // 零位姿态: 机械臂竖直向上
 Config_RobotParam_t robot_config = {
-    .chassis_param = {
-    /* DJI3508µç»ú*/
-        .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	
-			},
+.chassis_param={
+    .pid={  
+		.Radder_DIR_Omega={
+		    .k=1.0f,
+        .p=0.5f,
+        .i=0.15f,
+        .d=0.0f,
+        .i_limit=1.0f,
+        .out_limit=1.0f,
+        .d_cutoff_freq= -1.0f,
+        .range=-1.0f
+		},
+		.Radder_DIR_Angle={
+        .k=0.15f,
+        .p=0.20f,
+        .i=0.1f,
+        .d=0.001f,
+        .i_limit=1.0f,
+        .out_limit=30.0f,
+        .d_cutoff_freq= -1.0f,
+        .range=360
+		},
+		.Wheel_DIR_Omega={
+        .k=0.5f,
+        .p=2.0f,
+        .i=0.1f,
+        .d=0.7f,
+        .i_limit=1.0f,
+        .out_limit=1.0f,
+        .d_cutoff_freq= -1.0f,
+        .range=-1.0f
+		},
     },
-		.type = CHASSIS_TYPE_MECANUM,
-    /* PID  */
-    .pid = {
-      /* µ×Å̵ç»ú PID */
-      .motor_pid_param = {
-        .k = 0.0015f,
-        .p = 1.0f,
-        .i = 0.0f,
-        .d = 0.0f,
-        .i_limit = 1.0f,
-        .out_limit = 1.0f,
-        .d_cutoff_freq = -1.0f,
-        .range = -1.0f,
-      },
+    .motor={
+    .Wheel_DIR[0]={BSP_CAN_2,0x203,MOTOR_M3508,false,false},
+    .Wheel_DIR[1]={BSP_CAN_2,0x204,MOTOR_M3508,true,false},
+    .Wheel_DIR[2]={BSP_CAN_2,0x202,MOTOR_M3508,true,false},
+    .Wheel_DIR[3]={BSP_CAN_2,0x201,MOTOR_M3508,false,false},
 
-      /* ¸úËæ */
-      .follow_pid_param = {
-        .k = 1.2f,
-        .p = 1.0f,
-        .i = 0.5f,
-        .d = 0.01f,
-        .i_limit = 1.0f,
-        .out_limit = 1.0f,
-        .d_cutoff_freq = -1.0f,
-        .range = M_2PI,
-      },
+    .Radder_DIR[0]={BSP_CAN_2,0x205,MOTOR_GM6020,false,false},
+    .Radder_DIR[1]={BSP_CAN_2,0x206,MOTOR_GM6020,false,false},
+    .Radder_DIR[2]={BSP_CAN_2,0x207,MOTOR_GM6020,false,false},
+    .Radder_DIR[3]={BSP_CAN_2,0x208,MOTOR_GM6020,false,false},
     },
-
-    .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 
-    },
-  },
+},
   .cmd_param={
         .source_priority = {
             CMD_SRC_RC,
@@ -107,8 +77,8 @@ Config_RobotParam_t robot_config = {
         },
         .rc_mode_map = {
             .sw_left_up   = CHASSIS_MODE_RELAX,
-            .sw_left_mid  = CHASSIS_MODE_RELAX,
-            .sw_left_down = CHASSIS_MODE_INDEPENDENT,
+            .sw_left_mid  = RC,
+           // .sw_left_down = ,
 
         },
     },
diff --git a/User/module/config.h b/User/module/config.h
index 9b1ce38..20dac4f 100644
--- a/User/module/config.h
+++ b/User/module/config.h
@@ -17,7 +17,7 @@ extern "C" {
 #include "component/PowerControl.h"
 typedef struct {
     // Arm_Params_t arm_param;
-    Chassis_Params_t chassis_param;
+    Chassis_Param_t chassis_param;
     CMD_Config_t cmd_param;
 } Config_RobotParam_t;
 
diff --git a/User/module/struct_typedef.h b/User/module/struct_typedef.h
new file mode 100644
index 0000000..01a08a9
--- /dev/null
+++ b/User/module/struct_typedef.h
@@ -0,0 +1,14 @@
+#ifndef STRUCT_TYPEDEF_H
+#define STRUCT_TYPEDEF_H
+
+#include <stdint.h>
+
+typedef unsigned char bool_t;
+typedef float fp32;
+typedef double fp64;
+
+////
+#endif
+
+
+
diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c
index cefd47b..fe7c19b 100644
--- a/User/task/ctrl_chassis.c
+++ b/User/task/ctrl_chassis.c
@@ -18,7 +18,7 @@
 /* USER STRUCT BEGIN */
 Chassis_t chassis;
 Chassis_CMD_t chassis_cmd;   
-Chassis_IMU_t chassis_imu;
+//Chassis_IMU_t chassis_imu;
 float max =50.0f;
 
 /* USER STRUCT END */
@@ -37,25 +37,20 @@ void Task_ctrl_chassis(void *argument) {
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
 
-  Config_RobotParam_t *cfg = Config_GetRobotParam();
-  Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ);
-  chassis.mech_zero=0.57f;
+   chassis_init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-    float encoder_gimbalYawMotor;
-    osMessageQueueGet(task_runtime.msgq.chassis.yaw, &encoder_gimbalYawMotor, NULL, 0);
-		osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);
-    chassis.feedback.encoder_gimbalYawMotor=encoder_gimbalYawMotor;
-    
-	  Chassis_UpdateFeedback(&chassis);
-    Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
+   
 
-    Chassis_Power_Control(&chassis,max);
+		osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);
+    Chassis_update(&chassis);   
+    Chassis_Control(&chassis, &chassis_cmd,tick);
+		Chassis_Setoutput(&chassis);	
     
-    Chassis_Output(&chassis);
+
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
diff --git a/User/task/init.c b/User/task/init.c
index 753cb25..7587311 100644
--- a/User/task/init.c
+++ b/User/task/init.c
@@ -33,7 +33,7 @@ void Task_Init(void *argument) {
   
   /* 创建任务线程 */
   task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
-  task_runtime.thread.arm_main = osThreadNew(Task_arm_main, NULL, &attr_arm_main);
+  //task_runtime.thread.arm_main = osThreadNew(Task_arm_main, NULL, &attr_arm_main);
   task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
   task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
   task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
diff --git a/ozone/arm.jdebug.user b/ozone/arm.jdebug.user
index 4efbc51..b42e2b9 100644
--- a/ozone/arm.jdebug.user
+++ b/ozone/arm.jdebug.user
@@ -32,7 +32,7 @@ OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=463, h=544, FilterBarShown=0
 OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="200 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;268", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1122;-71", CodeGraphLegendShown=0, CodeGraphLegendPosition="1138;0"
+OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="200 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;268", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1122;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1138;0"
 OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=0, 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=[120;144;639]
@@ -42,7 +42,7 @@ TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", Vi
 TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1334]
 TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[250;229;145;279;100]
 TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ik_test_ret";" ik_test_ret_analytical";" ik_test_ret_numerical";" ik_test_solver_dbg"], ColWidths=[100;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;100;100;100;134;110;126;126]
+TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;124;124;124;104;110;126;126]
 TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
 WatchedExpression="robot_arm", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="ik_from_fk_result", RefreshRate=5, Window=Watched Data 1