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

damiao?

Browse Source
This commit is contained in:
TuxMonkey
2026-04-10 21:36:02 +08:00
parent fb6eb3b4a8
commit ee893d1095
3 changed files with 38 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
application/chassis/balance.c
View File

@@ -4,7 +4,8 @@
#include "robot_def.h"
#include "general_def.h"
#include "ins_task.h"
#include "HT04.h"
// #include "HT04.h"
#include "dmmotor.h"
#include "LK9025.h"
#include "controller.h"
#include "can_comm.h"
@@ -31,7 +32,7 @@ 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 DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2];
// 两个腿的参数,0为左腿,1为右腿
@@ -102,19 +103,19 @@ void BalanceInit()
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
},
.motor_type = HT04};
.motor_type = DM8009P};
joint_conf.can_init_config.tx_id = 1;
joint_conf.can_init_config.rx_id = 11;
joint[LF] = lf = HTMotorInit(&joint_conf);
joint[LF] = lf = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 2;
joint_conf.can_init_config.rx_id = 12;
joint[LB] = lb = HTMotorInit(&joint_conf);
joint[LB] = lb = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 3;
joint_conf.can_init_config.rx_id = 13;
joint[RF] = rf = HTMotorInit(&joint_conf);
joint[RF] = rf = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 4;
joint_conf.can_init_config.rx_id = 14;
joint[RB] = rb = HTMotorInit(&joint_conf);
joint[RB] = rb = DMMotorInit(&joint_conf);
// 驱动轮电机
Motor_Init_Config_s driven_conf = {
@@ -206,7 +207,7 @@ void BalanceInit()
static void EnableAllMotor() /* 打开所有电机 */
{
for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机
HTMotorEnable(joint[i]);
DMMotorEnable(joint[i]);
for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机
LKMotorEnable(driven[i]);
}
@@ -293,22 +294,22 @@ static void ResetChassis()
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
// 若关节完成复位,进入ready态
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)
if (abs(lf->measure.total_round) < 0.05 &&
abs(lb->measure.total_round) < 0.05 &&
abs(rf->measure.total_round) < 0.05 &&
abs(rb->measure.total_round) < 0.05)
{
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
}
else if (abs(lf->measure.total_angle) <= 0.03 &&
abs(lb->measure.total_angle) <= 0.03 &&
abs(rf->measure.total_angle) <= 0.03 &&
abs(rb->measure.total_angle) <= 0.03)
else if (abs(lf->measure.total_round) <= 0.03 &&
abs(lb->measure.total_round) <= 0.03 &&
abs(rf->measure.total_round) <= 0.03 &&
abs(rb->measure.total_round) <= 0.03)
{ // 双阈值保证关节能够复位而不会进入死区
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
DMMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
return; // 退出函数不再执行关节指令
}
@@ -318,8 +319,8 @@ static void ResetChassis()
// 还在复位中,关节改为位置环,执行复位
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
HTMotorOuterLoop(joint[i], ANGLE_LOOP);
HTMotorSetRef(joint[i], 0);
DMMotorOuterLoop(joint[i], ANGLE_LOOP);
DMMotorSetRef(joint[i], 0);
}
}
@@ -342,7 +343,7 @@ static void WokingStateSet()
// chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorStop(joint[i]);
DMMotorStop(joint[i]);
for (uint8_t i = 0; i < DRIVEN_CNT; i++)
LKMotorStop(driven[i]);
return; // 关闭所有电机,发送的指令为零
@@ -352,7 +353,7 @@ static void WokingStateSet()
EnableAllMotor();
// 保证关节电机为开环扭矩控制
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorOuterLoop(joint[i], OPEN_LOOP);
DMMotorOuterLoop(joint[i], OPEN_LOOP);
// 设置目标速度/腿长/距离
l_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
@@ -396,16 +397,16 @@ static void ParamAssemble()
chassis.roll_w = Chassis_IMU_data->Gyro[1];
// HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正
l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_angle;
l_side.phi1_w = -lb->measure.speed_rads;
l_side.phi4 = -lf->measure.total_angle - LIMIT_LINK_RAD;
l_side.phi4_w = -lf->measure.speed_rads;
l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_round;
l_side.phi1_w = -lb->measure.velocity;// 注意速度的正负,HT04电机顺时针旋转为正,而模型中左腿前关节顺时针旋转为负 原本是speed_rads
l_side.phi4 = -lf->measure.total_round - LIMIT_LINK_RAD;
l_side.phi4_w = -lf->measure.velocity;
l_side.w_ecd = l_driven->measure.speed_rads;
r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_angle;
r_side.phi1_w = rb->measure.speed_rads;
r_side.phi4 = rf->measure.total_angle - LIMIT_LINK_RAD;
r_side.phi4_w = rf->measure.speed_rads;
r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_round;
r_side.phi1_w = rb->measure.velocity;
r_side.phi4 = rf->measure.total_round - LIMIT_LINK_RAD;
r_side.phi4_w = rf->measure.velocity;
r_side.w_ecd = -r_driven->measure.speed_rads;
}
@@ -451,10 +452,11 @@ static void LegControl() /* 腿长控制和Roll补偿 */
static void WattLimitSet() /* 设定运动模态的输出 */
{
HTMotorSetRef(lf, 0.2857f * -l_side.T_front); // 根据扭矩常数计算得到的系数
HTMotorSetRef(lb, 0.2857f * -l_side.T_back);
HTMotorSetRef(rf, 0.2857f * r_side.T_front);
HTMotorSetRef(rb, 0.2857f * r_side.T_back);
DMMotorSetRef(lf, 0.2857f * -l_side.T_front); // 根据扭矩常数计算得到的系数 todo 需修改
DMMotorSetRef(lb, 0.2857f * -l_side.T_back);
DMMotorSetRef(rf, 0.2857f * r_side.T_front);
DMMotorSetRef(rb, 0.2857f * r_side.T_back);
LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
}

4
application/robot_def.h
View File

@@ -18,8 +18,8 @@
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
// #define ONE_BOARD // 单板控制整车
// #define CHASSIS_BOARD //底盘板
#define GIMBAL_BOARD //云台板
#define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
// #define VISION_USE_UART // 使用串口发送视觉数据

1
modules/motor/motor_def.h
View File

@@ -106,6 +106,7 @@ typedef enum
M2006,
LK9025,
HT04,
DM8009P,
} Motor_Type_e;
/**