TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

云台跟随，连续发射

Browse Source
This commit is contained in:
chenfu
2024-05-20 22:22:24 +08:00
parent 4f80821435
commit 0c0d1201ea
12 changed files with 658 additions and 193 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
application/chassis/balance.c
View File

@@ -19,18 +19,18 @@
#include "lqr_calc.h"
#include "speed_estimation.h"
#include "fly_detection.h"
#include "buzzer.h"
// 计时变量
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 RC_ctrl_t *rc_data; // 底盘单独调试用
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据
// 四个关节电机和两个驱动轮电机
static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2];
// 两个腿的参数,0为左腿,1为右腿
@@ -38,23 +38,35 @@ 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 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_info_t *referee_data; // 用于获取裁判系统的数据
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
void BalanceInit()
{
rc_data = RemoteControlInit(&huart3);
Chassis_IMU_data = INS_Init();
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
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),
};
cmd_can_comm = CANCommInit(&comm_conf);
// 关节电机
Motor_Init_Config_s joint_conf = {
// 写一个,剩下的修改方向和id即可
@@ -173,7 +185,7 @@ void BalanceInit()
// 状态初始化
l_side.target_len = r_side.target_len = 0.12;
chassis.vel_cov = 100; // 速度协方差初始化
chassis.vel_cov = 100; // 速度协方差初始化
chassis_status = ROBOT_READY;
DWT_GetDeltaT(&balance_dwt_cnt);
}
@@ -191,7 +203,7 @@ static uint8_t JointMotorIsLost()
{
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
if(joint[i]->motor_daemon->temp_count == 0)
if (joint[i]->motor_daemon->temp_count == 0)
return 1;
}
@@ -201,9 +213,9 @@ static uint8_t JointMotorIsLost()
// 检查驱动轮电机是否离线
static uint8_t DrivenMotorIsLost()
{
for(uint8_t i = 0; i < DRIVEN_CNT; i++)
for (uint8_t i = 0; i < DRIVEN_CNT; i++)
{
if(driven[i]->daemon->temp_count == 0)
if (driven[i]->daemon->temp_count == 0)
return 1;
}
@@ -212,7 +224,7 @@ static uint8_t DrivenMotorIsLost()
/* 切换底盘遥控器控制和云台双板控制 */
static void ControlSwitch()
{
{
// 根据裁判系统底盘输出电压设定底盘状态
float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001;
if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost())
@@ -232,21 +244,22 @@ static void ControlSwitch()
}
else
{
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial;
chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
}
}
else
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
{
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
}
}
/* 腿缩回复位,只允许驱动轮电机移动 */
static void ResetChassis()
{
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
// 目标速度置0
chassis.target_v = 0;
@@ -278,7 +291,7 @@ static void ResetChassis()
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
return; // 退出函数不再执行关节指令
return; // 退出函数不再执行关节指令
}
else
chassis_status = ROBOT_STOP;
@@ -291,7 +304,6 @@ static void ResetChassis()
}
}
// 工作状态设定
static void WokingStateSet()
{
@@ -334,8 +346,11 @@ static void WokingStateSet()
chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
// 角度输入
chassis.target_yaw = chassis_cmd_recv.offset_angle;
if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
{
chassis.target_yaw = chassis_cmd_recv.offset_angle;
}
chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle*DEGREE_2_RAD;
// TODO 转向速度限幅
// TODO 最大dist误差限幅
@@ -343,7 +358,6 @@ static void WokingStateSet()
// TODO 最大速度误差限幅
}
/**
* @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
*
@@ -352,7 +366,7 @@ static void WokingStateSet()
*
*/
static void ParamAssemble()
{
{
// 机体参数,视为平面刚体
chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
chassis.pitch_w = Chassis_IMU_data->Gyro[0];
@@ -375,16 +389,11 @@ static void ParamAssemble()
r_side.w_ecd = -r_driven->measure.speed_rads;
}
static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
{
if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
{
// 双环控制
float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
}
float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
l_side.T_wheel -= steer_v_pid.Output;
r_side.T_wheel += steer_v_pid.Output;
@@ -396,7 +405,6 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
}
static void LegControl() /* 腿长控制和Roll补偿 */
{
PIDCalculate(&roll_compensate_pid, chassis.roll, 0);
@@ -423,7 +431,7 @@ static void WattLimitSet() /* 设定运动模态的输出 */
void BalanceTask()
{
del_t = DWT_GetDeltaT(&balance_dwt_cnt);
BuzzerOn();
// 切换遥控器控制or云台板控制
ControlSwitch();
// 设置目标参数和工作模式
@@ -457,4 +465,5 @@ void BalanceTask()
// 运动模态,电机输出映射和限幅
WattLimitSet();
CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
}