#ifndef __M3508_H
#define __M3508_H
#include "main.h"
#include "PID.h"

// 底盘参数（根据实际机械结构调整）
#define WHEEL_RADIUS          0.075f// 轮子半径 (米)
#define WHEELBASE_WIDTH       0.21f      // 轮距宽度/2 (米)
#define WHEELBASE_LENGTH      0.17f      // 轮距长度/2 (米)

typedef struct
{
 uint16_t ecd;//编码器值 -16384--+16384
 int16_t speed_rpm;//转速 转每分钟
 int16_t given_current;//输出电流值
	uint8_t temperate;//电机温度
	int16_t last_ecd;//上次编码器值
} motor_measure_t;


// 电机控制结构体
typedef struct {
    motor_measure_t measure;    // 电机测量值（速度单位：RPM）
    PID_Controller_t speed_pid; // 速度环PID
    float target_speed;         // 目标速度 (rad/s)
} Motor_Controller_t;


extern motor_measure_t motor_chassis[4];
extern Motor_Controller_t motor_controller[4];
extern int16_t motor_commands[4];


extern void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4);
extern float RPM_To_RadPerSec(int16_t rpm);
extern int16_t RadPerSec_To_RPM(float rad_per_sec);
extern void Set_Motor_Target_Speed(uint8_t motor_id, float target_speed_rad_s);
extern void Motor_Speed_Control_Task(float dt);
extern void Drive_Motor(float vx, float vy, float vz);
extern float Map_Remote_to_Speed(int16_t input, float max_speed);
extern void Process_Remote_Control(void);
#endif