扭矩模式，增加同侧差速PID

app/app_brake.c

@@ -88,7 +88,6 @@ static void brakeTimerProcess(void *signal_id)
         if (shouldApplyBrake())
         {
             brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
-            brake_data.brake_motor_state = 1;
             if( 0 == brake_data.brake_direction)
             {
                brake_data.brake_motor_state = 1;               
@@ -106,8 +105,7 @@ static void brakeTimerProcess(void *signal_id)
         if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
         {
             brake_data.state = BRAKE_STATE_RELEASING_BRAKE;
-            brake_data.brake_motor_state = 2;
+
-            brakeOutput(NULL);
             if( 0 == brake_data.brake_direction)
             {
                brake_data.brake_motor_state = 2;               
@@ -116,6 +114,7 @@ static void brakeTimerProcess(void *signal_id)
             {
                brake_data.brake_motor_state = 1;                    
             }         
+            brakeOutput(NULL);
             timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
         }
         break;
@@ -151,8 +150,6 @@ static void brakeTimerProcess(void *signal_id)
         setParam("brk_pos", (float)brake_data.brake_position);
         brake_data.old_brake_position = brake_data.brake_position;
         
-        
-        
         printf("writeE2 brake_position = %d\n",brake_data.brake_position);
         
     }

app/app_differential_drive.c

@@ -19,7 +19,8 @@ DiffData diff_data;
 PID_t speed_pid;
 PID_t yaw_rate_pid;
 
-
+PID_t Acc_front_speed_pid;
+PID_t Dec_front_speed_pid;
 
 
 /**
@@ -52,8 +53,62 @@ float calculateTorqueOutput(uint8_t gear, float input_torque)
     return output_torque;
 }
 
+/**
+ * @brief 车辆状态控制状态机
+ * @note 根据车速和扭矩方向切换前进/后退状态，带扭矩回滞保护
+ * @param ctx 状态机上下文，包含当前状态(STATE_INIT/FORWARD/BACKWARD)
+ * @param speed 当前车速（单位：km/h），0表示静止状态
+ * @param torque 当前扭矩（单位：Nm），正数表示前进方向，负数表示后退方向
+ */
+void handleVehicleState(MotorState *ctx, float speed, float torque) 
+{
+    switch (*ctx) 
+    {
+        // 初始状态：根据扭矩方向初始化
+        case STATE_INIT:
+        {
+            if (torque >= 0.0f) 
+            {
+                *ctx = STATE_FORWARD;  // 正扭矩进前进档
+            }
+            else 
+            {
+                *ctx = STATE_BACKWARD; // 负扭矩进倒档
+            }
+            break;
+        }
 
+        // 前进状态：零速且反向扭矩超阈值切倒档
+        case STATE_FORWARD:
+        {
+            if ( (speed == 0.0f) && (torque <= -TORQUE_HYSTERESIS_THRESHOLD) ) 
+            {
+                *ctx = STATE_BACKWARD;  // 满足条件切换
+            }
+            else 
+            {
+                *ctx = STATE_FORWARD;   // 否则保持
+            }
+            break;
+        }
 
+        // 倒车状态：零速且正向扭矩超阈值切前进
+        case STATE_BACKWARD:
+        {
+            if ( (speed == 0.0f) && (torque >= TORQUE_HYSTERESIS_THRESHOLD) ) 
+            {
+                *ctx = STATE_FORWARD;   // 满足条件切换
+            }
+            else 
+            {
+                *ctx = STATE_BACKWARD;  // 否则保持
+            }
+            break;
+        }
+        
+        default:;  // 异常处理
+    }
+}
 
 
 // 设置电机输出
@@ -66,33 +121,19 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
     float abs_right_rear_speed  = 0;
     
     // 档位
-    if(diff_data.state != STATE_STATIC_TURN)
+     abs_left_front_speed  = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向
-    {
+     abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]);     
-        un_motor_output1.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡
+     abs_left_rear_speed   = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]);     
-        un_motor_output2.bit_data.gear = diff_data.state;
+     abs_right_rear_speed  = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]);     
-        un_motor_output3.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡
-        un_motor_output4.bit_data.gear = diff_data.state;    
-    }
-    else//原地转向状态的话，根据扭矩大小确定方向
-    {
-        un_motor_output1.bit_data.gear = (out_torq[0] < 0) ? STATE_BACKWARD : STATE_FORWARD;
-        un_motor_output2.bit_data.gear = (out_torq[1] < 0) ? STATE_BACKWARD : STATE_FORWARD;      
-        un_motor_output3.bit_data.gear = (out_torq[2] < 0) ? STATE_BACKWARD : STATE_FORWARD;      
-        un_motor_output4.bit_data.gear = (out_torq[3] < 0) ? STATE_BACKWARD : STATE_FORWARD;      
-    }
 
+     un_motor_output1.bit_data.gear = diff_data.motor_state[0];
+     un_motor_output2.bit_data.gear = diff_data.motor_state[1];
      
-     abs_left_front_speed  = calculateTorqueOutput(un_motor_output1.bit_data.gear, out_torq[0]); //根据挡位增加转矩方向
+    if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相
-     abs_right_front_speed = calculateTorqueOutput(un_motor_output2.bit_data.gear, out_torq[1]);     
-     abs_left_rear_speed   = calculateTorqueOutput(un_motor_output3.bit_data.gear, out_torq[2]);     
-     abs_right_rear_speed  = calculateTorqueOutput(un_motor_output4.bit_data.gear, out_torq[3]);     
-    
-     
-    if(STATE_FORWARD == un_motor_output3.bit_data.gear)//把后两台电机反相
     {
        un_motor_output3.bit_data.gear = STATE_BACKWARD;   
     }
-    else if(STATE_BACKWARD == un_motor_output3.bit_data.gear)
+    else if(STATE_BACKWARD == diff_data.motor_state[2])
     {
        un_motor_output3.bit_data.gear = STATE_FORWARD;    
     }
@@ -101,11 +142,11 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
        un_motor_output3.bit_data.gear = STATE_INIT;     
     } 
     
-    if(STATE_FORWARD == un_motor_output4.bit_data.gear)
+    if(STATE_FORWARD == diff_data.motor_state[3])
     {
        un_motor_output4.bit_data.gear = STATE_BACKWARD;   
     }
-    else if(STATE_BACKWARD == un_motor_output4.bit_data.gear)
+    else if(STATE_BACKWARD == diff_data.motor_state[3])
     {
        un_motor_output4.bit_data.gear = STATE_FORWARD;    
     }
@@ -382,13 +423,17 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
     }
 
 #if THROTTLE_PID_MODE
-    float max_torque = (float)getParam("maxTorq");
+  
+    float left_speed_mps  = 0.0f;  
+    float right_speed_mps = 0.0f;
+    
+    float max_torque = diff_data.max_Torq;
 
     linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
     yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
     
-    float left_speed_mps = linear_velocity_x + yaw_rate;
+    left_speed_mps = linear_velocity_x + yaw_rate;
-    float right_speed_mps = linear_velocity_x - yaw_rate;  
+    right_speed_mps = linear_velocity_x - yaw_rate;
 
 //扭矩分配		
     if(max_torque < left_speed_mps)
@@ -414,35 +459,24 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
     else{}
     
 //    printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);   
+//    adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);     
+//    adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);      
+//    printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);  
     
-    motor_speed[0] = left_speed_mps;
+    motor_speed[0] = left_speed_mps;//加速状态，没有负扭矩，要么前进加速要么后退加速
     motor_speed[2] = left_speed_mps; 
 
     motor_speed[1] = right_speed_mps;
     motor_speed[3] = right_speed_mps;  
 
-//    adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);     
+    handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed,   motor_speed[0]); //通过扭矩以及速度来判断挡位
-//    adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);      
+    handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed,  motor_speed[1]); 
-//    
+    handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed,    motor_speed[2]); 
-      
+    handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed,   motor_speed[3]); 
-//    printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);  
-    
-  
-    
-    
-    
-    
-    
-    
 
 //    distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq);
 //    distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq);  
     
-//     printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", motor_speed[0], motor_speed[1], motor_speed[2], motor_speed[3]);  
-    
-//    // 返回计算结果
-//    *left_motor_speed = left_speed_mps;
-//    *right_motor_speed = right_speed_mps;
 #else    
     
     // 限制线速度和偏航率
@@ -535,108 +569,6 @@ float mapRemoteControlSpeed(
     return output_speed;
 }
 
-/**
- * @brief 状态机处理函数（修改后版本）
- */
-void handleVehicleState(DiffData *ctx) 
-{
-    switch (ctx->state) 
-    {
-        //-------------------------------------------
-        // 初始状态：根据期望速度方向跳转
-        //-------------------------------------------
-        case STATE_INIT:
-        {
-            if (ctx->desired_speed < 0.0f) 
-            {
-                ctx->state = STATE_BACKWARD;
-            }
-            else if( (ctx->desired_speed == 0.0f) && (ctx->desired_curvature != 0) )//无速度有转向
-            {
-                ctx->state = STATE_STATIC_TURN;
-            }
-            else 
-            {
-                ctx->state = STATE_FORWARD;
-            }
-            break;
-        }
-
-        //-------------------------------------------
-        // 前进状态：处理反向指令（新增else分支）
-        //-------------------------------------------
-        case STATE_FORWARD:
-        {
-            if( (ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f) ) 
-            {
-                ctx->state = STATE_BACKWARD;  // 零速时允许切换方向
-            } 
-            else if( (ctx->desired_speed < 0.0f) && (ctx->speed != 0.0f) ) 
-            {
-                ctx->desired_speed = 0.0f;    // 非零速时清空期望速度
-                ctx->state = STATE_FORWARD;   // 显式保持当前状态
-            }
-            else if( (ctx->desired_speed == 0.0f) && (ctx->speed == 0.0f) && (ctx->desired_curvature != 0.0f) )
-            {
-                ctx->state = STATE_STATIC_TURN;   // 原地转向                
-            }
-            else 
-            {
-                ctx->state = STATE_FORWARD;    // 新增：其他情况保持前进状态
-            }
-            break;
-        }
-
-        //-------------------------------------------
-        // 倒车状态：处理正向指令（新增else分支）
-        //-------------------------------------------
-        case STATE_BACKWARD:
-        {
-            if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f)) 
-            {
-                ctx->state = STATE_FORWARD;   // 零速时允许切换方向
-            } 
-            else if ((ctx->desired_speed > 0.0f) && (ctx->speed != 0.0f)) 
-            {
-                ctx->desired_speed = 0.0f;    // 非零速时清空期望速度
-                ctx->state = STATE_BACKWARD;  // 显式保持当前状态
-            }
-            else if( (ctx->desired_speed == 0.0f) && (ctx->speed == 0.0f) && (ctx->desired_curvature != 0.0f) )
-            {
-                ctx->state = STATE_STATIC_TURN;   // 原地转向                
-            }            
-            else 
-            {
-                ctx->state = STATE_BACKWARD;   // 新增：其他情况保持倒车状态
-            }
-            break;
-        }
-        
-         //-------------------------------------------
-        // 原地转向状态
-        //-------------------------------------------
-        case STATE_STATIC_TURN:
-        {
-            if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f)) 
-            {
-                ctx->state = STATE_FORWARD;   // 零速时允许切换方向
-            } 
-            else if ((ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f)) 
-            {
-                ctx->state = STATE_BACKWARD;  // 显式保持当前状态
-            }
-            else 
-            {
-                ctx->state = STATE_STATIC_TURN;   // 原地转向
-            }
-            break;
-        } 
-        
-        default:;
-    }
-}
-
-
 
 
 // 差速处理函数
@@ -666,22 +598,16 @@ static void diffProcess(void *signal_id)
         diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed;
     }
     
-    handleVehicleState(&diff_data); //20250704 换挡函数  速度为0才能换挡
-    
-    
 //    printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
     // 使用 PID 控制器计算输出速度和曲率
     float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt);
     float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt);
     
-    
-    
     // 计算最大加速度，用函数计算
     float max_acceleration = calculateMaxAcceleration();
     // 限制输出速度在当前速度和最大加速度计算出来的速度之间
     // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt);
 
-    
     if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0 
     {
         resetPidIntegral(&speed_pid);
@@ -690,37 +616,67 @@ static void diffProcess(void *signal_id)
         output_yaw_rate = 0;
     }   
     
+        // 使用差速车辆动力学模型计算左右电机的期望速度
+    float out_torque[4] = {0,0,0,0};
+    // 使用差速车辆动力学模型计算左右电机的期望速度
+    computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque);
+   
+    
+    if( ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍
+    {
+        diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; 
+        diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID           
+    }
+    else
+    {
+        diff_data.left_speed_diff = 0;
+        Acc_front_speed_pid.integral = 0;
+        diff_data.left_diff_touue = 0;
+    }
+    
+    
+    if( ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍
+    {
+        diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed; 
+        diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID
+    }
+    else
+    {
+        diff_data.right_speed_diff = 0;
+        Dec_front_speed_pid.integral = 0;
+        diff_data.right_diff_touue = 0;
+    }
+    
+    if(out_torque[0] > 0)
+    {
+        diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -2*out_torque[0], 2*out_torque[0]);
+    }
+    else
+    {
+        diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, 2*out_torque[0], -2*out_torque[0]);       
+    }
+    
+    
+    if(out_torque[1] > 0)
+    {
+        diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -2*out_torque[1], 2*out_torque[1]);
+    }
+    else
+    {
+        diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, 2*out_torque[1], -2*out_torque[1]);       
+    }    
+    
+    
+    diff_data.out_torq[0] = (2*out_torque[0] + diff_data.left_diff_touue)/2.0f;//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。
+    diff_data.out_torq[2] = (2*out_torque[0] - diff_data.left_diff_touue)/2.0f;    
+    
+    diff_data.out_torq[1] = (2*out_torque[1] + diff_data.right_diff_touue)/2.0f;
+    diff_data.out_torq[3] = (2*out_torque[1] - diff_data.right_diff_touue)/2.0f;      
+    
+    
 //    printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);    
     
     
-//    if(diff_data.desired_yaw_rate != 0)//有转向的情况下下
-//    {
-//        if( (output_yaw_rate > -500) && (output_yaw_rate < 500) )//如果是转向输出在-500~500之间，那么开始原地转向扭矩太小，所以设定最小扭矩。
-//        {
-//           output_yaw_rate = 500;
-//        }
-//    }
-    
-    
-    // 使用差速车辆动力学模型计算左右电机的期望速度
-    computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]);
-    
-    
-    
-    
-    
-    
-    
-//     if( (left_speed < 200) && (left_speed > -200) )
-//     {
-//       left_speed = 0;
-//     }
-//     
-//     if( (right_speed < 200) && (right_speed > -200) )
-//     {
-//       right_speed = 0;
-//     }      
-    
     
     // 设置电机输出
     setMotorOutput(&diff_data.out_torq[0], 
@@ -744,8 +700,8 @@ static void diffProcess(void *signal_id)
     un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10);
     un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10);
     
-    un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_motor_speed);
+    un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_speed_diff);
-    un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_motor_speed);    
+    un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff);    
     
     publishMessage(&diff_data, 1);     
     
@@ -800,7 +756,7 @@ static void diffInput(void *signal_id)
             diff_data.desired_speed = diff_data.desired_speed * 0.01f;
             diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
             // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
-            diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);         
+            diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 1, 20, 5, 5, 0.5);         
             diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5);
             
             diff_data.desired_curvature = -diff_data.desired_curvature;
@@ -843,8 +799,6 @@ static void diffInput(void *signal_id)
         diff_data.left_rear_motor_speed  = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量
         diff_data.left_rear_motor_speed  = - diff_data.left_rear_motor_speed;//20250708 增加反相   
         
-        
-        
         if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
         {
            motor_speed_temp = diff_data.left_front_motor_speed;
@@ -902,8 +856,20 @@ static void diffInput(void *signal_id)
 //    {
 //       diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
 //    }
-     
+    if((power_data.current_state == POWER_WORKING))//电机上电才运行
+    {
        diffProcess(&diff_data);//计算左右电机期望转速
+    }
+    else
+    {
+        resetPidIntegral(&speed_pid);
+        resetPidIntegral(&yaw_rate_pid);
+        diff_data.motor_state[0] = STATE_INIT;
+        diff_data.motor_state[1] = STATE_INIT;
+        diff_data.motor_state[2] = STATE_INIT;
+        diff_data.motor_state[3] = STATE_INIT;        
+    }
+
 }
 
 
@@ -969,7 +935,37 @@ void diffParametersInit(void *signal_id)
                           getParam("crv_ol")
          );       
     }
+    
+    // 设置曲率 PID 控制器的参数
+     setPidParameters(&Dec_front_speed_pid,
+                      getParam("mot_kp"),
+                      getParam("mot_ki"),
+                      getParam("mot_kd"),
+                      getParam("mot_il"),
+                      getParam("mot_ol")
+     );     
+     
+    // 设置曲率 PID 控制器的参数
+     setPidParameters(&Acc_front_speed_pid,
+                      Dec_front_speed_pid.kp,
+                      Dec_front_speed_pid.ki,
+                      Dec_front_speed_pid.kd,
+                      Dec_front_speed_pid.integral_limit,
+                      Dec_front_speed_pid.output_limit    
+     );   
+     
     diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩   
+    diff_data.max_Torq = (float)getParam("maxTorq"); 
+    
+    if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。
+    {
+       diff_data.diff_dead_zone = 2;
+    }
+    else
+    {
+       diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩       
+    }   
+    
     
     
     printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
@@ -987,12 +983,13 @@ void diffParametersInit(void *signal_id)
     deffspeed = deffspeed * 0.01f;
     deffcurvature = deffcurvature * 0.0001f;   
     
-    printf(" car state = %d\n", diff_data.state);       
+    printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature);    
+    
+    printf(" car state = %d,%d,%d,%d\n", diff_data.motor_state[0],diff_data.motor_state[1],diff_data.motor_state[2],diff_data.motor_state[3]);       
      
     timerStart(&diff_app_timer,1000,1);//1s调用一次 
 }
 
-
 // 差速初始化函数
 void diffAppInit(void)
 {
@@ -1032,6 +1029,29 @@ void diffAppInit(void)
                       getParam("crv_ol")
      );
 
+    // 初始化减速 PID 控制器
+    initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
+    // 设置 PID 控制器的参数
+     setPidParameters(&Dec_front_speed_pid,
+                      getParam("mot_kp"),
+                      getParam("mot_ki"),
+                      getParam("mot_kd"),
+                      getParam("mot_il"),
+                      getParam("mot_ol")
+     );     
+     
+    // 初始化加速 PID 控制器
+    initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
+    // 设置 PID 控制器的参数
+     setPidParameters(&Acc_front_speed_pid,
+                      Dec_front_speed_pid.kp,
+                      Dec_front_speed_pid.ki,
+                      Dec_front_speed_pid.kd,
+                      Dec_front_speed_pid.integral_limit,
+                      Dec_front_speed_pid.output_limit
+     );            
+     
+     
     subscribe(&diff_app_timer, diffParametersInit);
     timerStart(&diff_app_timer,1000,1);//1s调用一次 
 

app/app_differential_drive.h

@@ -14,11 +14,15 @@ extern "C"
 #define  SPEED_PID_MODE        0
 #define  THROTTLE_PID_MODE     1  
   
-  
+#define  TURN_MIN_TOUQUE       1   //n*m
 
 #define  SPEED_MODE          0x01
 #define  TORQUE_MODE         0x02   
   
+  
+#define  TORQUE_HYSTERESIS_THRESHOLD           0.3f   
+  
+  
 #define  MOTOR_MODE          TORQUE_MODE  
   
 
@@ -32,7 +36,6 @@ typedef enum
     STATE_INIT,        ///< 初始状态（转速为0且等待扭矩方向判定）
     STATE_FORWARD,     ///< 前进
     STATE_BACKWARD,    ///< 后退
-    STATE_STATIC_TURN  ///< 原地转向
 } MotorState;  
   
 typedef enum
@@ -44,7 +47,7 @@ typedef enum
 typedef struct DiffData
 {
     ControlMode mode ;               // 控制模式
-    MotorState state;                //当前车辆状态        
+    MotorState motor_state[4];       //当前车辆状态    
     float desired_speed;             // 期望速度    
     float desired_curvature;         // 期望曲率
     float left_motor_speed;          // 当前左电机速度
@@ -70,6 +73,14 @@ typedef struct DiffData
     float out_torq[4];               //4个电机扭矩
     float max_Torq;                 // 最大扭矩限制   
     float min_Torq;                 // 最小扭矩限制 
+    
+    float left_speed_diff;            // 左侧转速差   
+    float right_speed_diff;           // 右侧转速差
+    
+    float left_diff_touue;            // 左侧扭矩差   
+    float right_diff_touue;           // 右侧扭矩差      
+    float diff_dead_zone;             // 差速速度死区     
+    
 } DiffData;
 
 

app/app_param_manage.h

@@ -53,6 +53,12 @@ extern "C"
     X(Ocrv_ol)  \
     X(minTorq)  \
     X(brk_rev)  \
+    X(mot_kp)  \
+    X(mot_ki)  \
+    X(mot_kd)  \
+    X(mot_il)  \
+    X(mot_ol)  \
+    X(diff_sp) \
     X(test)
 
 // 定义一个包含所有参数名称的结构体

interface_can.c

@@ -323,7 +323,7 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle,
                     un_motor_input3.arr[i] = buf->dataBuffer[i];
                 }
                 //<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
-                publishMessage(&un_motor_input3, 1);               
+//                publishMessage(&un_motor_input3, 1);    //<2F>޸<EFBFBD>Ϊ           
             } 
             else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) )
             {