mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-24 03:27:45 +08:00
241 lines
8.0 KiB
C
241 lines
8.0 KiB
C
// app
|
|
#include "balance.h"
|
|
#include "robot_def.h"
|
|
#include "general_def.h"
|
|
#include "ins_task.h"
|
|
#include "HT04.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 "linkNleg.h"
|
|
#include "speed_estimation.h"
|
|
|
|
|
|
static uint32_t balance_dwt_cnt;
|
|
static float del_t;
|
|
|
|
/* 底盘拥有的模块实例 */
|
|
static attitude_t *imu_data;
|
|
static RC_ctrl_t *rc_data; // 底盘单独调试用
|
|
static Referee_Interactive_info_t my_ui;
|
|
static referee_info_t *referee_data;
|
|
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
|
|
static Chassis_Upload_Data_s chassis_feed;
|
|
static CANCommInstance *ci;
|
|
static SuperCapInstance *cap;
|
|
|
|
// 四个关节电机和两个驱动轮电机
|
|
static HTMotorInstance *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 steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
|
|
static PIDInstance anti_crash_pid, phi5_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
|
|
static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
|
|
static PIDInstance roll_compensate_pid, rolldot_pid; // roll轴补偿,用于保持机体水平
|
|
|
|
static Robot_Status_e chassis_status;
|
|
|
|
void BalanceInit()
|
|
{
|
|
rc_data = RemoteControlInit(&huart3);
|
|
imu_data = INS_Init();
|
|
|
|
// 双板通信
|
|
CANComm_Init_Config_s commconf = {
|
|
.can_config = {
|
|
.can_handle = &hcan2,
|
|
.tx_id = 0x40,
|
|
.rx_id = 0x41},
|
|
.recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
|
|
.send_data_len = sizeof(Chassis_Upload_Data_s)};
|
|
ci = CANCommInit(&commconf);
|
|
|
|
// 超级电容
|
|
SuperCap_Init_Config_s cap_conf = {
|
|
.can_config = {
|
|
.can_handle = &hcan2,
|
|
.tx_id = 0x302, // todo 电容id
|
|
.rx_id = 0x301}};
|
|
cap = SuperCapInit(&cap_conf);
|
|
|
|
// 关节电机
|
|
Motor_Init_Config_s joint_conf = {
|
|
// 写一个,剩下的修改方向和id即可
|
|
.can_init_config = {
|
|
.can_handle = &hcan1},
|
|
.controller_param_init_config = {
|
|
.angle_PID = {
|
|
.Kp = 0.3,
|
|
.Kd = 0.1,
|
|
.Ki = 0,
|
|
.DeadBand = 0.0001,
|
|
.Improve = PID_DerivativeFilter,
|
|
.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 = HT04};
|
|
joint_conf.can_init_config.tx_id = 4;
|
|
joint_conf.can_init_config.rx_id = 14;
|
|
joint[LF] = lf = HTMotorInit(&joint_conf);
|
|
joint_conf.can_init_config.tx_id = 3;
|
|
joint_conf.can_init_config.rx_id = 13;
|
|
joint[LB] = lb = HTMotorInit(&joint_conf);
|
|
joint_conf.can_init_config.tx_id = 2;
|
|
joint_conf.can_init_config.rx_id = 12;
|
|
joint[RF] = rf = HTMotorInit(&joint_conf);
|
|
joint_conf.can_init_config.tx_id = 1;
|
|
joint_conf.can_init_config.rx_id = 11;
|
|
joint[RB] = rb = HTMotorInit(&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
|
|
PID_Init_Config_s steer_p_pid_conf = {
|
|
.Kp = 2,
|
|
.Kd = 1,
|
|
.Ki = 0.0f,
|
|
.MaxOut = 4,
|
|
.DeadBand = 0.01f,
|
|
.Improve = PID_DerivativeFilter,
|
|
.Derivative_LPF_RC = 0.05,
|
|
};
|
|
PIDInit(&steer_p_pid, &steer_p_pid_conf);
|
|
PID_Init_Config_s steer_v_pid_conf = {
|
|
.Kp = 2,
|
|
.Kd = 0.0f,
|
|
.Ki = 0.0f,
|
|
.MaxOut = 100,
|
|
.DeadBand = 0.0f,
|
|
.Improve = PID_DerivativeFilter | PID_Integral_Limit,
|
|
.Derivative_LPF_RC = 0.05,
|
|
.IntegralLimit = 2,
|
|
};
|
|
PIDInit(&steer_v_pid, &steer_v_pid_conf);
|
|
|
|
// 抗劈叉
|
|
PID_Init_Config_s anti_crash_pid_conf = {
|
|
.Kp = 8,
|
|
.Kd = 2.5,
|
|
.Ki = 0.4,
|
|
.MaxOut = 45,
|
|
.DeadBand = 0.01f,
|
|
.Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit,
|
|
.Derivative_LPF_RC = 0.05,
|
|
.CoefA = 0.05,
|
|
.CoefB = 0.05,
|
|
.IntegralLimit = 2,
|
|
};
|
|
PIDInit(&anti_crash_pid, &anti_crash_pid_conf);
|
|
|
|
// 腿长控制
|
|
PID_Init_Config_s leg_length_pid_conf = {
|
|
.Kp = 450,
|
|
.Kd = 150,
|
|
.Ki = 5,
|
|
.MaxOut = 60,
|
|
.DeadBand = 0.0001f,
|
|
.Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
|
|
.CoefA = 0.01,
|
|
.CoefB = 0.02,
|
|
.Derivative_LPF_RC = 0.08,
|
|
};
|
|
PIDInit(&leglen_pid_l, &leg_length_pid_conf);
|
|
PIDInit(&leglen_pid_r, &leg_length_pid_conf);
|
|
|
|
// 横滚角补偿
|
|
PID_Init_Config_s roll_compensate_pid_conf = {
|
|
.Kp = 0.0008f,
|
|
.Kd = 0.00065f,
|
|
.Ki = 0.0f,
|
|
.MaxOut = 0.04,
|
|
.DeadBand = 0.005f,
|
|
.Improve = PID_DerivativeFilter,
|
|
.Derivative_LPF_RC = 0.05,
|
|
};
|
|
PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);
|
|
|
|
l_side.target_len = r_side.target_len = 0.23; // 初始腿长
|
|
chassis.vel_cov = 1000; // 初始化速度协方差
|
|
chassis_status = ROBOT_READY;
|
|
DWT_GetDeltaT(&balance_dwt_cnt);
|
|
}
|
|
|
|
static void EnableAllMotor() /* 打开所有电机 */
|
|
{
|
|
for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机
|
|
HTMotorEnable(joint[i]);
|
|
for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机
|
|
LKMotorEnable(driven[i]);
|
|
}
|
|
|
|
/* 切换底盘遥控器控制和云台双板控制 */
|
|
static void ControlSwitch()
|
|
{
|
|
// 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
|
|
if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline())
|
|
{
|
|
if (rc_data->rc.rocker_l1 < -600)
|
|
{
|
|
chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
|
|
chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
|
|
}
|
|
else // 设定值覆盖双板
|
|
{
|
|
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
|
|
chassis_cmd_recv.vx = 0.02 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
|
|
chassis_cmd_recv.offset_angle = 0.001 * (float)rc_data[TEMP].rc.rocker_r_; // rotate? follow.
|
|
chassis_cmd_recv.delta_leglen = -0.0000015f * (float)rc_data[TEMP].rc.dial;
|
|
}
|
|
}
|
|
else if (CANCommIsOnline(ci) && !switch_is_down(rc_data->rc.switch_right))
|
|
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(ci); // 获取云台板指令
|
|
else
|
|
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
|
|
}
|
|
|
|
|
|
void BalanceTask()
|
|
{
|
|
del_t = DWT_GetDeltaT(&balance_dwt_cnt);
|
|
|
|
// 切换遥控器控制or云台板控制
|
|
ControlSwitch();
|
|
|
|
} |