TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

添加裁判系统串口，修复关节复位bug

Browse Source
This commit is contained in:
kai
2024-04-29 11:38:32 +08:00
parent 8fc322fd51
commit d4d0d0ac54
3 changed files with 52 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
application/chassis/balance.c
View File

@@ -45,11 +45,14 @@ 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
void BalanceInit()
{
rc_data = RemoteControlInit(&huart3);
Chassis_IMU_data = INS_Init();
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
// 关节电机
Motor_Init_Config_s joint_conf = {
@@ -197,7 +200,7 @@ static void ControlSwitch()
else
{
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
chassis_cmd_recv.vx = 0.004 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.delta_leglen = -0.000001f * (float)rc_data[TEMP].rc.dial;
chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
}
@@ -219,14 +222,14 @@ static void ResetChassis()
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
// 若关节完成复位,进入ready态
if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.03 &&
abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.03 &&
abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.03 &&
abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.03)
if (abs(lf->measure.total_angle) < 0.05 &&
abs(lb->measure.total_angle) < 0.05 &&
abs(rf->measure.total_angle) < 0.05 &&
abs(rb->measure.total_angle) < 0.05)
{
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
}
@@ -303,7 +306,6 @@ static void WokingStateSet()
// TODO 最大dist误差限幅
// TODO 最大速度误差限幅
}

2
application/chassis/balance.h
View File

@@ -8,7 +8,7 @@
#define LIMIT_LINK_RAD 0.220039368 // 初始限位角度,见ParamAssemble
#define BALANCE_GRAVITY_BIAS 0
#define ROLL_GRAVITY_BIAS 0
#define MAX_ACC_REF 0.8f
#define MAX_ACC_REF 0.9f
#define MAX_DIST_TRACK 1.0f
#define MAX_VEL_TRACK 0.5f