第一次提交

app/app_differential_drive.h

@@ -0,0 +1,96 @@
+#ifndef APP_DIFFERENTIAL_DRIVE_H
+#define APP_DIFFERENTIAL_DRIVE_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "app_config.h"
+
+#define  SPEED_FITER_NUM       6  
+  
+  
+#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  
+  
+
+  
+#define  ALPHA                 0.1f  // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
+#define  LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
+  
+// 状态机内部状态
+typedef enum
+{
+    STATE_INIT,        ///< 初始状态（转速为0且等待扭矩方向判定）
+    STATE_FORWARD,     ///< 前进
+    STATE_BACKWARD,    ///< 后退
+} MotorState;  
+  
+typedef enum
+{
+    MODE_MANUAL, // 手动模式
+    MODE_AUTO    // 自动模式    
+} ControlMode;
+
+typedef struct DiffData
+{
+    ControlMode mode ;               // 控制模式
+    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;    // 当前右后电机速度     
+    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 diff_dead_zone;             // 差速速度死区     
+    
+} DiffData;
+
+
+// 声明外部变量
+extern DiffData diff_data;
+
+void diffAppInit(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // APP_DIFFERENTIAL_DRIVE_H