#include "chassis_control.h"
#include "CAN_receive.h"

static pid_t pid_motor1;
static pid_t pid_motor2;
static pid_t pid_motor3;
static pid_t pid_motor4;

static int16_t target_speed_m1;
static int16_t target_speed_m2;
static int16_t target_speed_m3;
static int16_t target_speed_m4;

/**
 * @brief 初始化底盘电机的PID
 *        从左到右依次是：PID作用模块，KP,KI,KD,电机最大输出，电机最大限制输出
 */
void chassis_control_init(void)
{
    pid_init(&pid_motor1, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机1
    pid_init(&pid_motor2, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机2
    pid_init(&pid_motor3, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机3
    pid_init(&pid_motor4, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机4
}

/**
 * @brief 控制底盘电机的转速
 */
void chassis_control_task(void)
{
    const motor_measure_t *m1 = get_chassis_motor_measure_point(0);
    const motor_measure_t *m2 = get_chassis_motor_measure_point(1);
    const motor_measure_t *m3 = get_chassis_motor_measure_point(2);
    const motor_measure_t *m4 = get_chassis_motor_measure_point(3);

    //pid运算
    int16_t cur1 = (int16_t)pid_calc(&pid_motor1, target_speed_m1, m1->speed_rpm);
    int16_t cur2 = (int16_t)pid_calc(&pid_motor2, target_speed_m2, m2->speed_rpm);
    int16_t cur3 = (int16_t)pid_calc(&pid_motor3, target_speed_m3, m3->speed_rpm);
    int16_t cur4 = (int16_t)pid_calc(&pid_motor4, target_speed_m4, m4->speed_rpm);

    CAN_cmd_chassis(cur1, cur2, cur3, cur4);
}

/**
 * @brief 设置电机转速的电流值，范围（0~16384）
 */
void chassis_set_speed(int16_t m1, int16_t m2, int16_t m3, int16_t m4)
{
    target_speed_m1 = m1;
    target_speed_m2 = m2;
    target_speed_m3 = m3;
    target_speed_m4 = m4;
}