/*
 * 配置相关
 */

/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include "module/chassis.h"
#include "bsp/can.h"
#include "device/motor_rm.h"
#include "component/pid.h"
#include "component/user_math.h"
#include <stdbool.h>

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */

/* Exported variables ------------------------------------------------------- */

Config_RobotParam_t robot_config = {
  .chassis_param = {
    /* ??3508???? */
    .motor_param = {
      { 
			.can = BSP_CAN_1, 
			.id = 0x201, 
			.module = MOTOR_M3508, 
			.reverse = false 
			},
      { 
			.can = BSP_CAN_1,
			.id = 0x202, 
			.module = MOTOR_M3508, 
			.reverse = false 
			},
      { 
			.can = BSP_CAN_1, 
			.id = 0x203, 
			.module = MOTOR_M3508, 
			.reverse = false 
			},
      { 
			.can = BSP_CAN_1,
			.id = 0x204, 
			.module = MOTOR_M3508, 
			.reverse = false
			},
    },

    /* PID ?? */
    .pid = {
      /* ????/??? PID */
      .motor_pid_param = {
        .k = 0.001f,
        .p = 8.0f,
        .i = 0.1f,
        .d = 0.5f,
        .i_limit = 2000.0f,
        .out_limit = 16000.0f,
        .d_cutoff_freq = 100.0f,
        .range = -1.0f,
      },

      /* ?????????? */
      .follow_pid_param = {
        .k = 0.5f,
        .p = 6.0f,
        .i = 0.0f,
        .d = 0.2f,
        .i_limit = 1.0f,
        .out_limit = 2.0f,
        .d_cutoff_freq = 30.0f,
        .range = M_2PI,
      },
    },

    /* ??/?????? */
    .low_pass_cutoff_freq = {
      .in = 50.0f,
      .out = 50.0f,
    },

    /* ???? */
    .reverse = {
      .yaw = true,
    },
  },
};
/* Private function prototypes ---------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

/**
 * @brief 获取机器人配置参数
 * @return 机器人配置参数指针
 */
Config_RobotParam_t* Config_GetRobotParam(void) {
    return &robot_config;
}