/*
 * 配置相关
 */

/* Includes ----------------------------------------------------------------- */
#include "user_math.h"
#include "module/config.h"
#include "bsp/can.h"

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

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

// 机器人参数配置
Config_RobotParam_t robot_config = {

.chassis_param = {
    /* DJI3508µç»ú*/
    .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_MECANUM,
    /* PID  */
    .pid = {
      /* µ×Å̵ç»ú PID */
      .motor_pid_param = {
        .k = 0.001f,
        .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 = 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,    
      .max_current = 16000.0f 
    },
  },
.gimbal_param = {
	/*欧拉角限位和电机角度限位*/
	 .Set_Limit_t= {
						.pit_max=0.25,
						.pit_min=-0.53,
						.yaw_max= 1.0,//yaw的限位作用于小yaw
						.yaw_min=-1.0,
						.pit_ecd_max=0,
						.pit_ecd_min=0,
						.yaw_ecd_max=0,
						.yaw_ecd_min=0,
	},
	 
//	.mech_zero = {
//            .yaw = 0.0f,
//            .pit = -0.69f,
//        },
	
	/*是否启动限位,1启动，-1不启动
		小yaw默认限位
		一般pit统一限位*/
				.travel = {
            .yaw = -1.0f,
            .pit = 1.0f,
						.ecd_yaw=-1.0f,
						.ecd_pit=1.0f,
        },
	
				
				.motor={
					/*按自己需求选择电机*/
					.pit=DM,
					.yaw=DM,
//					.pit_rm_motor={BSP_CAN_2,0x20A,MOTOR_GM6020,false,false},
//					.yaw_rm_motor={BSP_CAN_1,0x205,MOTOR_GM6020,false,false},	
						/*达妙电机参数自己配*/
					.pit_dm_motor={
					 .can = BSP_CAN_1,
           .can_id = 0x2,
           .master_id = 0x12,
           .module = MOTOR_DM_J4310,
           .reverse = false,
					},
					.yaw_dm_motor={
            .can = BSP_CAN_1,
            .can_id = 0x50,
            .master_id = 0x60,
            .module = MOTOR_DM_J4310,
            .reverse = false,
				},
			},
				
			.dm_Params_t={
			.yaw_dm={.kd=0.3,},
			.yaw_dm_Reduction_ratio=8.0f,//减速比
			.pit_dm={.kd=1.0,},
			.pit_dm_Reduction_ratio=5.0f,
		},
    .low_pass_cutoff_freq = {
      .out = -1.0f,
      .gyro = 1000.0f,
    },

        .pid = {
					/*欧拉角控制参数*/
             .yaw_omega = {
                .k = 1.0f,
                .p = 0.1f,
                .i = 0.2f,
                .d = 0.0f,
                .i_limit = 1.0f,
                .out_limit = 1.0f,
                .d_cutoff_freq = -1.0f,
                .range = -1.0f,
            },
            .yaw_angle = {
                .k = 8.0f,
                .p = 4.0f,
                .i = 0.0f,
                .d = 0.2f,
                .i_limit = 0.0f,
                .out_limit = 10.0f,
                .d_cutoff_freq = -1.0f,
                .range = M_2PI,
            },
            .pit_omega = {
                .k = 0.4f,
                .p = 0.0f,
                .i = 0.0f,
                .d = 0.0f,
                .i_limit = 1.0f,
                .out_limit = 1.0f,
                .d_cutoff_freq = -1.0f,
                .range = -1.0f,
            },
            .pit_angle = {
                .k = 8.0f,
                .p = 5.0f,
                .i = 2.5f,
                .d = 0.2f,
                .i_limit = 0.0f,
                .out_limit = 10.0f,
                .d_cutoff_freq = -1.0f,
                .range = M_2PI,
            },
						/*电机控制参数*/
             .yaw_velocity = {
                .k = 1.0f,
                .p = 1.0f,
                .i = 0.0f,
                .d = 0.000f,//0
                .i_limit = 1.0f,
                .out_limit = 1.0f,
                .d_cutoff_freq = -1.0f,
                .range = -1.0f,
            },
            .yaw_ecd_angle = {
                .k = 1.0f,
                .p = 1.0f ,
                .i =	0.0f,
                .d = 0.0f,
                .i_limit = 0.0f,
                .out_limit = 10.0f,
                .d_cutoff_freq = -1.0f,
                .range = M_2PI,
            },
            .pit_velocity = {
                .k = 0.0f,
                .p = 0.0f,
                .i = 0.0f,
                .d = 0.0f,
                .i_limit = 1.0f,
                .out_limit = 1.0f,
                .d_cutoff_freq = -1.0f,
                .range = -1.0f,
            },
            .pit_ecd_angle = {
                .k = 1.0f,
                .p = 0.0f,
                .i = 0.0f,
                .d = 0.0f,
                .i_limit = 0.0f,
                .out_limit = 10.0f,
                .d_cutoff_freq = -1.0f,
                .range = M_2PI,	
            },						
					}       
		},
};


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