#include "module/chassis.hpp"
#include "module/dr16.h"

Chassis::Chassis() {
    motors_param[0] = {
        .can = BSP_CAN_2,
        .id = 0x201,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[1] = {
        .can = BSP_CAN_2,
        .id = 0x202,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[2] = {
        .can = BSP_CAN_2,
        .id = 0x203,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[3] = {
        .can = BSP_CAN_2,
        .id = 0x204,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
}

void Chassis::init() {
    MOTOR_RM_Register(&motors_param[0]);
    MOTOR_RM_Register(&motors_param[1]);
    MOTOR_RM_Register(&motors_param[2]);
    MOTOR_RM_Register(&motors_param[3]);
}

void Chassis::operator()() {
    //更新数据
    update_motors();
    //解算底盘数据

    //计算pid
    //发送控制
    // MOTOR_RM_SetOutput(&motors_param[0], 0.1f);
    // MOTOR_RM_Ctrl(&motors_param[0]);


}

void Chassis::update_motors(){
    MOTOR_RM_UpdateAll();

    MOTOR_RM_t *motor = NULL;
    motor = MOTOR_RM_GetMotor(&motors_param[0]);
    chassis_motors[0] = *motor;
    motor = MOTOR_RM_GetMotor(&motors_param[1]);
    chassis_motors[1] = *motor;
    motor = MOTOR_RM_GetMotor(&motors_param[2]);
    chassis_motors[2] = *motor;
    motor = MOTOR_RM_GetMotor(&motors_param[3]);
    chassis_motors[3] = *motor;
}

void Chassis::calc_chassis(){
    if(dr16.data.sw_r == DR16_SW_DOWN){
        
    }
}

void Chassis::calc_pid(){

}

void Chassis::send_control(){

}