#ifndef CHASSIS_CONTROL_H
#define CHASSIS_CONTROL_H

#include "pid.h"
#include "main.h"

#define LENGTH_A             200.0f   // 底盘长度一半 (mm)
#define LENGTH_B             166.0f   // 底盘宽度一半 (mm)
#define WHEEL_PERIMETER      0.152f   // 麦轮周长 (m)
#define CHASSIS_DECELE_RATIO 19.0f    // 底盘减速比
#define MAX_CHASSIS_SPEED    2.0f     // 底盘最大线速度(m/s)，摇杆推满对应此值
#define RC_DEADZONE          10       // 摇杆死区，绝对值小于此值视为0

typedef enum {
    CHASSIS_NORMAL = 0,     // 正常模式：遥控器控制底盘不跟随云台
    CHASSIS_GYROSCOPE = 1,  // 小陀螺模式：底盘原地旋转
    CHASSIS_SLOW = 2,       // 停止模式：底盘不动
} ChassisMode;

typedef struct {
    float vx;  // 前后方向速度 (m/s)
    float vy;  // 左右方向速度 (m/s)
    float vw;  // 旋转角速度 (rad/s)
} Chassis_Speed;

void chassis_control_init(void);
void chassis_control_task(void);
void chassis_set_target_speed(const Chassis_Speed *speed);
void chassis_set_mode(ChassisMode mode);

#endif