/*
 * 配置相关
 */

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

// 机器人参数配置
Config_RobotParam_t robot_config = {
    .ai_param = {
        .can= BSP_CAN_2,
        .vision_id= 0x104,
    },
    .gimbal_param = {
        .pid = {
            .yaw_omega = {
                .k = 1.0f,
                .p = 1.0f,
                .i = 0.3f,
                .d = 0.0f,
                .i_limit = 1.0f,
                .out_limit = 1.0f,
                .d_cutoff_freq = -1.0f,
                .range = -1.0f,
            },
            .yaw_angle = {
                .k = 6.0f,
                .p = 4.0f,
                .i = 0.0f,
                .d = 0.05f,
                .i_limit = 0.0f,
                .out_limit = 10.0f,
                .d_cutoff_freq = -1.0f,
                .range = M_2PI,
            },
            .pit_omega = {
                .k = 0.4f,
                .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,
            },
            .pit_angle = {
                .k = 8.0f,
                .p = 5.0f,
                .i = 2.5f,
                .d = 0.03f,
                .i_limit = 0.0f,
                .out_limit = 10.0f,
                .d_cutoff_freq = -1.0f,
                .range = M_2PI,
            },
        },
        .mech_zero = {
            .yaw = 0.0f,
            .pit = 2.98220015,  //0.195206895
        },
        .travel = {
            .yaw = -1.0f,
            .pit = 0.6,
        },
        .low_pass_cutoff_freq = {
            .out = -1.0f,
            .gyro = 1000.0f,
        },
        .pit_motor ={
            .can = BSP_CAN_2,
            .can_id = 0x2,
            .master_id = 0x12,
            .module = MOTOR_DM_J4310,
            .reverse = false,
        },
        .yaw_motor = {
            .can = BSP_CAN_2,
            .can_id = 0x50,
            .master_id = 0x60,
            .module = MOTOR_DM_J4310,
            .reverse = false,
        }
    },

    .shoot_param = {
        .basic={
          .projectileType=SHOOT_PROJECTILE_42MM,
		      .fric_num=6,
		      .extra_deceleration_ratio=1.0f,
          .num_trig_tooth=5,
          .shot_freq=1.0f,
          .shot_burst_num=3,
          .ratio_multilevel = {0.8f, 1.0f},
        },   
        .jamDetection={
            .enable=true,
            .threshold=310.0f, 
            .suspectedTime=0.5f, 
        },
        .motor={
            .fric = {
                {
                    .param = {
                        .can = BSP_CAN_1,
                        .id = 0x201,
                        .module = MOTOR_M3508,
                        .reverse = false,
                        .gear = false,
                    },
                    .level=1,
                },
                {
                    .param = {
                        .can = BSP_CAN_1,
                        .id = 0x202,
                        .module = MOTOR_M3508,
                        .reverse = true,
                        .gear = false,
                    },
                    .level=1,
                },
                {
                    .param = {
                        .can = BSP_CAN_1,
                        .id = 0x203,
                        .module = MOTOR_M3508,
                        .reverse = true,
                        .gear = false,
                    },
                    .level=1,
                },
                {
                    .param = {
                        .can = BSP_CAN_1,
                        .id = 0x204,
                        .module = MOTOR_M3508,
                        .reverse = false,
                        .gear = false,
                    },
                    .level=2,
                },
                {
                    .param = {
                        .can = BSP_CAN_1,
                        .id = 0x205,
                        .module = MOTOR_M3508,
                        .reverse = true,
                        .gear = false,
                    },
                    .level=2,
                },
                {
                    .param = {
                        .can = BSP_CAN_1,
                        .id = 0x206,
                        .module = MOTOR_M3508,
                        .reverse = true,
                        .gear = false,
                    },
                    .level=2,
                }
            },
            .trig = {
                .can = BSP_CAN_2,
                .id = 0x205,
                .module = MOTOR_M3508,
                .reverse = false,
                .gear=true,
            },
        },
        .pid={      
            .fric_follow = {
                .k=1.0f,
                .p=1.5f,
                .i=0.3f,
                .d=0.0f,
                .i_limit=0.2f,
                .out_limit=0.9f, 
                .d_cutoff_freq=-1.0f,
                .range=-1.0f,
            },
            .fric_err = {
                .k=0.0f,
                .p=4.0f,
                .i=0.4f,
                .d=0.0f,
                .i_limit=0.25f,
                .out_limit=0.25f, 
                .d_cutoff_freq=-1.0f,
                .range=-1.0f, 
            },
            .trig_2006 = {
                .k=2.5f,
                .p=1.0f,
                .i=0.1f,
                .d=0.04f,
                .i_limit=0.4f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=M_2PI,
            },
            .trig_omg_2006 = {
                .k=1.0f,
                .p=1.5f,
                .i=0.3f,
                .d=0.5f,
                .i_limit=0.2f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=-1.0f,
            },
            .trig_3508 = {
                .k=0.5f,
                .p=1.8f,
                .i=0.3f,
                .d=0.1f,
                .i_limit=0.15f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=M_2PI,
            },
            .trig_omg_3508 = {
                .k=1.0f,
                .p=1.0f,
                .i=0.0f,
                .d=0.0f,
                .i_limit=0.0f,
                .out_limit=1.0f,
                .d_cutoff_freq=-1.0f,
                .range=-1.0f,
            },
        },
		.filter={
            .fric = {
                .in = 30.0f,
                .out = 30.0f,
            },
            .trig = {
                .in = 30.0f,
                .out = 30.0f,
            },
        },  
    },
    
};
/* Private function prototypes ---------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

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