/*
 * 配置相关
 */

/* 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={
    .pid={  
		.Radder_DIR_Omega={
		    .k=1.0f,
        .p=0.5f,
        .i=0.15f,
        .d=0.0f,
        .i_limit=1.0f,
        .out_limit=1.0f,
        .d_cutoff_freq= -1.0f,
        .range=-1.0f
		},
		.Radder_DIR_Angle={
        .k=0.15f,
        .p=0.20f,
        .i=0.1f,
        .d=0.001f,
        .i_limit=1.0f,
        .out_limit=30.0f,
        .d_cutoff_freq= -1.0f,
        .range=360
		},
		.Wheel_DIR_Omega={
        .k=0.5f,
        .p=2.0f,
        .i=0.1f,
        .d=0.7f,
        .i_limit=1.0f,
        .out_limit=1.0f,
        .d_cutoff_freq= -1.0f,
        .range=-1.0f
		},
    },
    .motor={
    .Wheel_DIR[0]={BSP_CAN_2,0x203,MOTOR_M3508,false,false},
    .Wheel_DIR[1]={BSP_CAN_2,0x204,MOTOR_M3508,true,false},
    .Wheel_DIR[2]={BSP_CAN_2,0x202,MOTOR_M3508,true,false},
    .Wheel_DIR[3]={BSP_CAN_2,0x201,MOTOR_M3508,false,false},

    .Radder_DIR[0]={BSP_CAN_2,0x205,MOTOR_GM6020,false,false},
    .Radder_DIR[1]={BSP_CAN_2,0x206,MOTOR_GM6020,false,false},
    .Radder_DIR[2]={BSP_CAN_2,0x207,MOTOR_GM6020,false,false},
    .Radder_DIR[3]={BSP_CAN_2,0x208,MOTOR_GM6020,false,false},
    },
},
  .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  = RC,
           // .sw_left_down = ,

        },
    },
};
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;
}