97 lines
3.1 KiB
C
97 lines
3.1 KiB
C
#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
|