diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c
index 9847a76..7484776 100644
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@@ -68,7 +68,7 @@ custom_control_t CustomControl; //自定义控制器数据
 
 void RobotCMDInit() {
     rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
-    vision_recv_data = VisionInit(&huart1); // 视觉通信串口
+    //vision_recv_data = VisionInit(&huart1); // 视觉通信串口
 
     referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
     referee_VT_data = VTInit(&huart1);
@@ -96,7 +96,7 @@ void RobotCMDInit() {
     };
     cmd_can_comm = CANCommInit(&comm_conf);
 #endif // GIMBAL_BOARD
-    gimbal_cmd_send.pitch = 40;
+    //gimbal_cmd_send.pitch = PITCH_MIN;
 
     robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
 }
@@ -106,26 +106,26 @@ void RobotCMDInit() {
  *        单圈绝对角度的范围是0~360,说明文档中有图示
  *
  */
-static void CalcOffsetAngle() {
-    // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
-    static float angle;
-    angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
-#if YAW_ECD_GREATER_THAN_4096                               // 如果大于180度
-    if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
-        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
-    else if (angle > 180.0f + YAW_ALIGN_ANGLE)
-        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE - 360.0f;
-    else
-        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
-#else // 小于180度
-    if (angle > YAW_ALIGN_ANGLE)
-        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
-    else if (angle <= YAW_ALIGN_ANGLE && angle >= YAW_ALIGN_ANGLE - 180.0f)
-        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
-    else
-        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f;
-#endif
-}
+//static void CalcOffsetAngle() {
+//    // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
+//    static float angle;
+//    angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
+//#if YAW_ECD_GREATER_THAN_4096                               // 如果大于180度
+//    if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
+//        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
+//    else if (angle > 180.0f + YAW_ALIGN_ANGLE)
+//        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE - 360.0f;
+//    else
+//        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
+//#else // 小于180度
+//    if (angle > YAW_ALIGN_ANGLE)
+//        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
+//    else if (angle <= YAW_ALIGN_ANGLE && angle >= YAW_ALIGN_ANGLE - 180.0f)
+//        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
+//    else
+//        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f;
+//#endif
+//}
 
 static void death_check()
 {
@@ -145,6 +145,7 @@ static void update_ui_data() {
 
     ui_data.xyz[1] = to_stretch_fetch_data.protract_x;
     ui_data.xyz[2] = to_stretch_fetch_data.lift_z;
+    ui_data.diff_pitch_angle = gimbal_fetch_data.diff_pitch_feedback;
 }
 
 
@@ -165,24 +166,25 @@ static void RemoteControlSet() {
 
 
 
-    if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],底盘运动,伸缩保持不动
+    if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],底盘运动
     {
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP;
-        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+        gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE;
         // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度
         chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
         chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
         chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转
-        to_stretch_cmd_send.tc += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//图传
+        to_stretch_cmd_send.tc =  3.0F * (float) rc_data[TEMP].rc.rocker_l1;//图传
         // 图传限位
-        if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE;
-        if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE;
+//        if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE;
+//        if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE;
     } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩运动
     {
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
-        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+        gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE;
+        to_stretch_cmd_send.tc = 0;
 
         chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
         chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
@@ -197,47 +199,42 @@ static void RemoteControlSet() {
 
     // 右侧开关状态为[下],机械臂
     if (switch_is_down(rc_data[TEMP].rc.switch_right)) {
-        // 左侧开关状态为[下],前两轴
+        to_stretch_cmd_send.tc = 0;
+        // 左侧开关状态为[下],大臂 + 龙门架 + 底盘平移
         if(switch_is_down(rc_data[TEMP].rc.switch_left))
         {
-            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-            gimbal_cmd_send.pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1;
-            gimbal_cmd_send.yaw += 0.001f * (float) rc_data[TEMP].rc.rocker_r_;
+            gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE;
+            to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
+            gimbal_cmd_send.pitch = 0.001f * (float) rc_data[TEMP].rc.rocker_l1; //增量控制
+
+            to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_r_;//抬升
+            to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_r1;//前伸
 
         }
-        // 左侧开关状态为[中],后三轴
+//        // 左侧开关状态为[中],后三轴
+//        if(switch_is_mid(rc_data[TEMP].rc.switch_left))
+//        {
+//            gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE;
+//            gimbal_cmd_send.roll += 0.001f * (float) rc_data[TEMP].rc.rocker_l_;
+//            gimbal_cmd_send.diff_pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1;
+//            gimbal_cmd_send.diff_roll += 0.001f * (float) rc_data[TEMP].rc.rocker_r_;
+//        }
+        // 左侧开关状态为[中],逆解模式
         if(switch_is_mid(rc_data[TEMP].rc.switch_left))
-        {
-            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-            gimbal_cmd_send.roll += 0.001f * (float) rc_data[TEMP].rc.rocker_l_;
-            gimbal_cmd_send.diff_pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1;
-            gimbal_cmd_send.diff_roll += 0.001f * (float) rc_data[TEMP].rc.rocker_r_;
-        }
-        // 左侧开关状态为[上],逆解模式
-        if(switch_is_up(rc_data[TEMP].rc.switch_left))
         {
             gimbal_cmd_send.gimbal_mode = GIMBAL_IKINE_MODE;
-            gimbal_cmd_send.x_add = 1e-6f * (float) rc_data[TEMP].rc.rocker_r_;
-            gimbal_cmd_send.y_add = 1e-6f * (float) rc_data[TEMP].rc.rocker_r1;
-            gimbal_cmd_send.z_add = 1e-6f * (float) rc_data[TEMP].rc.rocker_l1;
-
-            //自定义控制器控制末端姿态
-            for (int i = 0; i < 4; ++i) {
-                gimbal_cmd_send.quat[i] = (float)(CustomControl.q[i])*1e-4f;
-            }
-
-            gimbal_cmd_send.pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1;
-            gimbal_cmd_send.yaw += 0.001f * (float) rc_data[TEMP].rc.rocker_r_;
-
+            gimbal_cmd_send.pitch = 0.001f * (float) rc_data[TEMP].rc.rocker_l1; //增量控制
+            chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
         }
     }
-    // 云台软件限位
-    gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch,PITCH_MIN,PITCH_MAX);
-    gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw,-YAW,YAW);
-    gimbal_cmd_send.roll = float_constrain(gimbal_cmd_send.roll,-ROLL,ROLL);
 
-    gimbal_cmd_send.diff_pitch = float_constrain(gimbal_cmd_send.diff_pitch,-DIFF_PITCH,DIFF_PITCH);
-    gimbal_cmd_send.diff_roll = float_constrain(gimbal_cmd_send.diff_roll,-DIFF_ROLL,DIFF_ROLL);
+    //自定义控制器控制末端姿态
+    for (int i = 0; i < 4; ++i) {
+        gimbal_cmd_send.quat[i] = (float)(CustomControl.q[i])*1e-4f;
+    }
+
+    QuaternionToEularAngle(gimbal_cmd_send.quat,&gimbal_cmd_send.rpy[2],&gimbal_cmd_send.rpy[1],&gimbal_cmd_send.rpy[0]);
+
 
 }
 
@@ -246,25 +243,9 @@ static void RemoteControlSet() {
  *
  */
 static void MouseKeySet() {
-    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 3000 - rc_data[TEMP].key[KEY_PRESS].s * 3000; // 系数待测
-    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 3000 - rc_data[TEMP].key[KEY_PRESS].d * 3000;
-    chassis_cmd_send.wz = (float) rc_data[TEMP].mouse.x / 660 * 4;
-
-    to_stretch_cmd_send.tc -= (float) rc_data[TEMP].mouse.y / 660 * 6;
-
-    to_stretch_cmd_send.ud += rc_data[TEMP].key[KEY_PRESS].q * 10 - rc_data[TEMP].key[KEY_PRESS].e * 10;
-    to_stretch_cmd_send.fb += rc_data[TEMP].key[KEY_PRESS].z * 10 - rc_data[TEMP].key[KEY_PRESS].c * 10;
-
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
-    {
-        case 1:
-            MyUIInit();
-            rc_data[TEMP].key_count[KEY_PRESS][Key_R]++;
-            break;
-        default:
-            break;
-    }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F按键开关气泵
+    chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+    to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 2) //G按键开关气泵
     {
         case 0:
             Pump_stop(air_pump);
@@ -275,17 +256,113 @@ static void MouseKeySet() {
             ui_data.pump_mode = PUMP_OPEN;
             break;
     }
-//    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
+    switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_B] % 2) //  CTRL + B键手动刷新UI
+    {
+        case 1:
+            MyUIInit();
+            rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_B]++;
+            break;
+        default:
+            break;
+    }
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_X] % 2) // X 取银矿模式
+    {
+        case 1:
+            gimbal_cmd_send.gimbal_mode = GIMBAL_SILVER_MODE; //取银矿姿态
+            rc_data[TEMP].key_count[KEY_PRESS][Key_X]++;
+            break;
+        default:
+            break;
+    }
+    switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_Z] % 2) // ctrl + Z 取地矿模式
+    {
+        case 1:
+            gimbal_cmd_send.gimbal_mode = GIMBAL_FLOOR_MODE; //取银矿姿态
+            rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_Z]++;
+            break;
+        default:
+            break;
+    }
+
+//    switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_X] % 2) // ctrl+x 调整零点
 //    {
-//        case 0:
-//            chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
-//            gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
+//        case 1:
+//            gimbal_cmd_send.gimbal_mode = ADJUST_PITCH_MODE; //调整pitch零点
+//            rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_X]++;
 //            break;
 //        default:
-//            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-//            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
 //            break;
 //    }
+
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V 开启自定义控制器
+    {
+        case 1:
+            gimbal_cmd_send.gimbal_mode = GIMBAL_IKINE_MODE; //自定义
+            rc_data[TEMP].key_count[KEY_PRESS][Key_V]++;
+            break;
+        default:
+            break;
+    }
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) //B 切手动
+    {
+        case 0:
+            gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE; //自定义
+            rc_data[TEMP].key_count[KEY_PRESS][Key_B]++;
+            break;
+        default:
+            break;
+    }
+
+    //银矿  吸盘朝前
+    if (gimbal_cmd_send.gimbal_mode == GIMBAL_SILVER_MODE)
+    { gimbal_cmd_send.rpy[0] = 0; gimbal_cmd_send.rpy[1] = -180; gimbal_cmd_send.rpy[2] = 0;}
+
+    //地矿  吸盘朝下
+    if (gimbal_cmd_send.gimbal_mode == GIMBAL_FLOOR_MODE)
+        { gimbal_cmd_send.rpy[0] = 0; gimbal_cmd_send.rpy[1] = -90; gimbal_cmd_send.rpy[2] = 0;}
+    //存矿  矿仓角度
+    if (gimbal_cmd_send.gimbal_mode == GIMBAL_STORAGE_MODE)
+    { gimbal_cmd_send.rpy[0] = 0; gimbal_cmd_send.rpy[1] = -30; gimbal_cmd_send.rpy[2] = 0;}
+
+    //手动模式  可以用遥控器调节末端姿态
+    if (gimbal_cmd_send.gimbal_mode == GIMBAL_MANUAL_MODE)
+    {   //增量控制
+        gimbal_cmd_send.roll = 0.001f * (float) rc_data[TEMP].rc.rocker_l_;
+        gimbal_cmd_send.diff_pitch = -0.001f * (float) rc_data[TEMP].rc.rocker_r1;
+        gimbal_cmd_send.diff_roll = 0.001f * (float) rc_data[TEMP].rc.rocker_r_;
+    }
+
+    //自定义控制器 控制末端姿态
+    if (gimbal_cmd_send.gimbal_mode == GIMBAL_IKINE_MODE)
+    {//自定义控制器控制末端姿态
+
+        if(rc_data[TEMP].key[KEY_PRESS].ctrl)
+        {
+            for (int i = 0; i < 4; ++i) {
+                gimbal_cmd_send.quat[i] = (float)(CustomControl.q[i])*1e-4f;
+            }
+            QuaternionToEularAngle(gimbal_cmd_send.quat,&gimbal_cmd_send.rpy[2],&gimbal_cmd_send.rpy[1],&gimbal_cmd_send.rpy[0]);
+            //ctrl按下调整姿态
+        } else
+        {
+            to_stretch_cmd_send.fb += CustomControl.dx * 0.04f;
+            to_stretch_cmd_send.ud += CustomControl.dz * 0.15f;
+        }
+
+    }
+    else
+    {
+        to_stretch_cmd_send.ud += (float)(rc_data[TEMP].key[KEY_PRESS].q - rc_data[TEMP].key[KEY_PRESS].e) * 0.15f;
+        to_stretch_cmd_send.fb += (float)(rc_data[TEMP].key[KEY_PRESS].z - rc_data[TEMP].key[KEY_PRESS].c) * 0.3f;
+    }
+
+    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 8000 - rc_data[TEMP].key[KEY_PRESS].s * 8000; // 系数待测
+    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 8000 - rc_data[TEMP].key[KEY_PRESS].d * 8000;
+    chassis_cmd_send.wz = -(float) rc_data[TEMP].mouse.x * 8;
+
+    to_stretch_cmd_send.tc = -(float) rc_data[TEMP].mouse.y * 20;
+
+    gimbal_cmd_send.pitch = (float)(rc_data[TEMP].key[KEY_PRESS].r  - rc_data[TEMP].key[KEY_PRESS].f) * 0.25f; //增量控制
 }
 
 /**
@@ -328,13 +405,29 @@ void RobotCMDTask() {
     SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
     update_ui_data();
     // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
-    CalcOffsetAngle();
+    //CalcOffsetAngle();
     // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
-//    if (switch_is_down(rc_data[TEMP].rc.switch_left) ||
-//        switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制
+    if (switch_is_down(rc_data[TEMP].rc.switch_left) ||
+        switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制
         RemoteControlSet();
-//    else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
-//        MouseKeySet();
+    else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
+        MouseKeySet();
+
+// 云台软件限位
+//    gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch,PITCH_MIN,PITCH_MAX);
+//    gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw,-YAW,YAW);
+//    gimbal_cmd_send.roll = float_constrain(gimbal_cmd_send.roll,-ROLL,ROLL);
+//
+//    gimbal_cmd_send.diff_pitch = float_constrain(gimbal_cmd_send.diff_pitch,-DIFF_PITCH,DIFF_PITCH);
+//    gimbal_cmd_send.diff_roll = float_constrain(gimbal_cmd_send.diff_roll,-DIFF_ROLL,DIFF_ROLL);
+
+    //抬升软件限位
+    if(to_stretch_cmd_send.ud <= UD_MIN) to_stretch_cmd_send.ud = UD_MIN;
+    if(to_stretch_cmd_send.ud >= UD_MAX) to_stretch_cmd_send.ud = UD_MAX;
+
+    //前伸软件限位
+    if(to_stretch_cmd_send.fb <= FB_MIN) to_stretch_cmd_send.fb = FB_MIN;
+    if(to_stretch_cmd_send.fb >= FB_MAX) to_stretch_cmd_send.fb = FB_MAX;
 
     EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
 
@@ -349,6 +442,5 @@ void RobotCMDTask() {
 #endif // GIMBAL_BOARD
     PubPushMessage(to_stretch_cmd_pub, (void *) &to_stretch_cmd_send);
     PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send);
-    VisionSend(&vision_send_data);
 }
 
diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp
index b9f3652..d377300 100644
--- a/application/gimbal/gimbal.cpp
+++ b/application/gimbal/gimbal.cpp
@@ -44,6 +44,10 @@ static float pitch_spd,yaw_spd,roll_spd;
 first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter;
 static float diff_r_spd,diff_l_spd;
 
+first_order_filter_type_t Qset_filter[3]; //关节目标值滤波
+
+float q_set[5] ; //五个关节的目标值
+
 //机械臂参数初始化
 //float arm_q[5] = {0}; // 机械臂各关节位置
 robotics::Link link[5];
@@ -83,9 +87,9 @@ void GimbalInit()
             .controller_param_init_config = {
                     .other_speed_feedback_ptr = &yaw_spd,
                     .speed_PID = {
-                            .Kp = 3.5f,//5,
+                            .Kp = 0.0f,//3.5f,//5,
                             .Ki = 0.0f,
-                            .Kd = 0.02f,
+                            .Kd = 0,//0.02f,
                             .MaxOut = 10,
                             .Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
                             .IntegralLimit = 10.0F,
@@ -109,10 +113,7 @@ void GimbalInit()
             .motor_type = DM6006,
     };
 
-    yaw_motor_config.can_init_config.can_handle = &hcan2;
-    yaw_motor_config.can_init_config.tx_id = 1;
-    yaw_motor_config.can_init_config.rx_id = 2;
-    yaw_motor = DMMotorInit(&yaw_motor_config);
+
 
     Motor_Init_Config_s pitch_motor_config = {
             .controller_param_init_config = {
@@ -120,8 +121,6 @@ void GimbalInit()
                     //.current_feedforward_ptr = &arm_gravity_feedforward,
             },
             .controller_setting_init_config = {
-//                    .outer_loop_type = ANGLE_LOOP,
-//                    .close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
                     .outer_loop_type = OPEN_LOOP,
                     .close_loop_type = OPEN_LOOP,
                     .angle_feedback_source = MOTOR_FEED,
@@ -131,10 +130,7 @@ void GimbalInit()
             .motor_type = DM6006,
     };
 
-    pitch_motor_config.can_init_config.can_handle = &hcan2;
-    pitch_motor_config.can_init_config.tx_id = 0x03;
-    pitch_motor_config.can_init_config.rx_id = 0x04;
-    pitch_motor_l = DMMotorInit(&pitch_motor_config);
+
 
     PID_Init_Config_s pitch_spd_config= {
             .Kp = 2.5f,
@@ -160,15 +156,15 @@ void GimbalInit()
             .controller_param_init_config = {
                     .other_speed_feedback_ptr = &roll_spd,
                     .speed_PID = {
-                            .Kp = 0.8f,
-                            .Ki = 0.3f,
+                            .Kp = 0.2f,
+                            .Ki = 0.0f,
                             .Kd = 0.02f,
                             .MaxOut = 10,
-                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
+                            .Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
                             .IntegralLimit = 10.0F,
                     },
                     .angle_PID = {
-                            .Kp = 10.0f,
+                            .Kp = 25.0f,
                             .Ki = 0.0f,
                             .Kd = 0.0f,
                             .MaxOut = 10,
@@ -186,17 +182,14 @@ void GimbalInit()
             .motor_type = DM4310,
     };
 
-    roll_motor_config.can_init_config.can_handle = &hcan2;
-    roll_motor_config.can_init_config.tx_id = 5;
-    roll_motor_config.can_init_config.rx_id = 6;
-    roll_motor = DMMotorInit(&roll_motor_config);
+
 
     Motor_Init_Config_s diff_motor_config = {
             .controller_param_init_config = {
                     .speed_PID = {
-                            .Kp = 0.2f,//0.6f,
-                            .Ki = 0,//0.1f,
-                            .Kd = 0.02,//0.02f,
+                            .Kp = 0.6f,
+                            .Ki = 0.1f,
+                            .Kd = 0.02f,
                             .MaxOut = 10,
                             .Improve = static_cast<PID_Improvement_e>( PID_Integral_Limit | PID_Derivative_On_Measurement),
                             .IntegralLimit = 10.0F,
@@ -213,17 +206,7 @@ void GimbalInit()
             .motor_type = DM4310,
     };
 
-    diff_motor_config.can_init_config.can_handle = &hcan2;
-    diff_motor_config.can_init_config.tx_id = 7;
-    diff_motor_config.can_init_config.rx_id = 8;
-    diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd;
-    diff_r_motor = DMMotorInit(&diff_motor_config);
 
-    diff_motor_config.can_init_config.can_handle = &hcan2;
-    diff_motor_config.can_init_config.tx_id = 9;
-    diff_motor_config.can_init_config.rx_id = 10;
-    diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd;
-    diff_l_motor = DMMotorInit(&diff_motor_config);
 
     const float spd_filter_num = 0.05f;
     first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num);
@@ -236,8 +219,13 @@ void GimbalInit()
     first_order_filter_init(&diff_r_spd_filter,5e-3,&spd_filter_num_diff);
     first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff);
 
+    const float Qset_filter_num = 0.1f;
+    first_order_filter_init(&Qset_filter[0],5e-3,&Qset_filter_num);
+    first_order_filter_init(&Qset_filter[1],5e-3,&Qset_filter_num);
+    first_order_filter_init(&Qset_filter[2],5e-3,&Qset_filter_num);
+
     PID_Init_Config_s diff_pitch_config= {
-            .Kp = 20.0F,//15.0f,
+            .Kp = 20.0f,//20.0F,//15.0f,
             .Ki = 0,
             .Kd = 0,
             .MaxOut = 10,
@@ -246,16 +234,6 @@ void GimbalInit()
     };
     PIDInit(&diff_pitch_loop,&diff_pitch_config);
 
-    PID_Init_Config_s diff_pitch_spd_config= {
-            .Kp = 0.6f,
-            .Ki = 2.0f,
-            .Kd = 0.01f,
-            .MaxOut = 10,
-            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
-            .IntegralLimit = 100,
-    };
-    PIDInit(&diff_pitch_spd_loop,&diff_pitch_spd_config);
-
     PID_Init_Config_s diff_roll_config= {
             .Kp = 14.0f,
             .Ki = 0,
@@ -266,15 +244,36 @@ void GimbalInit()
     };
     PIDInit(&diff_roll_loop,&diff_roll_config);
 
-    PID_Init_Config_s diff_roll_spd_config= {
-            .Kp = 1.0f,
-            .Ki = 1.0f,
-            .Kd = 0.02f,
-            .MaxOut = 10,
-            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
-            .IntegralLimit = 100,
-    };
-    PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config);
+
+    //需要按id顺序初始化 否则有电机不能正确运行
+
+    //决定放弃yaw轴
+//    yaw_motor_config.can_init_config.can_handle = &hcan2;
+//    yaw_motor_config.can_init_config.tx_id = 1;
+//    yaw_motor_config.can_init_config.rx_id = 2;
+//    yaw_motor = DMMotorInit(&yaw_motor_config);
+
+    pitch_motor_config.can_init_config.can_handle = &hcan2;
+    pitch_motor_config.can_init_config.tx_id = 3;
+    pitch_motor_config.can_init_config.rx_id = 4;
+    pitch_motor_l = DMMotorInit(&pitch_motor_config);
+
+    roll_motor_config.can_init_config.can_handle = &hcan2;
+    roll_motor_config.can_init_config.tx_id = 5;
+    roll_motor_config.can_init_config.rx_id = 6;
+    roll_motor = DMMotorInit(&roll_motor_config);
+
+    diff_motor_config.can_init_config.can_handle = &hcan2;
+    diff_motor_config.can_init_config.tx_id = 7;
+    diff_motor_config.can_init_config.rx_id = 8;
+    diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd;
+    diff_r_motor = DMMotorInit(&diff_motor_config);
+
+    diff_motor_config.can_init_config.can_handle = &hcan2;
+    diff_motor_config.can_init_config.tx_id = 9;
+    diff_motor_config.can_init_config.rx_id = 10;
+    diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd;
+    diff_l_motor = DMMotorInit(&diff_motor_config);
 
     pitch_motor_config.can_init_config.tx_id = 11; //按顺序初始化！！！
     pitch_motor_config.can_init_config.rx_id = 12;
@@ -285,8 +284,17 @@ void GimbalInit()
     gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
     gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
 
+//    //上电校准两个4310零点
+//    DMMotorCaliEncoder(diff_r_motor);
+//    DMMotorCaliEncoder(diff_l_motor);
+////水平方向3.02 矿仓 0
+//    DMMotorCaliEncoder(pitch_motor_l);
+
+    q_set[1] = PITCH_MIN * DEGREE_2_RAD; //上电给pitch轴赋值
 
 }
+
+//#define PITCH_OFFSET 3.02F //pitch轴水平时编码器值
 //static void DMMotroEnable()
 //{
 //    if(gimbal_cmd_recv.MotorEnableFlag)
@@ -301,16 +309,16 @@ void GimbalInit()
 
 /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
 
-//float debug_diff_kp = 0.1f,debug_diff_ki=0,debug_diff_kd = 0;
+float debug_diff_kp = 0.3f,debug_diff_ki=0,debug_diff_kd = 0.01f;
 void GimbalTask()
 {
-//    diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
-//    diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
-//    diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
-//
-//    diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
-//    diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
-//    diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
+    diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
+    diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
+    diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
+
+    diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
+    diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
+    diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
     // 获取云台控制数据
     // 后续增加未收到数据的处理
     SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
@@ -337,8 +345,8 @@ void GimbalTask()
     /* 控制参数计算 ------------------------------------------------------------------------*/
 
     //正运动学
-    arm_q[0][0] = yaw_motor->measure.position;
-    arm_q[1][0] = -pitch_motor_l->measure.position;
+    arm_q[0][0] = 0;//yaw_motor->measure.position;
+    arm_q[1][0] = -(pitch_motor_l->measure.position);
     arm_q[2][0] = roll_motor->measure.position;
     arm_q[3][0] = diff_pitch_angle;
     arm_q[4][0] = diff_roll_angle;
@@ -366,30 +374,25 @@ void GimbalTask()
         DMMotorEnable(diff_r_motor);DMMotorEnable(diff_l_motor);
     }
 
-    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_GYRO_MODE) //各关节分开控制
+//    if(gimbal_cmd_recv.gimbal_mode == ADJUST_PITCH_MODE)
+//    {
+//        //校准两个4310零点
+//        DMMotorCaliEncoder(diff_r_motor);
+//        DMMotorCaliEncoder(diff_l_motor);
+//    }
+
+    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_MANUAL_MODE) //各关节分开控制
     {
-        float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD);
-        float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, gimbal_cmd_recv.diff_roll * DEGREE_2_RAD);
-
-        float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out;
-        float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out;
-
-        //pitch轴双环PID
-        float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,gimbal_cmd_recv.pitch * DEGREE_2_RAD);
-        float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward;
-
-        DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2);
-        DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2);
-        DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD);
-        DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD);
-
-        DMMotorSetRef(diff_r_motor,r_speed_set);
-        DMMotorSetRef(diff_l_motor,-l_speed_set);
+        q_set[0] = 0;
+        q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
+        q_set[2] += gimbal_cmd_recv.roll * DEGREE_2_RAD;
+        q_set[3] += gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD;
+        q_set[4] += gimbal_cmd_recv.diff_roll * DEGREE_2_RAD;
     }
-    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE)
+    if(gimbal_cmd_recv.gimbal_mode != GIMBAL_MANUAL_MODE && gimbal_cmd_recv.gimbal_mode != GIMBAL_ZERO_FORCE)
     {
-        static float q_set[5] ; //记录五个关节的目标值
-        if(last_gimbal_mode == GIMBAL_GYRO_MODE)
+
+        if(last_gimbal_mode == GIMBAL_MANUAL_MODE)
         {
             //切换至逆解模式时，目标值设置为当前值
             cmd_R = robotics::t2r( fk_T);
@@ -397,67 +400,102 @@ void GimbalTask()
         for (int i = 0; i < 4; ++i) {
             cmd_quat[i][0] = gimbal_cmd_recv.quat[i];
         }
-        float trans_rpy_data[3] = {-PI,0,0};
-        Matrixf<3,1> trans_rpy(trans_rpy_data);
-            cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans_rpy); //转到基座坐标系下的姿态
-//        T_cmd = robotics::p2t(cmd_xyz);
+
+        if(gimbal_cmd_recv.gimbal_mode == GIMBAL_SILVER_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_FLOOR_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_STORAGE_MODE)
+        {
+            float trans_rpy_data[3] = {gimbal_cmd_recv.rpy[0]*DEGREE_2_RAD,gimbal_cmd_recv.rpy[1]*DEGREE_2_RAD,gimbal_cmd_recv.rpy[2]*DEGREE_2_RAD};
+            Matrixf<3,1> trans_rpy(trans_rpy_data);
+            cmd_R = robotics::rpy2r(trans_rpy);
+        }
+        if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE)
+        {
+            float trans_data[3] = {0,-PI,0};
+            Matrixf<3,1> trans(trans_data);
+            cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans); //转到基座坐标系下的姿态
+        }
+
+
+
 
         ik_q3 = robotics::spherical_wrist_ikine(cmd_R,arm_q[0][0],arm_q[1][0]);
 
             //后三关节的误差 选用误差小的一组解
-            float err[2] = {0};
+            float err[2][3] = {0};
             Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0);
             for (int i = 0; i < 2; ++i) {
                 Matrixf<3,1> ik = ik_q3.block<3,1>(0,i);
-                err[i]  = (ik - arm_q3).norm();
+                for (int j = 0; j < 3; ++j) {
+                    err[i][j] = loop_float_constrain(ik[j][0] - arm_q3[j][0],-PI,PI);
+                }
             }
-
-//            if (err[1] >= err[0])
+            float err1 = abs(err[1][0]) + abs(err[1][1]) + abs(err[1][2]);
+            float err0 = abs(err[0][0]) + abs(err[0][1]) + abs(err[0][2]);
+            //选用 总 误差小的一组解
+//            if (err1 >= err0)
 //                ik_q3_cmd = ik_q3.block<3,1>(0,0);
 //            else
 //                ik_q3_cmd = ik_q3.block<3,1>(0,1);
 
-//            //选用 roll误差小的一组解
-            if (abs(ik_q3[0][0] - arm_q3[0][0]) <= abs(ik_q3[0][1]- arm_q3[0][0]))
+//            // 选用 roll 误差小的一组解
+            float roll_err1 = ik_q3[0][0] - arm_q3[0][0];
+            roll_err1 = loop_float_constrain(roll_err1,-PI,PI);
+
+            float roll_err2 = ik_q3[1][0] - arm_q3[0][0];
+            roll_err2 = loop_float_constrain(roll_err2,-PI,PI);
+
+
+            if (abs(roll_err1) <= abs(roll_err2))
                 ik_q3_cmd = ik_q3.block<3,1>(0,0);
             else
                 ik_q3_cmd = ik_q3.block<3,1>(0,1);
+            //选用 第一组解
+        //ik_q3_cmd = ik_q3.block<3,1>(0,0);
+            ik_q3_cmd[0][0] += loop_float_constrain(ik_q3_cmd[0][0] - arm_q3[0][0],-PI,PI);
+            ik_q3_cmd[1][0] += loop_float_constrain(ik_q3_cmd[1][0] - arm_q3[1][0],-PI,PI);
+            ik_q3_cmd[2][0] += loop_float_constrain(ik_q3_cmd[2][0] - arm_q3[2][0],-PI,PI);
 
             ik_q3_cmd[0][0] = float_constrain(ik_q3_cmd[0][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);
-
             //ik_q3_cmd[1][0] = float_constrain(ik_q3_cmd[1][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
             ik_q3_cmd[2][0] = float_constrain(ik_q3_cmd[2][0],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);
 
             for (int i = 0; i < 3; ++i) {
-                q_set[i] = ik_q3_cmd[i][0];
+                first_order_filter_cali(&Qset_filter[i],ik_q3_cmd[i][0]);
+                q_set[i+2] = Qset_filter[i].out;
             }
+            //大臂pitch用遥控器数据
+        q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
 
 
-
-        float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[1]);
-        float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, q_set[2]);
-
-        float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out;
-        float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out;
-
-        //pitch轴双环PID pitch yaw先遥控器控制
-        float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,gimbal_cmd_recv.pitch * DEGREE_2_RAD);
-        float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward;
-
-        DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2);
-        DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2);
-        DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD);
-
-        DMMotorSetRef(roll_motor,q_set[0]);
-
-        DMMotorSetRef(diff_r_motor,r_speed_set);
-        DMMotorSetRef(diff_l_motor,-l_speed_set);
-
     }
     //保存上次模式
     last_gimbal_mode = gimbal_cmd_recv.gimbal_mode;
 
+    q_set[1] = float_constrain(q_set[1],PITCH_MIN* DEGREE_2_RAD,PITCH_MAX* DEGREE_2_RAD);
 
+    q_set[2] = float_constrain(q_set[2],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);
+
+    q_set[3] = float_constrain(q_set[3],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
+    q_set[4] = float_constrain(q_set[4],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);
+
+
+    float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[3]);
+    float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, q_set[4]);
+
+    float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out;
+    float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out;
+
+    //pitch轴双环PID pitch yaw先遥控器控制
+    float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,q_set[1]);
+    float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward;
+
+    DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2);
+    DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2);
+    DMMotorSetRef(yaw_motor,q_set[0]);
+
+    DMMotorSetRef(roll_motor,q_set[2]);
+
+    DMMotorSetRef(diff_r_motor,r_speed_set);
+    DMMotorSetRef(diff_l_motor,-l_speed_set);
 
 
 
@@ -468,7 +506,7 @@ void GimbalTask()
 
     // 设置反馈数据,主要是imu和yaw的ecd
     gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
-    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
+    gimbal_feedback_data.diff_pitch_feedback = diff_pitch_angle * RAD_2_DEGREE;
 
     // 推送消息
     PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
diff --git a/application/robot.c b/application/robot.c
index 4619b61..7f8c403 100644
--- a/application/robot.c
+++ b/application/robot.c
@@ -32,11 +32,11 @@ void RobotInit()
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDInit();
     GimbalInit();
-    //To_stretchInit();
+    To_stretchInit();
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
-    //ChassisInit();
+    ChassisInit();
 #endif
 
     OSTaskInit(); // 创建基础任务
@@ -50,11 +50,11 @@ void RobotTask()
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDTask();
     GimbalTask();
-    //To_stretchTask();
+    To_stretchTask();
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
-    //ChassisTask();
+    ChassisTask();
 #endif
 
 }
\ No newline at end of file
diff --git a/application/robot_def.h b/application/robot_def.h
index 211637e..edd962c 100644
--- a/application/robot_def.h
+++ b/application/robot_def.h
@@ -29,18 +29,18 @@
 #define DIFF_PITCH 90  //差动手腕关节
 #define DIFF_ROLL 180
 
-#define ROLL 90
-#define PITCH_MIN (-160.0f) //注意此处和坐标系是反的
+#define ROLL 170
+#define PITCH_MIN (-174.0f) //注意此处和坐标系是反的
 #define PITCH_MAX (40.0F)
 
 #define YAW 60
 
 //龙门架限位
-#define UD_MAX 100
+#define UD_MAX 210
 #define UD_MIN 0
 
 #define FB_MIN 0
-#define FB_MAX 100
+#define FB_MAX 205
 
 // 云台参数
 #define YAW_CHASSIS_ALIGN_ECD 0  // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改
@@ -112,14 +112,15 @@ typedef enum
 {
     GIMBAL_ZERO_FORCE = 0, // 电流零输入
     GIMBAL_FREE_MODE,      // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
-    GIMBAL_GYRO_MODE,      // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
+    GIMBAL_MANUAL_MODE,      // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
     GIMBAL_IKINE_MODE,     // 机械臂运动学逆解模式，输入6d-tof位置
+    GIMBAL_FLOOR_MODE,     // 地矿模式
     GIMBAL_SILVER_MODE,     // 取银矿模式
     GIMBAL_GOLD_MODE,       // 取金矿模式
     GIMBAL_STORAGE_MODE,    // 放入矿石仓
     GIMBAL_FETCH_MODE,      // 取出矿石
 
-    ADJUST_PITCH_MODE,      //找pitch的限位
+    ADJUST_PITCH_MODE,      // 调整pitch零点
 } gimbal_mode_e;
 //气泵模式
 typedef enum
@@ -171,6 +172,7 @@ typedef struct
     float z_add;
 
     float quat[4]; //末端姿态四元数
+    float rpy[3];  //末端姿态rpy角
 
     float chassis_rotate_wz;
     uint8_t MotorEnableFlag;
@@ -210,8 +212,7 @@ typedef struct
 typedef struct
 {
     attitude_t gimbal_imu_data;
-    float yaw_motor_single_round_angle;
-    float pitch_motor_total_angle;
+    float diff_pitch_feedback;
 } Gimbal_Upload_Data_s;
 
 typedef struct
diff --git a/application/to_stretch/to_stretch.c b/application/to_stretch/to_stretch.c
index 136b4d3..e419951 100644
--- a/application/to_stretch/to_stretch.c
+++ b/application/to_stretch/to_stretch.c
@@ -162,8 +162,8 @@ void To_stretchInit() {
             .controller_setting_init_config = {
                     .angle_feedback_source = MOTOR_FEED,
                     .speed_feedback_source = MOTOR_FEED,
-                    .outer_loop_type = ANGLE_LOOP,
-                    .close_loop_type =  ANGLE_LOOP | SPEED_LOOP,
+                    .outer_loop_type = SPEED_LOOP,
+                    .close_loop_type =  SPEED_LOOP,
                     .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
             },
             .motor_type = M2006
@@ -248,6 +248,7 @@ void To_stretchTask()
             DJIMotorSetRef(motor_ru, -ud_speed_set);
             DJIMotorSetRef(motor_ld, -fb_speed_set);
             DJIMotorSetRef(motor_rd, fb_speed_set);
+            DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
             break;
         case TO_STRETCH_KEEP:
             DJIMotorEnable(motor_lu);
diff --git a/engineering.jdebug b/engineering.jdebug
index 1a5c097..708c732 100644
--- a/engineering.jdebug
+++ b/engineering.jdebug
@@ -33,6 +33,7 @@ void OnProjectLoad (void) {
   //
   // User settings
   //
+  Edit.SysVar (VAR_HSS_SPEED, FREQ_500_HZ);
   Project.SetOSPlugin ("FreeRTOSPlugin_CM4");
   File.Open ("$(ProjectDir)/cmake-build-debug/basic_framework.elf");
 }
diff --git a/engineering.jdebug.user b/engineering.jdebug.user
index 247246d..b889ff7 100644
--- a/engineering.jdebug.user
+++ b/engineering.jdebug.user
@@ -2,16 +2,18 @@
 
 
 Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:280:5, State=BP_STATE_DISABLED
-OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=52
-OpenDocument="referee_task.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_task.c", Line=221
+GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f
+GraphedExpression="(lift_position_loop).Measure", Color=#35792b
+OpenDocument="controller.c", FilePath="D:/CLion/Project/engineering/modules/algorithm/controller.c", Line=141
+OpenDocument="referee_task.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_task.c", Line=202
 OpenDocument="referee_UI.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.c", Line=9
 OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=70
-OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=36
-OpenDocument="controller.c", FilePath="D:/CLion/Project/engineering/modules/algorithm/controller.c", Line=141
+OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=270
+OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=108
 OpenDocument="referee_UI.h", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.h", Line=0
 OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107
 OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=213
-OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=9
+OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=52
 OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=316
 OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=29
 OpenDocument="referee_VT.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_VT.c", Line=59
@@ -22,16 +24,16 @@ OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/CLion/Project/engineering/Middl
 OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1412
 OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering/application/chassis/chassis.c", Line=165
 OpenToolbar="Debug", Floating=0, x=0, y=0
-OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=367, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=264, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=360, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=271, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=687, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=979, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=611, h=632, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="710;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="724;0"
+OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=611, h=632, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="312;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0"
 OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=252, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
 TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;889]
-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=[341;100;100;100;100;100;100;107;478]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (lift_position_loop).Ref";" (lift_position_loop).Measure"], ColWidths=[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=[341;100;100;110;100;100;100;107;107]
 TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
 TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
 TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;177;104;100;100]
@@ -63,4 +65,8 @@ WatchedExpression="yaw_motor", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="(((yaw_motor)->motor_controller).speed_PID).Ref", Window=Watched Data 1
 WatchedExpression="(((yaw_motor)->motor_controller).angle_PID).Ref", Window=Watched Data 1
 WatchedExpression="yaw_spd_filter", Window=Watched Data 1
-WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1
\ No newline at end of file
+WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="tuchuan", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="lift_position_loop", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="rc_data[TEMP].key_count[KEY_PRESS][Key_Shift]", Window=Watched Data 1
+WatchedExpression="rc_data", Window=Watched Data 1
\ No newline at end of file
diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c
index 2d1086b..c441537 100644
--- a/modules/motor/DMmotor/dmmotor.c
+++ b/modules/motor/DMmotor/dmmotor.c
@@ -39,7 +39,13 @@ static void DMMotorDecode(CANInstance *motor_can)
     DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
     DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
 
-    DaemonReload(motor->motor_daemon);
+//    DaemonReload(motor->motor_daemon);
+    //0 失能  1 使能  8 超压 9 欠压  A 过流  B MOS过压  C 电机线圈过温   D 通信丢失  E 过载
+    tmp =  (uint8_t)(rxbuff[0]>>4);
+    measure->state = tmp;
+    if(measure->state){
+        DaemonReload(motor->motor_daemon);
+    }
 
     measure->last_position = measure->position;
 
@@ -71,6 +77,10 @@ static void DMMotorLostCallback(void *motor_ptr)
     DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
     uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
     LOGWARNING("[dm_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
+
+    DMMotorEnable(motor);
+    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
+    LOGWARNING("[dm_motor] Tring to restart motor, can bus [%d] , id [%d]\"", can_bus, motor->motor_can_instance->tx_id);
 }
 void DMMotorCaliEncoder(DMMotorInstance *motor)
 {
diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h
index 3d26871..310713d 100644
--- a/modules/motor/DMmotor/dmmotor.h
+++ b/modules/motor/DMmotor/dmmotor.h
@@ -8,8 +8,8 @@
 
 #define DM_MOTOR_CNT 6
 
-#define DM_P_MIN  (-12.5f)//(-3.1415926f)
-#define DM_P_MAX  12.5f//3.1415926f
+#define DM_P_MIN  (-12.56637)//(-3.1415926f)
+#define DM_P_MAX  12.56637//3.1415926f
 #define DM_V_MIN  (-45.0f)
 #define DM_V_MAX  45.0f
 #define DM_T_MIN  (-18.0f)
diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c
index f2eac6e..17c281e 100644
--- a/modules/motor/motor_task.c
+++ b/modules/motor/motor_task.c
@@ -10,7 +10,7 @@ void MotorControlTask()
     // static uint8_t cnt = 0; 设定不同电机的任务频率
     // if(cnt%5==0) //200hz
     // if(cnt%10==0) //100hz
-    //DJIMotorControl();
+    DJIMotorControl();
 
     /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
     //LKMotorControl();
diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c
index 0c790cc..125e707 100644
--- a/modules/referee/referee_task.c
+++ b/modules/referee/referee_task.c
@@ -54,7 +54,6 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int
 
 void UITask()
 {
-    RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
     MyUIRefresh(referee_recv_info, Interactive_data);
     xyz_refresh(referee_recv_info, Interactive_data);
 }
@@ -62,6 +61,7 @@ void UITask()
 static String_Data_t UI_State_sta[7];  // 机器人状态,静态只需画一次
 static String_Data_t UI_State_dyn[7];  // 机器人状态,动态先add才能change
 static Graph_Data_t UI_State_dyn_graph[2];
+static Graph_Data_t UI_angle[2];
 
 #define AXIS_LEN 250
 #define AXIS_X0 550
@@ -79,9 +79,16 @@ void MyUIInit()
     UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
 
     // 绘制车辆状态标志指示
-    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 10, 750, "F.AIR_PUMP:");
+    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 750, "F.AIR_PUMP:");
+    UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 700, "ARM_MODE:");
+
+    UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 650, "PITCH:");
+
 
     UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
+    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
+    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
+
     // xyz坐标系 静态
     UILineDraw(&xyz_axis[0], "x_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0);
     UILineDraw(&xyz_axis[1], "y_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0+AXIS_LEN);
@@ -100,116 +107,75 @@ void MyUIInit()
     // 绘制车辆状态标志，动态
     // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
 
-    //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743);
-    UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 350, 720, 10);
+    UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_ADD, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
+    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
 
+    UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 350, 800, 10);
     UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]);
-//    UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
-//    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
 
-}
+    // 底盘功率显示,动态
+    UIFloatDraw(&UI_angle[0], "pitch_angle", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 2, 230, 650, Interactive_data->diff_pitch_angle * 1000);
+    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_angle[0]);
+
 
-// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
-static uint8_t count = 0;
-static uint16_t count1 = 0;
-static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测试用函数，实现模式自动变化
-{
-    count++;
-    if (count >= 50)
-    {
-        count = 0;
-        count1++;
-    }
-    switch (count1 % 4)
-    {
-    case 0:
-    {
-        _Interactive_data->chassis_mode = CHASSIS_ZERO_FORCE;
-        _Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
-    }
-    case 1:
-    {
-        _Interactive_data->chassis_mode = CHASSIS_ROTATE;
-        _Interactive_data->gimbal_mode = GIMBAL_FREE_MODE;
-        break;
-    }
-    case 2:
-    {
-        _Interactive_data->chassis_mode = CHASSIS_NO_FOLLOW;
-        _Interactive_data->gimbal_mode = GIMBAL_GYRO_MODE;
-        break;
-    }
-    case 3:
-    {
-        _Interactive_data->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
-        _Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
-        break;
-    }
-    default:
-        break;
-    }
 }
 
 static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
 {
     UIChangeCheck(_Interactive_data);
-    // chassis
-//    if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1)
-//    {
-//        switch (_Interactive_data->chassis_mode)
-//        {
-//        case CHASSIS_ZERO_FORCE:
-//            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
-//            break;
-//        case CHASSIS_ROTATE:
-//            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate   ");
-//            // 此处注意字数对齐问题，字数相同才能覆盖掉
-//            break;
-//        case CHASSIS_NO_FOLLOW:
-//            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow ");
-//            break;
-//        case CHASSIS_FOLLOW_GIMBAL_YAW:
-//            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow   ");
-//            break;
-//        }
-//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
-//        _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
-//    }
-//    // gimbal
-//    if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
-//    {
-//        switch (_Interactive_data->gimbal_mode)
-//        {
-//        case GIMBAL_ZERO_FORCE:
-//        {
-//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
-//            break;
-//        }
-//        case GIMBAL_FREE_MODE:
-//        {
-//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free     ");
-//            break;
-//        }
-//        case GIMBAL_GYRO_MODE:
-//        {
-//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro     ");
-//            break;
-//        }
-//        }
-//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
-//        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
-//    }
+    // 显示 pitch 轴编码器位置
+    UIFloatDraw(&UI_angle[0], "pitch_angle", UI_Graph_Change, 8, UI_Color_Green, 20, 2, 2, 230, 650, Interactive_data->diff_pitch_angle * 1000);
+    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_angle[0]);
+    // gimbal
+    if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
+    {
+        switch (_Interactive_data->gimbal_mode)
+        {
+        case GIMBAL_ZERO_FORCE:
+        {
+            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
+            break;
+        }
+        case GIMBAL_MANUAL_MODE:
+        {
+            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "MANUAL    ");
+            break;
+        }
+        case GIMBAL_SILVER_MODE:
+        {
+            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "SILVER    ");
+            break;
+        }
+        case GIMBAL_FLOOR_MODE:
+        {
+            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "FLOOR     ");
+            break;
+        }
+        case GIMBAL_IKINE_MODE:
+        {
+            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "CUSTOM    ");
+            break;
+        }
+        case GIMBAL_STORAGE_MODE:
+        {
+            UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "STORAGE    ");
+            break;
+        }
+        }
+        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
+        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
+    }
     // pump
     if (_Interactive_data->Referee_Interactive_Flag.pump_flag == 1)
     {
         switch (_Interactive_data->pump_mode) {
             case PUMP_STOP:
                 //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743);
-                UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 350, 720, 10);
+                UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 350, 800, 10);
                 break;
             case PUMP_OPEN:
                 //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 0, 330, 695, 330, 695);
-                UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Green, 10, 350, 720, 25);
+                UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Green, 10, 350, 800, 25);
                 break;
         }
         UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]);
@@ -247,8 +213,8 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
 
 static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
 {
-    const float y_max = 100;//单位mm
-    const float z_max = 100;//单位mm
+    const float y_max = FB_MAX;//单位mm
+    const float z_max = UD_MAX;//单位mm
 
     float y_ratio = _Interactive_data->xyz[1]/y_max;
     float z_ratio = _Interactive_data->xyz[2]/z_max;
@@ -256,6 +222,7 @@ static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_i
     UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0);
     UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+(AXIS_LEN*y_ratio) , AXIS_Y0+(AXIS_LEN*y_ratio));
     UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+(AXIS_LEN*z_ratio));
-    UIGraphRefresh(&referee_recv_info->referee_id,3,xyz_axis[3],xyz_axis[4],xyz_axis[5]);
+    UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[3],xyz_axis[4]);
+    UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[5]);
 
 }
diff --git a/modules/referee/rm_referee.h b/modules/referee/rm_referee.h
index a853152..7e7931f 100644
--- a/modules/referee/rm_referee.h
+++ b/modules/referee/rm_referee.h
@@ -95,6 +95,7 @@ typedef struct
 
     uint8_t aim_last_fire;
     float xyz[3];     // xyz三轴位置
+	float diff_pitch_angle;
 
 } Referee_Interactive_info_t;