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

关节电机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

17
bsp/can/bsp_can.c
View File

@@ -105,6 +105,7 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
instance->can_handle = config->can_handle;
instance->tx_id = config->tx_id; // 好像没用,可以删掉
instance->rx_id = config->rx_id;
instance->rx_id_match_any = config->rx_id_match_any;
instance->can_module_callback = config->can_module_callback;
instance->id = config->id;
@@ -141,6 +142,17 @@ uint8_t CANTransmit(CANInstance *_instance, float timeout)
return 1; // 发送成功
}
uint8_t CANTryTransmit(CANInstance *_instance)
{
if (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0u)
return 0u;
return HAL_CAN_AddTxMessage(_instance->can_handle,
&_instance->txconf,
_instance->tx_buff,
&_instance->tx_mailbox) == HAL_OK;
}
void CANSetDLC(CANInstance *_instance, uint8_t length)
{
// 发送长度错误!检查调用参数是否出错,或出现野指针/越界访问
@@ -168,15 +180,16 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff); // 从FIFO中获取数据
for (size_t i = 0; i < idx; ++i)
{ // 两者相等说明这是要找的实例
if (_hcan == can_instance[i]->can_handle && rxconf.StdId == can_instance[i]->rx_id)
if (_hcan == can_instance[i]->can_handle &&
(can_instance[i]->rx_id_match_any || rxconf.StdId == can_instance[i]->rx_id))
{
if (can_instance[i]->can_module_callback != NULL) // 回调函数不为空就调用
{
can_instance[i]->rx_std_id = rxconf.StdId;
can_instance[i]->rx_len = rxconf.DLC; // 保存接收到的数据长度
memcpy(can_instance[i]->rx_buff, can_rx_buff, rxconf.DLC); // 消息拷贝到对应实例
can_instance[i]->can_module_callback(can_instance[i]); // 触发回调进行数据解析和处理
}
break;
}
}
}

5
bsp/can/bsp_can.h
View File

@@ -21,7 +21,9 @@ typedef struct _
uint8_t tx_buff[8]; // 发送缓存,发送消息长度可以通过CANSetDLC()设定,最大为8
uint8_t rx_buff[8]; // 接收缓存,最大消息长度为8
uint32_t rx_id; // 接收id
uint32_t rx_std_id; // 实际收到的标准帧id
uint8_t rx_len; // 接收长度,可能为0-8
uint8_t rx_id_match_any; // 软件层接收该CAN总线上的任意标准帧
// 接收的回调函数,用于解析接收到的数据
void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones
void *id; // 使用can外设的模块指针(即id指向的模块拥有此can实例,是父子关系)
@@ -34,6 +36,7 @@ typedef struct
CAN_HandleTypeDef *can_handle; // can句柄
uint32_t tx_id; // 发送id
uint32_t rx_id; // 接收id
uint8_t rx_id_match_any; // 忽略rx_id,由模块自行解析报文id
void (*can_module_callback)(CANInstance *); // 处理接收数据的回调函数
void *id; // 拥有can实例的模块地址,用于区分不同的模块(如果有需要的话),如果不需要可以不传入
} CAN_Init_Config_s;
@@ -65,4 +68,6 @@ void CANSetDLC(CANInstance *_instance, uint8_t length);
*/
uint8_t CANTransmit(CANInstance *_instance,float timeout);
uint8_t CANTryTransmit(CANInstance *_instance);
#endif

51
modules/motor/DMmotor/dmmotor.c
View File

@@ -7,7 +7,7 @@
#include "stdlib.h"
#include "bsp_log.h"
#define DM_SEND_DELAY 0.2
#define DM_MODE_SEND_DELAY_S 0.01f
static uint8_t idx;
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
@@ -27,12 +27,20 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
static uint8_t DMMotorTransmitMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
{
memset(motor->motor_can_instace->tx_buff, 0xff, 7u);
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd;
return CANTransmit(motor->motor_can_instace, 1);
}
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
{
memset(motor->motor_can_instace->tx_buff, 0xff, 7);
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd;
CANTransmit(motor->motor_can_instace, 1);
DWT_Delay(DM_SEND_DELAY);
if (motor->rx_only && cmd != DM_CMD_RESET_MODE)
return;
DMMotorTransmitMode(cmd, motor);
DWT_Delay(DM_MODE_SEND_DELAY_S);
}
static void DMMotorDecode(CANInstance *motor_can)
@@ -42,9 +50,17 @@ static void DMMotorDecode(CANInstance *motor_can)
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
DM_Motor_Measure_s *measure = &(motor->measure);
if (motor_can->rx_len != 8u)
return;
uint8_t feedback_id = rxbuff[0] & 0x0fu;
if (feedback_id != (motor_can->tx_id & 0x0fu))
return;
DaemonReload(motor->motor_daemon);
measure->id = rxbuff[0];
measure->id = feedback_id;
measure->state = (rxbuff[0] >> 4) & 0xf;
measure->feedback_can_id = motor_can->rx_std_id;
measure->last_position = measure->position;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
@@ -60,6 +76,7 @@ static void DMMotorDecode(CANInstance *motor_can)
measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
measure->T_Mos = (float)rxbuff[6];
measure->T_Rotor = (float)rxbuff[7];
measure->feedback_count++;
}
static void DMMotorLostCallback(void *motor_ptr)
@@ -88,6 +105,7 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
config->can_init_config.can_module_callback = DMMotorDecode;
config->can_init_config.rx_id_match_any = 1u;
config->can_init_config.id = motor;
motor->motor_can_instace = CANRegister(&config->can_init_config);
@@ -109,6 +127,9 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref)
void DMMotorEnable(DMMotorInstance *motor)
{
if (motor->rx_only)
return;
motor->stop_flag = MOTOR_ENALBED;
}
@@ -117,6 +138,21 @@ void DMMotorStop(DMMotorInstance *motor)
motor->stop_flag = MOTOR_STOP;
}
uint8_t DMMotorSendDisable(DMMotorInstance *motor)
{
DMMotorStop(motor);
memset(motor->motor_can_instace->tx_buff, 0xff, 7u);
motor->motor_can_instace->tx_buff[7] = (uint8_t)DM_CMD_RESET_MODE;
return CANTryTransmit(motor->motor_can_instace);
}
void DMMotorSetRxOnly(DMMotorInstance *motor, uint8_t rx_only)
{
motor->rx_only = rx_only != 0u;
if (motor->rx_only)
DMMotorStop(motor);
}
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
{
motor->motor_settings.outer_loop_type = type;
@@ -186,7 +222,8 @@ void DMMotorTask(void const *argument)
motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
CANTransmit(motor->motor_can_instace, 1);
if (!motor->rx_only)
CANTransmit(motor->motor_can_instace, 1);
osDelayUntil(&currentTime, 4); // 4ms 的绝对延时
}

7
modules/motor/DMmotor/dmmotor.h
View File

@@ -21,6 +21,8 @@ typedef struct
{
uint8_t id;
uint8_t state;
volatile uint32_t feedback_count;
uint32_t feedback_can_id;
float velocity;
float last_position;
float position;
@@ -55,6 +57,7 @@ typedef struct
float *current_feedforward_ptr;
float pid_ref;
Motor_Working_Type_e stop_flag;
uint8_t rx_only; // Blocks enable, calibration, and periodic control frames.
CANInstance *motor_can_instace;
DaemonInstance *motor_daemon;
uint32_t lost_cnt;
@@ -74,8 +77,10 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref);
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e closeloop_type);
void DMMotorEnable(DMMotorInstance *motor);
void DMMotorStop(DMMotorInstance *motor);
uint8_t DMMotorSendDisable(DMMotorInstance *motor);
void DMMotorSetRxOnly(DMMotorInstance *motor, uint8_t rx_only);
void DMMotorCaliEncoder(DMMotorInstance *motor);
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
void DMMotorControlInit(void);
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor);
#endif // !DMMOTOR
#endif // !DMMOTOR