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

24对抗赛

Browse Source
This commit is contained in:
chenfu
2024-06-01 00:11:37 +08:00
parent da591dcfb4
commit c1ab3edf88
11 changed files with 136 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
application/chassis/balance.c
View File

@@ -20,7 +20,6 @@
#include "speed_estimation.h"
#include "fly_detection.h"
#include "buzzer.h"
// 计时变量
static uint32_t balance_dwt_cnt;
static float del_t;
@@ -30,7 +29,7 @@ static INS_t *Chassis_IMU_data;
static RC_ctrl_t *rc_data; // 底盘单独调试用
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据
static Chassis_Can_Comm chassis_can_recv;
// 四个关节电机和两个驱动轮电机
static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2];
@@ -290,8 +289,8 @@ static void ResetChassis()
// chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
// 若关节完成复位,进入ready态
if (abs(lf->measure.total_angle) < 0.05 &&
@@ -356,15 +355,15 @@ static void WokingStateSet()
HTMotorOuterLoop(joint[i], OPEN_LOOP);
// 设置目标速度/腿长/距离
l_side.target_len += chassis_cmd_recv.delta_leglen;
r_side.target_len += chassis_cmd_recv.delta_leglen;
l_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
r_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
// 腿长限幅
VAL_LIMIT(l_side.target_len, 0.12, 0.30);
VAL_LIMIT(r_side.target_len, 0.12, 0.30);
// 加速度限幅,防止键盘控制摔倒
chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
VAL_LIMIT(r_side.target_len, 0.002, 2.0);
// VAL_LIMIT(chassis.target_v, 0.002, 2.0);
// 角度输入
if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
{
@@ -420,11 +419,11 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
}
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺
{
PIDCalculate(&steer_v_pid, chassis.wz, chassis_cmd_recv.rotate_w);
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, -chassis_cmd_recv.rotate_w);
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;
@@ -463,16 +462,16 @@ static void WattLimitSet() /* 设定运动模态的输出 */
// 裁判系统,双板通信,电容功率控制等
static void CommNPower()
{
static uint8_t supercap_send_cnt = 0;
//static uint8_t supercap_send_cnt = 0;
// CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
supercap_send_cnt++;
if (supercap_send_cnt % 5 == 0)
{
DataSend2Cap[0] = referee_data->PowerHeatData.buffer_energy; // 200hz发送
DataSend2Cap[1] = referee_data->GameRobotState.chassis_power_limit;
SuperCapSend(cap, (uint8_t *)&DataSend2Cap);
supercap_send_cnt = 0;
}
// supercap_send_cnt++;
// if (supercap_send_cnt % 5 == 0)
// {
// DataSend2Cap[0] = referee_data->PowerHeatData.buffer_energy; // 200hz发送
// DataSend2Cap[1] = referee_data->GameRobotState.chassis_power_limit;
// SuperCapSend(cap, (uint8_t *)&DataSend2Cap);
// supercap_send_cnt = 0;
// }
/* 更新ui数据 */
ui_data.direction = chassis_cmd_recv.direction;
ui_data.friction_mode = chassis_cmd_recv.friction_mode;