half-steering-basic-framework/application/chassis/chassis.c

/**
 * @file chassis.c
 * @author NeoZeng neozng1@hnu.edu.cn
 * @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出
 *        注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为x正方向;横向运动的右侧为y正方向
 *
 * @version 0.1
 * @date 2022-12-04
 *
 * @copyright Copyright (c) 2022
 *
 */

#include "chassis.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "LKMotor.h"
#include "super_cap.h"
#include "message_center.h"
#include "referee_task.h"
#include "user_lib.h"

#include "general_def.h"
#include "bsp_dwt.h"
#include "referee_UI.h"
#include "arm_math.h"

/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
#if defined(CHASSIS_MCNAMEE_WHEEL)
#define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量
#elif defined(CHASSIS_STEERING_WHEEL)
#define CHASSIS_WHEEL_OFFSET 30.0f       // 机器人底盘轮子修正偏移量
#define SQRT2                1.41421356f // 根号2
#elif defined(CHASSIS_OMNI_WHEEL)
#define CHASSIS_WHEEL_OFFSET 0.7071f // 机器人底盘轮子修正偏移量，根号2/2，即45度，用于修正全向轮的安装位置
#endif                               // CHASSIS_OMNI_WHEEL

/* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
#ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
#include "can_comm.h"
#include "ins_task.h"
static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
attitude_t *Chassis_IMU_data;
#endif // CHASSIS_BOARD
#ifdef ONE_BOARD
static Publisher_t *chassis_pub;                    // 用于发布底盘的数据
static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控制命令
#endif                                              // !ONE_BOARD
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据

static referee_info_t* referee_data; // 用于获取裁判系统的数据
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI

static SuperCapInstance *cap;                                       // 超级电容
static LKMotorInstance *motor_lf,*motor_rb;                         // 轮电机9015
static DJIMotorInstance *motor_steering_lf,*motor_steering_rb;     // 舵电机6020
/* 用于自旋变速策略的时间变量 */
// static float t;

/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
static float vt_lf, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
static float at_lf, at_rb; // 底盘的角度解算后的临时输出,待进行限幅
static PIDInstance chassis_follow_pid;   // 底盘跟随云台的PID

void ChassisInit()
{
    // 轮电机9015初始化
    Motor_Init_Config_s chassis_motor_config = {
        .can_init_config.can_handle = &hcan1,
        .controller_param_init_config = {
            .speed_PID = {
                .Kp = 95, // 4.5
                .Ki = 0,  // 0
                .Kd = 0,  // 0
                .IntegralLimit = 8000,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 12000,
            },
            .current_PID = {
                .Kp = 1, // 0.4
                .Ki = 0,   // 0
                .Kd = 0,
                .IntegralLimit = 10500,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 20000,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
        },
        .motor_type = LK9015,
    };
    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
    
    chassis_motor_config.can_init_config.tx_id = 1;
    // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_rb = LKMotorInit(&chassis_motor_config);
    
    chassis_motor_config.can_init_config.tx_id = 2;
    // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_lf = LKMotorInit(&chassis_motor_config);

    // 舵电机6020初始化
    Motor_Init_Config_s chassis_motor_steering_config = {
        .can_init_config.can_handle   = &hcan1,
        .controller_param_init_config = {
            .angle_PID = {
                .Kp                = 12,
                .Ki                = 0.2,
                .Kd                = 0,
                .CoefA             = 3,
                .CoefB             = 0.1,
                .Improve           = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate,
                .IntegralLimit     = 1000,
                .MaxOut            = 5000,
                .Derivative_LPF_RC = 0.001,
                .DeadBand          = 0.5,
            },
            .speed_PID = {
                .Kp            = 40,
                .Ki            = 3,
                .Kd            = 0,
                .Improve       = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate | PID_OutputFilter,
                .IntegralLimit = 4000,
                .MaxOut        = 20000,
                .Output_LPF_RC = 0.03,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type       = ANGLE_LOOP,
            .close_loop_type       = ANGLE_LOOP | SPEED_LOOP,
            .motor_reverse_flag    = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = GM6020,
    };

    chassis_motor_steering_config.can_init_config.tx_id = 1;
    motor_steering_rb                                   = DJIMotorInit(&chassis_motor_steering_config);
    
    chassis_motor_steering_config.can_init_config.tx_id = 2;
    motor_steering_lf                                   = DJIMotorInit(&chassis_motor_steering_config);

    PID_Init_Config_s chassis_follow_pid_conf = {
        .Kp                = 20, // 6
        .Ki                = 0,
        .Kd                = 3, // 0.5
        .DeadBand          = 0.5f,
        //.CoefA             = 0.2,
        //.CoefB             = 0.3,
        //.Improve           = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
        //.IntegralLimit     = 50, // 200
        .MaxOut            = 45,
        //.Derivative_LPF_RC = 0.01, // 0.01
    };
    PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf);

    // 裁判系统初始化

    referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI

    SuperCap_Init_Config_s cap_conf = {
        .can_config = {
            .can_handle = &hcan2,
            .tx_id = 0x302, // 超级电容默认接收id
            .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
        }};
    cap = SuperCapInit(&cap_conf); // 超级电容初始化

    // 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD
    Chassis_IMU_data = INS_Init(); // 底盘IMU初始化

    CANComm_Init_Config_s comm_conf = {
        .can_config = {
            .can_handle = &hcan2,
            .tx_id = 0x311,
            .rx_id = 0x312,
        },
        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
        .send_data_len = sizeof(Chassis_Upload_Data_s),
    };
    chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化
#endif                                          // CHASSIS_BOARD

#ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息
    chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
    chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
#endif // ONE_BOARD
}

// #define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
// #define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
// #define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
// #define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)

// static void SteeringWheelRotateHandle()
// {
//     if (fabsf(chassis_cmd_recv.vx) > 0 || fabsf(chassis_cmd_recv.vy) > 0) {
//         chassis_cmd_recv.wz = 0;
//     }
// }

/**
 * @brief 使舵电机角度最小旋转，取优弧，防止电机旋转不必要的行程
 *          例如：上次角度为0，目标角度为135度，
 *          电机会选择逆时针旋转至-45度，而不是顺时针旋转至135度，
 *          两个角度都会让轮电机处于同一平行线上
 *
 * @param angle 目标角度
 * @param last_angle 上次角度
 *
 */
static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
{
    float target_angle = *angle;
    float actual_angle = *last_angle;
    float rotation = target_angle - actual_angle;
    float norm_rotation = rotation;
    while (norm_rotation > 180.0f) {
        norm_rotation -= 360.0f;
    }
    while (norm_rotation < -180.0f) {
        norm_rotation += 360.0f;
    }
    float threshold = 90.0f;
    float target_single=fmodf(target_angle,360.0f);
    if(target_single<0.0f) target_single+=360.0f;

    float current_single=fmodf(actual_angle,360.0f);
    if(current_single<0.0f) current_single+=360.0f;


    if (fabsf(norm_rotation) > threshold) {
        int32_t round_diff = (int32_t)(target_angle / 360.0f) - (int32_t)(actual_angle / 360.0f);
        *angle = actual_angle + norm_rotation-180.0f+round_diff*360.0f;
        *speed = -(*speed);
    }else if(norm_rotation<-threshold){
        int32_t round_diff = (int32_t)(target_angle / 360.0f) - (int32_t)(actual_angle / 360.0f);
        *angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f;
        *speed = -(*speed);
    }
    ANGLE_LIMIT_360_TO_180_ABS(*angle);
    // float rotation = *angle - *last_angle;
    // ANGLE_LIMIT_360_TO_180_ABS(rotation);
    // if (rotation > 90) {
    //     *angle -= 180;
    //     *speed = -(*speed);
    // } else if (rotation < -90) {
    //     *angle += 180;
    //     *speed = -(*speed);
    // }
    // ANGLE_LIMIT_360_TO_180_ABS(*angle);
}

/**
 * @brief 舵轮电机角度解算
 *
 */
static void SteeringWheelCalculate()
{
    float offset_lf, offset_rb;     // 用于计算舵轮的角度
    float at_lf_last, at_rb_last;   // 上次的角度
    at_lf_last = motor_steering_lf->measure.total_angle;
    at_rb_last = motor_steering_rb->measure.total_angle;
    if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) {
        // at_lf = at_lf_last;
        // at_rf = at_rf_last;
        // at_lb = at_lb_last;
        // at_rb = at_rb_last;
        vt_lf = 0, vt_rb = 0;
    } else {
        // 生成预计算变量，减少计算量，空间换时间
        float w      = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
        float temp_x = chassis_vx - w, temp_y = chassis_vy - w;
        arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x-
        temp_y = chassis_vy + w;                                 // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释
        // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
        // temp_x = chassis_vx + w;                                 // temp_y = chassis_vx + w;与上次相同因此注释
        arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+
        temp_y = chassis_vy - w;                                 // temp_x = chassis_vy + w;与上次相同因此注释
        // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+

        // 计算角度偏移
        offset_lf = atan2f(chassis_vx - w, chassis_vy - w) * RAD_2_DEGREE; // lf:  y- , x-
        offset_rb = atan2f(chassis_vx + w, chassis_vy + w) * RAD_2_DEGREE; // rb:  y+ , x+

        at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
        at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb;

        ANGLE_LIMIT_360_TO_180_ABS(at_lf);
        ANGLE_LIMIT_360_TO_180_ABS(at_rb);

        MinmizeRotation(&at_lf, &at_lf_last, &vt_lf);
        MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
    }

    DJIMotorSetRef(motor_steering_lf, at_lf);
    DJIMotorSetRef(motor_steering_rb, at_rb);
}

/**
 * @brief 计算每个轮毂电机的输出,正运动学解算
 *        用宏进行预替换减小开销,运动解算具体过程参考教程
 */
// static void MecanumCalculate()
// {
//     vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
//     vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
//     vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
//     vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
// }

/**
 * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
 *
 */
static void LimitChassisOutput()
{
    // 功率限制待添加
    // referee_data->PowerHeatData.chassis_power;
    // referee_data->PowerHeatData.chassis_power_buffer;

    // 完成功率限制后进行电机参考输入设定
    
    // LKMotorSetRef( motor_lf , vt_lf );
    // LKMotorSetRef( motor_rb , vt_rb );

    LKMotorSetRef( motor_lf , vt_lf );
    LKMotorSetRef( motor_rb , vt_rb );
}

/**
 * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
 *        对于双板的情况,考虑增加来自底盘板IMU的数据
 *
 */
static void EstimateSpeed()
{
    // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有)
    // chassis_feedback_data.vx vy wz =
    //  ...
}

/* 机器人底盘控制核心任务 */
void ChassisTask()
{
    // 后续增加没收到消息的处理(双板的情况)
    // 获取新的控制信息
#ifdef ONE_BOARD
    SubGetMessage(chassis_sub, &chassis_cmd_recv);
#endif
#ifdef CHASSIS_BOARD
    chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
#endif // CHASSIS_BOARD

    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
        LKMotorStop(motor_lf);
        LKMotorStop(motor_rb);
        DJIMotorStop(motor_steering_lf);
        DJIMotorStop(motor_steering_rb);
    }
    else
    {
        // 正常工作
        LKMotorEnable(motor_lf);
        LKMotorEnable(motor_rb);
        DJIMotorEnable(motor_steering_lf);
        DJIMotorEnable(motor_steering_rb);
    }

    // 根据控制模式设定旋转速度
    switch (chassis_cmd_recv.chassis_mode)
    {
    case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
        chassis_cmd_recv.wz = 0;
        break;
    case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
        float single_round_offset = chassis_cmd_recv.offset_angle;
        // float single_round_offset = fmodf(chassis_cmd_recv.offset_angle, 360.0f);
        if(single_round_offset > 180.0f)
        {
            single_round_offset -= 360.0f;
        }
        else if(single_round_offset < -180.0f)
        {
            single_round_offset += 360.0f;

        }
        chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100;
        // chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100;
        break;
    case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
        chassis_cmd_recv.wz = 0.5;
        break;
    default:
        break;
    }

    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
    static float sin_theta, cos_theta;
    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
    chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
    chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;

    // 根据控制模式进行正运动学解算,计算底盘输出
    SteeringWheelCalculate();
    // MecanumCalculate();

    // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
    LimitChassisOutput();

    // 根据电机的反馈速度和IMU(如果有)计算真实速度
    EstimateSpeed();

    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
    // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
    // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
    // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
    // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
    // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;

    // 推送反馈消息
#ifdef ONE_BOARD
    PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
#endif
#ifdef CHASSIS_BOARD
    CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
#endif // CHASSIS_BOARD
}