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

1
modules/motor/motor_def.h
View File

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