/*
 * 配置相关
 */

/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include "bsp/can.h"
#include "device/motor_dm.h"
#include <stdbool.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */

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

// 机器人参数配置
// DH参数来源: URDF模型 (newURDF.urdf, 2026-01-20)
// theta(变量), d(mm), a(mm), alpha(rad), theta_offset(rad)
// 零位姿态: 机械臂竖直向上
Config_RobotParam_t robot_config = {
    .chassis_param = {
    /* DJI3508µç»ú*/
        .motor_param = {
      { 
			.can = BSP_CAN_2, 
			.id = 0x201, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true
			},
      { 
			.can = BSP_CAN_2,
			.id = 0x202, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true
			},
      { 
			.can = BSP_CAN_2, 
			.id = 0x203, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true				
			},
      { 
			.can = BSP_CAN_2,
			.id = 0x204, 
			.module = MOTOR_M3508, 
			.reverse = false,
			.gear =  true	
			},
    },
		.type = CHASSIS_TYPE_MECANUM,
    /* PID  */
    .pid = {
      /* µ×Å̵ç»ú PID */
      .motor_pid_param = {
        .k = 0.0015f,
        .p = 1.0f,
        .i = 0.0f,
        .d = 0.0f,
        .i_limit = 1.0f,
        .out_limit = 1.0f,
        .d_cutoff_freq = -1.0f,
        .range = -1.0f,
      },

      /* ¸úËæ */
      .follow_pid_param = {
        .k = 1.2f,
        .p = 1.0f,
        .i = 0.5f,
        .d = 0.01f,
        .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,    
      .max_current = 16000.0f 
    },
  },
  .cmd_param={
        .source_priority = {
            CMD_SRC_RC,
            CMD_SRC_PC
        },
        .sensitivity = {
            .mouse_sens = 2.0f,
            .move_sens = 1.0f,
            .move_fast_mult = 1.5f,
            .move_slow_mult = 0.5f,
        },
        .rc_mode_map = {
            .sw_left_up   = CHASSIS_MODE_RELAX,
            .sw_left_mid  = CHASSIS_MODE_FOLLOW_GIMBAL,
            .sw_left_down = CHASSIS_MODE_RELAX,

        },
    },
};
power_model_t cha=
{
    .motor_num =4,
    .constant=  0.7910660075305308,
    .k1 =  98.97509148647588,
    .k2=  0.00018014375383727376,
    .toque_coefficient =0.4568913750568248,
    .power = (float[4]){}
  };

power_model_t cha2=
{
    .motor_num =4,
    .constant= 2.354223704504904,
    .k1 =  94.85324024176896,
    .k2=  0.00012622263917529537,
    .toque_coefficient =0.3680225540649464,

    .power = (float[4]){}
  };
/* Private function prototypes ---------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

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