// 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_ACC_REF 4.0f #define STOOL_BRAKE_ACC_REF 8.0f #define STOOL_PITCH_K 24.0f #define STOOL_PITCH_W_K 6.8f #define STOOL_VEL_K 3.2f #define STOOL_STOP_DIST_K 0.0f #define STOOL_PITCH_REF -0.05f #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 // 计时变量 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]; // 两个腿的参数,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 chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE; static void ClearMotionTargets(void) { chassis.target_v = 0.0f; chassis.target_dist = chassis.dist; stool_target_wz = 0.0f; } 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 = 3; joint_conf.can_init_config.rx_id = 14; joint[LF] = lf = DMMotorInit(&joint_conf); joint_conf.can_init_config.tx_id = 2; joint_conf.can_init_config.rx_id = 13; joint[LB] = lb = DMMotorInit(&joint_conf); joint_conf.can_init_config.tx_id = 1; joint_conf.can_init_config.rx_id = 12; joint[RF] = rf = DMMotorInit(&joint_conf); joint_conf.can_init_config.tx_id = 0; joint_conf.can_init_config.rx_id = 11; joint[RB] = rb = DMMotorInit(&joint_conf); // 驱动轮电机 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(); 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; if (!RemoteControlIsOnline()) { return; } if (switch_is_down(rc_data[TEMP].rc.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; chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF; } /* Joint motors must remain disabled on this demo chassis. */ 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; 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); BuzzerOn(); // 切换遥控器控制or云台板控制 ControlSwitch(); // 设置目标参数和工作模式 WokingStateSet(); // 裁判系统,双板通信,电容功率控制等 CommNPower(); // 参数组装 ParamAssemble(); 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(); }