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

关节电机measure

Browse Source
This commit is contained in:
TuxMonkey
2026-07-15 21:59:50 +08:00
parent 52813f45d2
commit ae8f1725b1
6 changed files with 227 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
application/chassis/balance.c
View File

@@ -40,6 +40,18 @@
#define STOOL_RC_DEADBAND 40
#define CHASSIS_UNDERVOLTAGE_LIMIT 15.0f
#define REFEREE_VOLTAGE_VALID_MIN 5.0f
// Stage 1 permits only 0xFD safety-disable probes.
#define JOINT_RX_ONLY_TEST 1u
#define JOINT_RX_PROBE_INTERVAL 0.01f
#define JOINT_RX_PROBE_START_DELAY 0.5f
#define JOINT_RB_MOTOR_ID 0x01u
#define JOINT_RF_MOTOR_ID 0x02u
#define JOINT_LB_MOTOR_ID 0x03u
#define JOINT_LF_MOTOR_ID 0x04u
#define JOINT_RB_MASTER_ID 0x11u
#define JOINT_RF_MASTER_ID 0x12u
#define JOINT_LB_MASTER_ID 0x13u
#define JOINT_LF_MASTER_ID 0x14u
// 计时变量
static uint32_t balance_dwt_cnt;
static float del_t;
@@ -52,6 +64,22 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2];
volatile JointMotorRxOnlyDebug_s joint_rx_debug = {
.rx_only = JOINT_RX_ONLY_TEST,
.left_front = {
.expected_motor_id = JOINT_LF_MOTOR_ID,
.expected_feedback_can_id = JOINT_LF_MASTER_ID},
.left_back = {
.expected_motor_id = JOINT_LB_MOTOR_ID,
.expected_feedback_can_id = JOINT_LB_MASTER_ID},
.right_front = {
.expected_motor_id = JOINT_RF_MOTOR_ID,
.expected_feedback_can_id = JOINT_RF_MASTER_ID},
.right_back = {
.expected_motor_id = JOINT_RB_MOTOR_ID,
.expected_feedback_can_id = JOINT_RB_MASTER_ID},
};
// 两个腿的参数,0为左腿,1为右腿
static LinkNPodParam l_side, r_side;
static ChassisParam chassis;
@@ -77,6 +105,83 @@ static float stool_stop_dist;
static float stool_target_wz;
static uint8_t stool_spin_enabled;
static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE;
static float joint_rx_probe_start_elapsed;
static float joint_rx_probe_elapsed;
static uint8_t joint_rx_probe_index;
static void UpdateJointRxDebugMotor(volatile JointMotorRxDebug_s *debug, DMMotorInstance *motor)
{
DM_Motor_Measure_s *measure = &motor->measure;
debug->feedback_motor_id = measure->id;
debug->state = measure->state;
debug->feedback_can_id = measure->feedback_can_id;
debug->feedback_count = measure->feedback_count;
debug->received = measure->feedback_count > 0u;
debug->online = debug->received && DaemonIsOnline(motor->motor_daemon);
debug->id_matches = debug->received &&
measure->id == debug->expected_motor_id;
debug->feedback_can_id_matches = debug->received &&
measure->feedback_can_id == debug->expected_feedback_can_id;
debug->position_rad = measure->position;
debug->velocity_rad_s = measure->velocity;
debug->torque = measure->torque;
debug->mos_temperature_c = measure->T_Mos;
debug->rotor_temperature_c = measure->T_Rotor;
}
static void UpdateJointRxDebug(void)
{
UpdateJointRxDebugMotor(&joint_rx_debug.left_front, lf);
UpdateJointRxDebugMotor(&joint_rx_debug.left_back, lb);
UpdateJointRxDebugMotor(&joint_rx_debug.right_front, rf);
UpdateJointRxDebugMotor(&joint_rx_debug.right_back, rb);
joint_rx_debug.rx_only = lf->rx_only && lb->rx_only &&
rf->rx_only && rb->rx_only;
joint_rx_debug.all_received = joint_rx_debug.left_front.received &&
joint_rx_debug.left_back.received &&
joint_rx_debug.right_front.received &&
joint_rx_debug.right_back.received;
joint_rx_debug.all_online = joint_rx_debug.left_front.online &&
joint_rx_debug.left_back.online &&
joint_rx_debug.right_front.online &&
joint_rx_debug.right_back.online;
joint_rx_debug.all_ids_match = joint_rx_debug.left_front.id_matches &&
joint_rx_debug.left_back.id_matches &&
joint_rx_debug.right_front.id_matches &&
joint_rx_debug.right_back.id_matches;
joint_rx_debug.all_feedback_can_ids_match =
joint_rx_debug.left_front.feedback_can_id_matches &&
joint_rx_debug.left_back.feedback_can_id_matches &&
joint_rx_debug.right_front.feedback_can_id_matches &&
joint_rx_debug.right_back.feedback_can_id_matches;
}
static void JointRxOnlyProbe(void)
{
if (!lf->rx_only || !lb->rx_only || !rf->rx_only || !rb->rx_only)
return;
if (joint_rx_probe_start_elapsed < JOINT_RX_PROBE_START_DELAY)
{
joint_rx_probe_start_elapsed += del_t;
return;
}
joint_rx_probe_elapsed += del_t;
if (joint_rx_probe_elapsed < JOINT_RX_PROBE_INTERVAL)
return;
joint_rx_probe_elapsed = 0.0f;
joint_rx_debug.disable_probe_count++;
if (!DMMotorSendDisable(joint[joint_rx_probe_index]))
joint_rx_debug.disable_probe_fail_count++;
joint_rx_probe_index++;
if (joint_rx_probe_index >= JOINT_CNT)
joint_rx_probe_index = 0u;
}
static void ClearMotionTargets(void)
{
@@ -213,18 +318,20 @@ void BalanceInit()
.speed_feedback_source = MOTOR_FEED,
},
.motor_type = DM8009P};
joint_conf.can_init_config.tx_id = 3;
joint_conf.can_init_config.rx_id = 14;
joint_conf.can_init_config.tx_id = JOINT_LF_MOTOR_ID;
joint_conf.can_init_config.rx_id = JOINT_LF_MASTER_ID;
joint[LF] = lf = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 2;
joint_conf.can_init_config.rx_id = 13;
joint_conf.can_init_config.tx_id = JOINT_LB_MOTOR_ID;
joint_conf.can_init_config.rx_id = JOINT_LB_MASTER_ID;
joint[LB] = lb = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 1;
joint_conf.can_init_config.rx_id = 12;
joint_conf.can_init_config.tx_id = JOINT_RF_MOTOR_ID;
joint_conf.can_init_config.rx_id = JOINT_RF_MASTER_ID;
joint[RF] = rf = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 0;
joint_conf.can_init_config.rx_id = 11;
joint_conf.can_init_config.tx_id = JOINT_RB_MOTOR_ID;
joint_conf.can_init_config.rx_id = JOINT_RB_MASTER_ID;
joint[RB] = rb = DMMotorInit(&joint_conf);
for (uint8_t i = 0; i < JOINT_CNT; i++)
DMMotorSetRxOnly(joint[i], JOINT_RX_ONLY_TEST);
// 驱动轮电机
Motor_Init_Config_s driven_conf = {
@@ -313,6 +420,7 @@ void BalanceInit()
chassis_status = ROBOT_STOP;
ResetStoolRuntime();
DisableAllJointMotor();
// Clear stale enable state if the MCU resets without cycling motor power.
for (uint8_t i = 0; i < JOINT_CNT; i++)
DMMotorSetMode(DM_CMD_RESET_MODE, joint[i]);
StopDrivenMotor();
@@ -373,7 +481,7 @@ static void ControlSwitch()
}
}
/* Joint motors must remain disabled on this demo chassis. */
/* Stage 1 keeps all joint motors disabled and receive-only. */
static void ResetChassis()
{
ClearMotionTargets();
@@ -565,6 +673,7 @@ static void CommNPower()
void BalanceTask()
{
del_t = DWT_GetDeltaT(&balance_dwt_cnt);
JointRxOnlyProbe();
BuzzerOn();
// 切换遥控器控制or云台板控制
@@ -575,6 +684,7 @@ void BalanceTask()
CommNPower();
// 参数组装
ParamAssemble();
UpdateJointRxDebug();
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||

38
application/chassis/balance.h
View File

@@ -38,6 +38,44 @@
#define LD 0u
#define RD 1u
typedef struct
{
uint8_t expected_motor_id;
uint8_t feedback_motor_id;
uint8_t state;
uint8_t received;
uint8_t online;
uint8_t id_matches;
uint8_t feedback_can_id_matches;
uint8_t reserved;
uint32_t expected_feedback_can_id;
uint32_t feedback_can_id;
uint32_t feedback_count;
float position_rad;
float velocity_rad_s;
float torque;
float mos_temperature_c;
float rotor_temperature_c;
} JointMotorRxDebug_s;
typedef struct
{
uint8_t rx_only;
uint8_t all_received;
uint8_t all_online;
uint8_t all_ids_match;
uint8_t all_feedback_can_ids_match;
uint8_t reserved[3];
uint32_t disable_probe_count;
uint32_t disable_probe_fail_count;
JointMotorRxDebug_s left_front;
JointMotorRxDebug_s left_back;
JointMotorRxDebug_s right_front;
JointMotorRxDebug_s right_back;
} JointMotorRxOnlyDebug_s;
extern volatile JointMotorRxOnlyDebug_s joint_rx_debug;
typedef struct
{
// joint