/*
 * ÅäÖÃÏà¹Ø
 */

/* 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,
			.gear =  true
			},
      { 
			.can = BSP_CAN_1,
			.id = 0x202, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true
			},
      { 
			.can = BSP_CAN_1, 
			.id = 0x203, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true				
			},
      { 
			.can = BSP_CAN_1,
			.id = 0x204, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true	
			},
    },
		.type = CHASSIS_TYPE_OMNI_PLUS,
    /* PID ?? */
    .pid = {
      /* ????/??? PID */
      .motor_pid_param = {
        .k = 0.001f,
        .p = 0.01f,
        .i = 0.01f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      },

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

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

    /* ???? */
    .reverse = {
      .yaw = true,
    },
		.limit = {
      .max_vx = 3.0f,     // ?????????????
      .max_vy = 3.0f,
      .max_wz = 2.0f,     // rad/s ???
      .max_current = 16000.0f  // ???????????(DJI ?? ±16384)
    },
  },
};
/* Private function prototypes ---------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

/**
 * @brief »ñÈ¡»úÆ÷ÈËÅäÖòÎÊý
 * @return »úÆ÷ÈËÅäÖòÎÊýÖ¸Õë
 */
Config_RobotParam_t* Config_GetRobotParam(void) {
    return &robot_config;
}