bf_original_balance_chassis/application/chassis/balance.c

// app
#include "balance.h"
#include "linkNleg.h"
#include "robot_def.h"
#include "general_def.h"
#include "ins_task.h"
// #include "HT04.h"
#include "dmmotor.h"
#include "LK9025.h"
#include "controller.h"
#include "can_comm.h"
#include "super_cap.h"
#include "user_lib.h"
#include "remote_control.h"
#include "referee_task.h"
#include "stdint.h"
#include "arm_math.h" // 需要用到较多三角函数
#include "bsp_dwt.h"
#include "bsp_log.h"
#include "lqr_calc.h"
#include "speed_estimation.h"
#include "fly_detection.h"
#include "buzzer.h"
#include "math.h"
#include "string.h"

#define STOOL_SPEED_REF 1.5f
#define STOOL_YAW_RATE_REF 3.5f
#define STOOL_SPIN_YAW_RATE_REF 10.0f
#define STOOL_SPIN_YAW_ACC_REF 5.0f
#define STOOL_ACC_REF 4.0f
#define STOOL_BRAKE_ACC_REF 8.0f
#define STOOL_PITCH_K 55.0f
#define STOOL_PITCH_W_K 7.0f
#define STOOL_VEL_K 10.0f
#define STOOL_STOP_DIST_K 8.0f
#define STOOL_PITCH_REF -0.10f
#define STOOL_VEL_LPF_RC 0.03f
#define STOOL_WHEEL_TORQUE_LIMIT 25.0f
#define STOOL_RC_DEADBAND 40
#define CHASSIS_UNDERVOLTAGE_LIMIT 15.0f
#define REFEREE_VOLTAGE_VALID_MIN 5.0f
// Stage 1 permits only 0xFD safety-disable probes.
#define JOINT_RX_ONLY_TEST 1u
#define JOINT_RX_PROBE_INTERVAL 0.01f
#define JOINT_RX_PROBE_START_DELAY 0.5f
#define JOINT_RB_MOTOR_ID 0x01u
#define JOINT_RF_MOTOR_ID 0x02u
#define JOINT_LB_MOTOR_ID 0x03u
#define JOINT_LF_MOTOR_ID 0x04u
#define JOINT_RB_MASTER_ID 0x11u
#define JOINT_RF_MASTER_ID 0x12u
#define JOINT_LB_MASTER_ID 0x13u
#define JOINT_LF_MASTER_ID 0x14u
// 计时变量
static uint32_t balance_dwt_cnt;
static float del_t;

// 底盘拥有的实例模块
static INS_t *Chassis_IMU_data;
static RC_ctrl_t *rc_data; // 底盘单独调试用
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
// 四个关节电机和两个驱动轮电机
static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2];

volatile JointMotorRxOnlyDebug_s joint_rx_debug = {
    .rx_only = JOINT_RX_ONLY_TEST,
    .left_front = {
        .expected_motor_id = JOINT_LF_MOTOR_ID,
        .expected_feedback_can_id = JOINT_LF_MASTER_ID},
    .left_back = {
        .expected_motor_id = JOINT_LB_MOTOR_ID,
        .expected_feedback_can_id = JOINT_LB_MASTER_ID},
    .right_front = {
        .expected_motor_id = JOINT_RF_MOTOR_ID,
        .expected_feedback_can_id = JOINT_RF_MASTER_ID},
    .right_back = {
        .expected_motor_id = JOINT_RB_MOTOR_ID,
        .expected_feedback_can_id = JOINT_RB_MASTER_ID},
};

// 两个腿的参数,0为左腿,1为右腿
static LinkNPodParam l_side, r_side;
static ChassisParam chassis;

// 综合运动补偿的PID控制器
static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬"
static PIDInstance roll_compensate_pid;        // roll轴补偿,用于保持机体水平
static PIDInstance steer_p_pid, steer_v_pid;   // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
static PIDInstance anti_crash_pid;             // 抗劈叉,将输出以相反的方向叠加到左右腿的上

// 底盘状态
static Robot_Status_e chassis_status;

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

static SuperCapInstance *cap; // 超级电容

static uint8_t stool_drive_key_pressed;
static float stool_vel;
static float stool_dist;
static float stool_stop_dist;
static float stool_target_wz;
static uint8_t stool_spin_enabled;
static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE;
static float joint_rx_probe_start_elapsed;
static float joint_rx_probe_elapsed;
static uint8_t joint_rx_probe_index;

static void UpdateJointRxDebugMotor(volatile JointMotorRxDebug_s *debug, DMMotorInstance *motor)
{
    DM_Motor_Measure_s *measure = &motor->measure;

    debug->feedback_motor_id = measure->id;
    debug->state = measure->state;
    debug->feedback_can_id = measure->feedback_can_id;
    debug->feedback_count = measure->feedback_count;
    debug->received = measure->feedback_count > 0u;
    debug->online = debug->received && DaemonIsOnline(motor->motor_daemon);
    debug->id_matches = debug->received &&
                        measure->id == debug->expected_motor_id;
    debug->feedback_can_id_matches = debug->received &&
                                     measure->feedback_can_id == debug->expected_feedback_can_id;
    debug->position_rad = measure->position;
    debug->velocity_rad_s = measure->velocity;
    debug->torque = measure->torque;
    debug->mos_temperature_c = measure->T_Mos;
    debug->rotor_temperature_c = measure->T_Rotor;
}

static void UpdateJointRxDebug(void)
{
    UpdateJointRxDebugMotor(&joint_rx_debug.left_front, lf);
    UpdateJointRxDebugMotor(&joint_rx_debug.left_back, lb);
    UpdateJointRxDebugMotor(&joint_rx_debug.right_front, rf);
    UpdateJointRxDebugMotor(&joint_rx_debug.right_back, rb);

    joint_rx_debug.rx_only = lf->rx_only && lb->rx_only &&
                             rf->rx_only && rb->rx_only;
    joint_rx_debug.all_received = joint_rx_debug.left_front.received &&
                                  joint_rx_debug.left_back.received &&
                                  joint_rx_debug.right_front.received &&
                                  joint_rx_debug.right_back.received;
    joint_rx_debug.all_online = joint_rx_debug.left_front.online &&
                                joint_rx_debug.left_back.online &&
                                joint_rx_debug.right_front.online &&
                                joint_rx_debug.right_back.online;
    joint_rx_debug.all_ids_match = joint_rx_debug.left_front.id_matches &&
                                   joint_rx_debug.left_back.id_matches &&
                                   joint_rx_debug.right_front.id_matches &&
                                   joint_rx_debug.right_back.id_matches;
    joint_rx_debug.all_feedback_can_ids_match =
        joint_rx_debug.left_front.feedback_can_id_matches &&
        joint_rx_debug.left_back.feedback_can_id_matches &&
        joint_rx_debug.right_front.feedback_can_id_matches &&
        joint_rx_debug.right_back.feedback_can_id_matches;
}

static void JointRxOnlyProbe(void)
{
    if (!lf->rx_only || !lb->rx_only || !rf->rx_only || !rb->rx_only)
        return;

    if (joint_rx_probe_start_elapsed < JOINT_RX_PROBE_START_DELAY)
    {
        joint_rx_probe_start_elapsed += del_t;
        return;
    }

    joint_rx_probe_elapsed += del_t;
    if (joint_rx_probe_elapsed < JOINT_RX_PROBE_INTERVAL)
        return;

    joint_rx_probe_elapsed = 0.0f;
    joint_rx_debug.disable_probe_count++;
    if (!DMMotorSendDisable(joint[joint_rx_probe_index]))
        joint_rx_debug.disable_probe_fail_count++;

    joint_rx_probe_index++;
    if (joint_rx_probe_index >= JOINT_CNT)
        joint_rx_probe_index = 0u;
}

static void ClearMotionTargets(void)
{
    chassis.target_v = 0.0f;
    chassis.target_dist = chassis.dist;
    stool_target_wz = 0.0f;
    stool_spin_enabled = 0;
}

static void ClearControlOutput(void)
{
    l_side.F_leg = 0.0f;
    l_side.T_hip = 0.0f;
    l_side.T_back = 0.0f;
    l_side.T_front = 0.0f;
    l_side.T_wheel = 0.0f;

    r_side.F_leg = 0.0f;
    r_side.T_hip = 0.0f;
    r_side.T_back = 0.0f;
    r_side.T_front = 0.0f;
    r_side.T_wheel = 0.0f;
}

static void ClearJointOutput(void)
{
    l_side.F_leg = 0.0f;
    l_side.T_hip = 0.0f;
    l_side.T_back = 0.0f;
    l_side.T_front = 0.0f;

    r_side.F_leg = 0.0f;
    r_side.T_hip = 0.0f;
    r_side.T_back = 0.0f;
    r_side.T_front = 0.0f;
}

static void ApproachFloat(float *value, float target, float step)
{
    if (*value < target)
    {
        *value += step;
        if (*value > target)
            *value = target;
    }
    else if (*value > target)
    {
        *value -= step;
        if (*value < target)
            *value = target;
    }
}

static float RCChannelToRatio(int16_t ch)
{
    if (ch > -STOOL_RC_DEADBAND && ch < STOOL_RC_DEADBAND)
        return 0.0f;
    return float_constrain((float)ch / 660.0f, -1.0f, 1.0f);
}

static void ResetStoolRuntime(void)
{
    stool_drive_key_pressed = 0;
    stool_vel = 0.0f;
    stool_dist = chassis.dist;
    stool_stop_dist = stool_dist;
    ClearMotionTargets();
    ClearControlOutput();
}

static void DisableAllJointMotor(void)
{
    for (uint8_t i = 0; i < JOINT_CNT; i++)
    {
        DMMotorSetRef(joint[i], 0.0f);
        DMMotorOuterLoop(joint[i], OPEN_LOOP);
        DMMotorStop(joint[i]);
    }
}

static void RefreshJointMotorDisable(void)
{
    DisableAllJointMotor();
}

static void EnableDrivenMotor(void)
{
    for (uint8_t i = 0; i < DRIVEN_CNT; i++)
        LKMotorEnable(driven[i]);
}

static void StopDrivenMotor(void)
{
    for (uint8_t i = 0; i < DRIVEN_CNT; i++)
    {
        LKMotorSetRef(driven[i], 0.0f);
        LKMotorStop(driven[i]);
    }
}

void BalanceInit()
{
    rc_data = RemoteControlInit(&huart3);
    Chassis_IMU_data = INS_Init();
    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); // ww超级电容初始化
    // 关节电机
    Motor_Init_Config_s joint_conf = {
        // 写一个,剩下的修改方向和id即可
        .can_init_config = {
            .can_handle = &hcan1},
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.1,
                .Kd = 0,
                .Ki = 0,
                .DeadBand = 0.0001,
                .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
                .MaxOut = 4,
                .Derivative_LPF_RC = 0.05,
            }, // 仅用于复位腿
        },
        .controller_setting_init_config = {
            .close_loop_type = ANGLE_LOOP,
            .outer_loop_type = OPEN_LOOP,
            .motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
        },
        .motor_type = DM8009P};
    joint_conf.can_init_config.tx_id = JOINT_LF_MOTOR_ID;
    joint_conf.can_init_config.rx_id = JOINT_LF_MASTER_ID;
    joint[LF] = lf = DMMotorInit(&joint_conf);
    joint_conf.can_init_config.tx_id = JOINT_LB_MOTOR_ID;
    joint_conf.can_init_config.rx_id = JOINT_LB_MASTER_ID;
    joint[LB] = lb = DMMotorInit(&joint_conf);
    joint_conf.can_init_config.tx_id = JOINT_RF_MOTOR_ID;
    joint_conf.can_init_config.rx_id = JOINT_RF_MASTER_ID;
    joint[RF] = rf = DMMotorInit(&joint_conf);
    joint_conf.can_init_config.tx_id = JOINT_RB_MOTOR_ID;
    joint_conf.can_init_config.rx_id = JOINT_RB_MASTER_ID;
    joint[RB] = rb = DMMotorInit(&joint_conf);
    for (uint8_t i = 0; i < JOINT_CNT; i++)
        DMMotorSetRxOnly(joint[i], JOINT_RX_ONLY_TEST);

    // 驱动轮电机
    Motor_Init_Config_s driven_conf = {
        // 写一个,剩下的修改方向和id即可
        .can_init_config.can_handle = &hcan2,
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = OPEN_LOOP,
            .close_loop_type = OPEN_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = LK9025,

    };
    driven_conf.can_init_config.tx_id = 1;
    driven[LD] = l_driven = LKMotorInit(&driven_conf);
    driven_conf.can_init_config.tx_id = 2;
    driven[RD] = r_driven = LKMotorInit(&driven_conf);

    // 腿长控制
    PID_Init_Config_s leg_length_pid_conf = {
        .Kp = 1200,
        .Kd = 300,
        .Ki = 0,
        .MaxOut = 60,
        .DeadBand = 0.0001f,
        .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
        .Derivative_LPF_RC = 0.05,
    };
    PIDInit(&leglen_pid_l, &leg_length_pid_conf);
    PIDInit(&leglen_pid_r, &leg_length_pid_conf);

    // roll轴补偿
    PID_Init_Config_s roll_compensate_pid_conf = {
        .Kp = 0.0008f,
        .Kd = 0.0002f,
        .Ki = 0.0f,
        .MaxOut = 0.05,
        .DeadBand = 0.001f,
        .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
        .Derivative_LPF_RC = 0.05,
    };
    PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);

    // 航向控制
    // 角度环
    PID_Init_Config_s steer_p_pid_conf = {
        .Kp = 5,
        .Kd = 0,
        .Ki = 0.0f,
        .MaxOut = 3,
        .DeadBand = 0.001f,
        .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
        .Derivative_LPF_RC = 0.05,
    };
    PIDInit(&steer_p_pid, &steer_p_pid_conf);
    // 速度环
    PID_Init_Config_s steer_v_pid_conf = {
        .Kp = 3,
        .Kd = 0.0f,
        .Ki = 0.0f,
        .MaxOut = 20,
        .DeadBand = 0.0f,
        .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
        .Derivative_LPF_RC = 0.05,
    };
    PIDInit(&steer_v_pid, &steer_v_pid_conf);

    // 抗劈叉
    PID_Init_Config_s anti_crash_pid_conf = {
        .Kp = 15,
        .Kd = 2,
        .Ki = 0.0,
        .MaxOut = 30,
        .DeadBand = 0.001f,
        .Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit,
        .Derivative_LPF_RC = 0.01,
    };
    PIDInit(&anti_crash_pid, &anti_crash_pid_conf);

    // 状态初始化
    l_side.target_len = r_side.target_len = 0.12;
    chassis.vel_cov = 100; // 速度协方差初始化
    chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
    chassis_status = ROBOT_STOP;
    ResetStoolRuntime();
    DisableAllJointMotor();
    // Clear stale enable state if the MCU resets without cycling motor power.
    for (uint8_t i = 0; i < JOINT_CNT; i++)
        DMMotorSetMode(DM_CMD_RESET_MODE, joint[i]);
    StopDrivenMotor();
    DWT_GetDeltaT(&balance_dwt_cnt);
}

// 检查驱动轮电机是否离线
static uint8_t DrivenMotorIsLost()
{
    for (uint8_t i = 0; i < DRIVEN_CNT; i++)
    {
        if (driven[i]->daemon->temp_count == 0)
            return 1;
    }

    return 0;
}

/* Chassis-board local RC control. Right switch down is hard emergency stop. */
static void ControlSwitch()
{
    memset(&chassis_cmd_recv, 0, sizeof(chassis_cmd_recv));
    chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
    chassis_cmd_recv.direction = CHASSIS_ALIGN;
    chassis_cmd_recv.friction_mode = FRICTION_OFF;
    chassis_cmd_recv.loader_mode = LOAD_STOP;
    chassis_cmd_recv.vision_mode = UNLOCK;
    chassis_cmd_recv.ui_mode = UI_KEEP;
    stool_spin_enabled = 0;

    if (!RemoteControlIsOnline())
    {
        return;
    }

    uint8_t switch_right = rc_data[TEMP].rc.switch_right;
    if (!switch_is_up(switch_right) && !switch_is_mid(switch_right))
        return;

    float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001f;
    if (chassis_vol > REFEREE_VOLTAGE_VALID_MIN &&
        chassis_vol < CHASSIS_UNDERVOLTAGE_LIMIT)
        return;

    if (DrivenMotorIsLost())
        return;

    chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE;
    chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF;
    if (switch_is_up(switch_right))
    {
        chassis_cmd_recv.offset_angle = -STOOL_SPIN_YAW_RATE_REF;
        stool_spin_enabled = 1;
    }
    else
    {
        chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;
    }
}

/* Stage 1 keeps all joint motors disabled and receive-only. */
static void ResetChassis()
{
    ClearMotionTargets();
    ClearControlOutput();
    l_side.target_len = r_side.target_len = 0.12f;
    chassis.dist = chassis.target_dist = 0.0f;
    chassis_status = ROBOT_STOP;
    RefreshJointMotorDisable();
    StopDrivenMotor();
}

// 工作状态设定
static void WokingStateSet()
{
    RefreshJointMotorDisable();

    if (last_chassis_mode != chassis_cmd_recv.chassis_mode)
    {
        if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
            ResetStoolRuntime();
        else
            ClearControlOutput();
        last_chassis_mode = chassis_cmd_recv.chassis_mode;
    }

    if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
        chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
    {
        ResetChassis();
        return;
    }

    if (chassis_cmd_recv.chassis_mode != CHASSIS_STOOL_MODE)
    {
        ResetChassis();
        return;
    }

    EnableDrivenMotor();
    chassis_status = ROBOT_READY;
    l_side.target_len = r_side.target_len = 0.12f;
    if (stool_spin_enabled || fabsf(stool_target_wz) > STOOL_YAW_RATE_REF)
        ApproachFloat(&stool_target_wz, chassis_cmd_recv.offset_angle, STOOL_SPIN_YAW_ACC_REF * del_t);
    else
        stool_target_wz = chassis_cmd_recv.offset_angle;

    if (fabsf(chassis_cmd_recv.vx) > 0.001f)
    {
        ApproachFloat(&chassis.target_v, chassis_cmd_recv.vx, STOOL_ACC_REF * del_t);
        stool_stop_dist = stool_dist;
        stool_drive_key_pressed = 1;
    }
    else
    {
        if (stool_drive_key_pressed)
            stool_stop_dist = stool_dist;
        ApproachFloat(&chassis.target_v, 0.0f, STOOL_BRAKE_ACC_REF * del_t);
        stool_drive_key_pressed = 0;
    }
    VAL_LIMIT(chassis.target_v, -STOOL_SPEED_REF, STOOL_SPEED_REF);
    chassis.target_dist = chassis.dist;
}

/**
 * @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
 *
 * @note HT04电机上电的编码器位置为零(校准过),请看Link2Pod()的note,以及HT04.c中的电机解码部分
 * @note 海泰04电机顺时针旋转为正; LK9025电机逆时针旋转为正,此处皆需要转换为模型中给定的正方向
 *
 */
static void ParamAssemble()
{
    // 机体参数,视为平面刚体
    chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
    chassis.pitch_w = Chassis_IMU_data->Gyro[1];
    chassis.yaw = Chassis_IMU_data->YawTotalAngle * DEGREE_2_RAD;
    chassis.wz = Chassis_IMU_data->Gyro[2];
    chassis.roll = Chassis_IMU_data->Roll * DEGREE_2_RAD;
    chassis.roll_w = Chassis_IMU_data->Gyro[1];

    // HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正
    l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_round;
    l_side.phi1_w = -lb->measure.velocity;// 注意速度的正负,HT04电机顺时针旋转为正,而模型中左腿前关节顺时针旋转为负 原本是speed_rads
    l_side.phi4 = -lf->measure.total_round - LIMIT_LINK_RAD;
    l_side.phi4_w = -lf->measure.velocity;
    l_side.w_ecd = l_driven->measure.speed_rads;

    r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_round;
    r_side.phi1_w = rb->measure.velocity;
    r_side.phi4 = rf->measure.total_round - LIMIT_LINK_RAD;
    r_side.phi4_w = rf->measure.velocity;
    r_side.w_ecd = -r_driven->measure.speed_rads;
}

static void StoolModeBalanceControl(void)
{
    float raw_vel = WHEEL_RADIUS * (l_side.w_ecd + r_side.w_ecd) * 0.5f;
    float vel_lpf_alpha = del_t / (STOOL_VEL_LPF_RC + del_t);
    VAL_LIMIT(vel_lpf_alpha, 0.0f, 1.0f);

    stool_vel += vel_lpf_alpha * (raw_vel - stool_vel);
    stool_dist += stool_vel * del_t;

    if (fabsf(chassis.target_v) > 0.001f)
        stool_stop_dist = stool_dist;

    float stop_dist_error = stool_dist - stool_stop_dist;
    float pitch_error = chassis.pitch - STOOL_PITCH_REF;
    float wheel_torque = -STOOL_PITCH_K * pitch_error -
                         STOOL_PITCH_W_K * chassis.pitch_w +
                         STOOL_VEL_K * (stool_vel - chassis.target_v) +
                         STOOL_STOP_DIST_K * stop_dist_error;

    VAL_LIMIT(wheel_torque, -STOOL_WHEEL_TORQUE_LIMIT, STOOL_WHEEL_TORQUE_LIMIT);

    l_side.T_wheel = wheel_torque;
    r_side.T_wheel = wheel_torque;
}

static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
{
    if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
    {
        PIDCalculate(&steer_v_pid, chassis.wz, stool_target_wz);
        l_side.T_wheel -= steer_v_pid.Output;
        r_side.T_wheel += steer_v_pid.Output;
        return;
    }

    if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG ||
        chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随
    {
        float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
        PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
    }
    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺
    {
        PIDCalculate(&steer_v_pid, chassis.wz, (float)chassis_cmd_recv.rotate_w);
    }
    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE_REVERSE)
    {
        PIDCalculate(&steer_v_pid, chassis.wz, (float)chassis_cmd_recv.rotate_w);
    }
    l_side.T_wheel -= steer_v_pid.Output;
    r_side.T_wheel += steer_v_pid.Output;

    // 抗劈叉
    static float swerving_speed_ff, ff_coef = 3;
    swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
    PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
    l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
    r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
}

static void LegControl() /* 腿长控制和Roll补偿 */
{
    PIDCalculate(&roll_compensate_pid, chassis.roll, 0);
    l_side.target_len += roll_compensate_pid.Output;
    r_side.target_len -= roll_compensate_pid.Output;

    static float gravity_ff = 60;
    static float roll_extra_comp_p = 400;
    float roll_comp = roll_extra_comp_p * chassis.roll;
    l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_ff - roll_comp;
    r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_ff + roll_comp;
}

static void WattLimitSet() /* 设定运动模态的输出 */
{
    RefreshJointMotorDisable();

    LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
    LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
}

// 裁判系统,双板通信,电容功率控制等
static void CommNPower()
{
    /* 更新ui数据 */
    ui_data.direction = chassis_cmd_recv.direction;
    ui_data.friction_mode = chassis_cmd_recv.friction_mode;
    ui_data.loader_mode = chassis_cmd_recv.loader_mode;
    ui_data.chassis_mode = chassis_cmd_recv.chassis_mode;
    ui_data.ui_mode = chassis_cmd_recv.ui_mode;
    ui_data.vision_mode = chassis_cmd_recv.vision_mode;
    memcpy(ui_data.coord, l_side.coord, sizeof(l_side.coord));
}

void BalanceTask()
{
    del_t = DWT_GetDeltaT(&balance_dwt_cnt);
    JointRxOnlyProbe();

    BuzzerOn();
    // 切换遥控器控制or云台板控制
    ControlSwitch();
    // 设置目标参数和工作模式
    WokingStateSet();
    // 裁判系统,双板通信,电容功率控制等
    CommNPower();
    // 参数组装
    ParamAssemble();
    UpdateJointRxDebug();

    if (chassis_status == ROBOT_STOP ||
        chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
        chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
        return;

    if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
    {
        StoolModeBalanceControl();
        SynthesizeMotion();
        ClearJointOutput();
        WattLimitSet();
        return;
    }

    // 将五连杆映射成单杆
    Link2Leg(&l_side, &chassis);
    Link2Leg(&r_side, &chassis);
    // 通过卡尔曼滤波估计机体速度
    SpeedEstimation(&l_side, &r_side, &chassis, Chassis_IMU_data, del_t);
    // 根据单杆计算处的角度和杆长,计算反馈增益
    CalcLQR(&l_side, &chassis);
    CalcLQR(&r_side, &chassis);
    // 转向和抗劈叉
    SynthesizeMotion();
    // 腿长控制,保持机体水平
    LegControl();
    // VMC映射成关节输出
    VMCProject(&l_side);
    VMCProject(&r_side);
    // 驱动轮支持力解算
    NormalForceSolve(&l_side, Chassis_IMU_data);
    NormalForceSolve(&r_side, Chassis_IMU_data);

    // 运动模态,电机输出映射和限幅
    WattLimitSet();
}