robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-11-02 04:23:10 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

修复rm

Browse Source
This commit is contained in:
Robofish 2025-09-20 16:03:14 +08:00
parent 92bb89124b
commit a29e097978
2 changed files with 51 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
assets/User_code/device/motor_rm.c
View File

@ -71,6 +71,15 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
return DEVICE_ERR; return DEVICE_ERR;
} }
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return 36.0f;
case MOTOR_M3508: return 19.0f;
case MOTOR_GM6020: return 1.0f;
default: return 1.0f;
}
}
static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) { static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) {
switch (module) { switch (module) {
case MOTOR_M2006: return M2006_MAX_ABS_LSB; case MOTOR_M2006: return M2006_MAX_ABS_LSB;
@ -97,12 +106,40 @@ static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) {
static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) { static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]); uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]);
int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]); int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]);
motor->motor.feedback.rotor_abs_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI;
motor->motor.feedback.rotor_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]);
int16_t lsb = MOTOR_RM_GetLSB(motor->param.module); int16_t lsb = MOTOR_RM_GetLSB(motor->param.module);
motor->motor.feedback.torque_current = raw_current * lsb / (float)MOTOR_CUR_RES; float ratio = MOTOR_RM_GetRatio(motor->param.module);
motor->motor.feedback.temp = msg->data[6];
float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI;
float rotor_speed = raw_speed;
float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES;
if (motor->param.gear) {
// 多圈累加
int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle;
if (delta > (MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count--;
} else if (delta < -(MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count++;
}
motor->last_raw_angle = raw_angle;
float single_turn = rotor_angle;
motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio;
// 输出轴多圈绝对值
motor->feedback.rotor_abs_angle = motor->gearbox_total_angle;
motor->feedback.rotor_speed = rotor_speed / ratio;
motor->feedback.torque_current = torque_current * ratio;
} else {
// 非gear模式，直接用转子单圈
motor->gearbox_round_count = 0;
motor->last_raw_angle = raw_angle;
motor->gearbox_total_angle = 0.0f;
motor->feedback.rotor_abs_angle = rotor_angle;
motor->feedback.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current;
}
motor->feedback.temp = msg->data[6];
} }
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */

12
assets/User_code/device/motor_rm.h
View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include "motor.h"
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
@ -44,12 +45,19 @@ typedef struct {
uint16_t id; uint16_t id;
MOTOR_RM_Module_t module; MOTOR_RM_Module_t module;
bool reverse; bool reverse;
bool gear;
} MOTOR_RM_Param_t; } MOTOR_RM_Param_t;
/*电机实例*/ typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t;
typedef struct MOTOR_RM_t {
typedef struct {
MOTOR_RM_Param_t param; MOTOR_RM_Param_t param;
MOTOR_RM_Feedback_t feedback;
MOTOR_t motor; MOTOR_t motor;
// 多圈相关变量，仅gear模式下有效
uint16_t last_raw_angle;
int32_t gearbox_round_count;
float gearbox_total_angle;
} MOTOR_RM_t; } MOTOR_RM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/ /*CAN管理器，管理一个CAN总线上所有的电机*/