/*
 * 配置相关
 */

/* 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 = {

.gimbal_param = {

	/*欧拉角限位和电机角度限位*/
	 .Limit_t= {
						.pit_max=0.391373187,
						.pit_min=-0.481532812,
//						.yaw_max= 1.0,//yaw的
//						.yaw_min=-1.0,
		 			/*零点参数*/
							.zero={
								.yaw_encoder=2.16981578,
									.travel={
										.yaw=1.95812655,
						},
				},
		 
	},
	 

			.feedforward={
				.imu = {
					.yaw=false,
					.pit=false,
					.coefficient_yaw=0,
					.coefficient_pit=0,
					},

			},
			

				
				.motor={
					/*按自己需求选择电机,若不用大yaw注释掉即可*/
					.major_yaw=DM,
					.pit=DM,
					.yaw=RM,
					/*是否开启限位*/
					.limit_yaw=true,
					.limit_pit=true,
				
//					.pit_rm_motor={BSP_CAN_2,0x20A,MOTOR_GM6020,false,false},
					.yaw_rm_motor={BSP_CAN_2,0x209,MOTOR_GM6020,false,false},	
						/*达妙电机参数自己配*/
					.major_yaw_dm_motor={						
						.can = BSP_CAN_2,
            .can_id = 0x1,
            .master_id = 0x11,
            .module = MOTOR_DM_J4310,
            .reverse = false
						},		
					
						.pit_dm_motor={           
						.can = BSP_CAN_2,
            .can_id = 0x2,
            .master_id = 0x12,
            .module = MOTOR_DM_J4310,
            .reverse = true
						},
//					.yaw_dm_motor={
//						.can = BSP_CAN_1,
//            .can_id = 0x01,
//            .master_id = 0x11,
//            .module = MOTOR_DM_J4310,
//            .reverse = false},
				},
			.dm_Params_t={
			.major_yaw_dm={.kd=0.0,},
			.major_yaw_dm_Reduction_ratio={1.0f,},
//			.yaw_dm={.kd=0.3,},
//			.yaw_dm_Reduction_ratio=1.0f,//减速比
			.pit_dm={.kd=0.2,},
			.pit_dm_Reduction_ratio=1.0f,
		},
			
    .low_pass_cutoff_freq = {
      .out = -1.0f,
      .gyro = 1000.0f,
    },

        .pid = {
					/*大yaw欧拉角控制参数*/					
						.major_yaw_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							
						},
					
            .major_yaw_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
            },								
					/*欧拉角控制参数*/
             .yaw_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
            },
            .yaw_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
            },
            .pit_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
            },
            .pit_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
            },		
					}       
		},
};


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