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

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,8 +47,8 @@ typedef enum
 typedef struct DiffData
 {
     ControlMode mode ;               // 控制模式
-    MotorState state;                //当前车辆状态        
-    float desired_speed;             // 期望速度
+    MotorState motor_state[4];       //当前车辆状态    
+    float desired_speed;             // 期望速度    
     float desired_curvature;         // 期望曲率
     float left_motor_speed;          // 当前左电机速度
     float right_motor_speed;         // 当前右电机速度
@@ -69,7 +72,15 @@ typedef struct DiffData
     float out_right_motor_speed;     // 输出右电机速度
     float out_torq[4];               //4个电机扭矩
     float max_Torq;                 // 最大扭矩限制   
-    float min_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;