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

添加腿部关节复位

Browse Source
This commit is contained in:
kai
2024-03-24 13:23:10 +08:00
parent 729df0f988
commit 57723a3b67
1 changed files with 61 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
application/chassis/balance.c
View File

@@ -50,8 +50,19 @@ void BalanceInit()
// 写一个,剩下的修改方向和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 | PID_Derivative_On_Measurement,
.MaxOut = 4,
.Derivative_LPF_RC = 0.05,
}, // 仅用于复位腿
},
.controller_setting_init_config = {
.close_loop_type = OPEN_LOOP,
.close_loop_type = ANGLE_LOOP,
.outer_loop_type = OPEN_LOOP,
.motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
.angle_feedback_source = MOTOR_FEED,
@@ -111,7 +122,7 @@ static void ControlSwitch()
if (rc_data->rc.rocker_l1 < -600)
{
chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
chassis_cmd_recv.vx = 0.005 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.vx = 0.05 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
}
else
{
@@ -124,10 +135,56 @@ static void ControlSwitch()
}
/* 腿缩回复位,只允许驱动轮电机移动 */
static void ResetChassis()
{
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
// // 撞墙时前后移动保证能重新站立,执行速度输入
// LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
// LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
// 若关节完成复位,进入ready态
if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.02 &&
abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.02 &&
abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.02 &&
abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.02)
{
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
}
else if (abs(lf->measure.total_angle) <= 0.02 &&
abs(lb->measure.total_angle) <= 0.02 &&
abs(rf->measure.total_angle) <= 0.02 &&
abs(rb->measure.total_angle) <= 0.02)
{ // 双阈值保证关节能够复位而不会进入死区
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
return; // 退出函数不再执行关节指令
}
else
chassis_status = ROBOT_STOP;
// 还在复位中,关节改为位置环,执行复位
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
HTMotorOuterLoop(joint[i], ANGLE_LOOP);
HTMotorSetRef(joint[i], 0);
}
}
// 工作状态设定
static void WokingStateSet()
{
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式
{
ResetChassis();
return;
}
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
{
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorStop(joint[i]);
@@ -198,5 +255,5 @@ void BalanceTask()
CalcLQR(&r_side, &chassis);
// 运动模态,电机输出映射和限幅
WattLimitSet();
// WattLimitSet();
}