#include "config.h"
#include "string.h"

#define DEBUG

#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)

#ifdef DEBUG
Config_Param_t config ={

#else

static const Config_Param_t config ={

#endif

.chassis={
    
    .motor_3508_param[0]={BSP_CAN_1,0x201,MOTOR_M3508,false,false},
    .motor_3508_param[1]={BSP_CAN_1,0x202,MOTOR_M3508,false,false},
    .motor_3508_param[2]={BSP_CAN_1,0x203,MOTOR_M3508,false,false},
    .motor_3508_param[3]={BSP_CAN_1,0x204,MOTOR_M3508,false,false},
    
    .motor_6020_param[0]={BSP_CAN_1,0x205,MOTOR_GM6020,false,false},
    .motor_6020_param[1]={BSP_CAN_1,0x206,MOTOR_GM6020,false,false},
    .motor_6020_param[2]={BSP_CAN_1,0x207,MOTOR_GM6020,false,false},
    .motor_6020_param[3]={BSP_CAN_1,0x208,MOTOR_GM6020,false,false},
    


    .C6020Omega_param={
        .k=1.0f,
        .p=0.5f,
        .i=0.1f,
        .d=0.0f,
        .i_limit=1.0f,
        .out_limit=1.0f,
        .d_cutoff_freq= -1.0f,
        .range=-1.0f
    },

    .C6020Angle_param={
        .k=0.2f,
        .p=0.21f,
        .i=0.1f,
        .d=0.0f,
        .i_limit=1.0f,
        .out_limit=30.0f,
        .d_cutoff_freq= -1.0f,
        .range=360
    },

    .M3508v_param={
        // .k=0.2f,
        // .p=0.25f,
        // .i=0.1f,
        // .d=0.0f,
        // .i_limit=1.0f,
        // .out_limit=1.0f,
        // .d_cutoff_freq= -1.0f,
        // .range=-1.0f
        .k=1.0f,
        .p=0.8f,
        .i=0.0f,
        .d=0.0f,
        .i_limit=1.0f,
        .out_limit=1.0f,
        .d_cutoff_freq= -1.0f,
        .range=-1.0f
    },
    .chassis_follow_gimbalPID ={
        .k=1.0f,
        .p=0.5f,
        .i=0.1f,
        .d=0.0f,
        .i_limit=1.0f,
        .out_limit=0.5f,
        .d_cutoff_freq= -1.0f,
        .range=M_2PI
    },

    /*这两个数据是云台6020的零点和机械限位*/
    .mech_zero=1.31f,
    .travel=1.4f,

},

 .gimbal={
.yaw_6020_motor={BSP_CAN_2,0x209,MOTOR_GM6020,false,true},

.yaw_4310_motor={ 
    .can=BSP_CAN_2,
    .can_id = 0x1,
    .master_id=0x11,
    .module = MOTOR_DM_J4310,
    .reverse=false
},
.pitch_4310_motor={
    .can=BSP_CAN_2,
    .can_id = 0x2,
    .master_id=0x12,
    .module = MOTOR_DM_J4310,
    .reverse=true
},
 .pid.pitch_4310_motor_angle={
    .k = -2.0f,
    .p = 2.0f,
    .i = 0.0f,
    .d = 0.0f,
    .i_limit = 1.0f,
    .out_limit = 10.0f,
    .d_cutoff_freq = -1.0f,
    .range = M_2PI
 },
 .pid.pitch_4310_motor_omega={
    .k = 0.3f,
    .p = 0.3f,
    .i = 0.0f,
    .d = 0.0f,
    .i_limit = 0.0f,
    .out_limit = 1.0f,
    .d_cutoff_freq = -1.0f,
    .range = -1.0f
 },
 .pid.yaw_4310_motor_angle={
    .k = 2.0f,
    .p = 7.0f,
    .i = 0.01f,
    .d = 0.1f,
    .i_limit = 0.0f,
    .out_limit = 7.0f,
    .d_cutoff_freq = 20.0f,
    .range = M_2PI
 },
 .pid.yaw_4310_motor_omega={
    .k = 0.0f,
    .p = 0.3f,
    .i = 0.0f,
    .d = 0.0f,
    .i_limit = 0.0f,
    .out_limit = 1.0f,
    .d_cutoff_freq = -1.0f,
    .range = -1.0f
 },
 .pid.yaw_6020_motor_angle={
    .k = 2.0f,
    .p = 2.5f,
    .i = 0.0f,
    .d = 0.0f,
    .i_limit = 0.0f,
    .out_limit = 10.0f,
    .d_cutoff_freq = 0.0f,
    .range = M_2PI
 },
 .pid.yaw_6020_motor_omega={
    .k = 0.30f,
    .p = 0.4f,
    .i = 0.01f,
    .d = 0.0f,
    .i_limit = 1.0f,
    .out_limit = 1.0f,
    .d_cutoff_freq = -1.0f,
    .range = -1.0f
 },

.mech_zero={
    .yaw_6020=1.31f,
    .yaw_4310=0.0f,/*大yaw无限位与零点*/
    .pitch_4310=1.0f,
},
.travel={
    .yaw_6020=1.4f,
    .yaw_4310=-1.0f,/*大yaw无限位*/
    .pitch_4310=0.9f,
    // .pitch_4310=-1.0f,/*pitch无限位*/
},




},


};


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