修改电机驱动

app/app_differential_drive.h

@@ -8,37 +8,34 @@ extern "C"
 
 #include "app_config.h"
 
-#define  SPEED_MODE          0x0C
-#define  THROTTLE_MODE       0x05 
-#define  TORQUE_MODE         0x0A   
+#define  SPEED_MODE          0x01
+#define  TORQUE_MODE         0x02    
   
-#define  MOTOR_MODE          THROTTLE_MODE
+#define  MOTOR_MODE          TORQUE_MODE
 
 #define  SPEED_PID_MODE        0
 #define  THROTTLE_PID_MODE     1  
   
-  
-  
-  
+#define  TORQUE_HYSTERESIS_THRESHOLD           0.3f     
 
 #define  SWITCH_SPEED          20.0f
   
-  
 #define  DIFF_SPEED            20.0f  
   
 
+// 状态机内部状态
 typedef enum
 {
     STATE_INIT,        ///< 初始状态（转速为0且等待扭矩方向判定）
-    STATE_FORWARD,     ///< 正向旋转状态
-    STATE_BACKWARD,    ///< 反向旋转状态
-    DEC_FORWARD,       ///< 正向减速状态
-    DEC_FORWARD2,       ///< 正向溜坡状态
-    DEC_BACKWARD,      ///< 反向减速状态
-    DEC_BACKWARD2      ///< 反向溜坡状态
-  
+    STATE_FORWARD,     ///< 前进
+    STATE_BACKWARD,    ///< 后退
 } MotorState;  
   
+typedef enum
+{
+    MODE_MANUAL, // 手动模式
+    MODE_AUTO    // 自动模式    
+} ControlMode;
   
 // 状态机内部状态
 typedef enum 
@@ -55,67 +52,47 @@ typedef enum
 } MotorDir;  
 
 
-
-typedef enum
-{
-    MODE_MANUAL, // 手动模式
-    MODE_AUTO    // 自动模式    
-} ControlMode;
-
 typedef struct DiffData
 {
     ControlMode mode ;               // 控制模式
-    MotorState state;                //当前状态机状态    
-    MotorDir   motor_dir;           // 当前车辆方向  
-    DirState   dir_state;          
-    
-    float desired_speed;             // 期望速度
+    MotorState motor_state[4];       //当前车辆状态    
+    float desired_speed;             // 期望速度    
     float desired_curvature;         // 期望曲率
     float left_motor_speed;          // 当前左电机速度
     float right_motor_speed;         // 当前右电机速度
-
-    float left_front_motor_speed;     // 当前左前电机速度    
-    float right_front_motor_speed;    // 当前右前电机速度 
-    float left_rear_motor_speed;      // 当前左后电机速度    
-    float right_rear_motor_speed;     // 当前右后电机速度
-    
-    uint8_t out_motor_dir;            // 电机状态    
-    
-    float speed_last;                 // 速度过去值   
-    float torq_last;                  // 扭矩过去值 
-    uint8_t counter;                  // 方向判断计数器   
+    float left_front_motor_speed;    // 当前左前电机速度
+    float right_front_motor_speed;   // 当前右前电机速度    
+    float left_rear_motor_speed;     // 当前左后电机速度
+    float right_rear_motor_speed;    // 当前右后电机速度     
+    float speed;                     // 当前车速
+    float curvature;                 // 当前曲率
+    float yaw_rate;                  // 当前角速度
+    float desired_yaw_rate;          // 期望角速度
+    float acceleration;              // 当前加速度
+    float deceleration;              // 当前减速度
+    float max_speed;                 // 最大速度
+    float desired_acceleration;      // 期望加速度
+    float desired_deceleration;      // 期望减速度
+    uint8_t emergency_stop_switch;   // 急停开关
+    uint8_t remote_emergency_stop;   // 遥控器急停开关
+    uint8_t emergency_stop_state;    // 急停状态
+    float out_left_motor_speed;      // 输出左电机速度
+    float out_right_motor_speed;     // 输出右电机速度
+    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 right_diff_touue;           // 右侧扭矩差      
     float diff_dead_zone;             // 差速速度死区     
-  
-    
-    float out_motor_speed[4];
-    float speed;                      // 当前车速
-    float curvature;                  // 当前曲率
-    float yaw_rate;                   // 当前角速度
-    float desired_yaw_rate;           // 期望角速度
-    float acceleration;               // 当前加速度
-    float deceleration;               // 当前减速度
-    float max_speed;                  // 最大速度
-    float desired_acceleration;       // 期望加速度
-    float desired_deceleration;       // 期望减速度
-    uint8_t emergency_stop_switch;    // 急停开关
-    uint8_t remote_emergency_stop;    // 遥控器急停开关
-    uint8_t emergency_stop_state;     // 急停状态
-    float out_left_motor_speed;       // 输出左电机速度
-    float out_right_motor_speed;      // 输出右电机速度
-    uint16_t max_Torq;                // 最大扭矩限制  
-    uint16_t speed_inter;
-    
     
 } DiffData;
 
 
+
 // 声明外部变量
 extern DiffData diff_data;