robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-09-14 12:54:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

修复灵足bug

Browse Source
This commit is contained in:
Robofish 2025-08-30 15:11:10 +08:00
parent d6eaed5f72
commit 70d3add6da
6 changed files with 66 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
assets/User_code/device/dm_imu.c
View File

@ -117,21 +117,26 @@ static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t l
/**
* @brief 初始化DM IMU设备
*/
int8_t DM_IMU_Init(DM_IMU_t *imu, BSP_CAN_t can) {
if (imu == NULL) {
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
if (imu == NULL || param == NULL) {
return DEVICE_ERR_NULL;
}
// 初始化设备头部
imu->header.online = false;
imu->header.last_online_time = 0;
imu->can = can;
// 配置参数
imu->param.can = param->can;
imu->param.can_id = param->can_id;
imu->param.device_id = param->device_id;
imu->param.master_id = param->master_id;
// 清零数据
memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
// 注册CAN接收队列，用于接收回复报文
int8_t result = BSP_CAN_RegisterId(imu->can, DM_IMU_MST_ID, 10);
int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
if (result != BSP_OK) {
return DEVICE_ERR;
}
@ -149,20 +154,20 @@ int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
// 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
uint8_t tx_data[4] = {
DM_IMU_ID & 0xFF, // id_L
(DM_IMU_ID >> 8) & 0xFF, // id_H
(uint8_t)rid, // RID
0xCC // 固定值
imu->param.device_id & 0xFF, // id_L
(imu->param.device_id >> 8) & 0xFF, // id_H
(uint8_t)rid, // RID
0xCC // 固定值
};
// 发送标准数据帧
BSP_CAN_StdDataFrame_t frame = {
.id = DM_IMU_CAN_ID,
.id = imu->param.can_id,
.dlc = 4,
};
memcpy(frame.data, tx_data, 4);
int8_t result = BSP_CAN_TransmitStdDataFrame(imu->can, &frame);
int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
}
@ -179,7 +184,7 @@ int8_t DM_IMU_Update(DM_IMU_t *imu) {
bool data_received = false;
// 持续接收所有可用消息
while ((result = BSP_CAN_GetMessage(imu->can, DM_IMU_MST_ID, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
// 验证回复数据格式（至少检查数据长度）
if (msg.dlc < 3) {
continue; // 跳过无效消息

18
assets/User_code/device/dm_imu.h
View File

@ -10,12 +10,19 @@ extern "C" {
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define DM_IMU_CAN_ID 0x6FF
#define DM_IMU_ID 0x42
#define DM_IMU_MST_ID 0x43
#define DM_IMU_CAN_ID_DEFAULT 0x6FF
#define DM_IMU_ID_DEFAULT 0x42
#define DM_IMU_MST_ID_DEFAULT 0x43
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
BSP_CAN_t can; // CAN总线句柄
uint16_t can_id; // CAN通信ID
uint8_t device_id; // 设备ID
uint8_t master_id; // 主机ID
} DM_IMU_Param_t;
typedef enum {
RID_ACCL = 0x01, // 加速度计
RID_GYRO = 0x02, // 陀螺仪
@ -33,7 +40,7 @@ typedef struct {
typedef struct {
DEVICE_Header_t header;
BSP_CAN_t can;
DM_IMU_Param_t param; // IMU参数配置
DM_IMU_Data_t data; // IMU数据
} DM_IMU_t;
@ -42,9 +49,10 @@ typedef struct {
/**
* @brief 初始化DM IMU设备
* @param imu DM IMU设备结构体指针
* @param param IMU参数配置指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Init(DM_IMU_t *imu, BSP_CAN_t can);
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
/**
* @brief 请求IMU数据

6
assets/User_code/device/led.c
View File

@ -1,5 +1,5 @@
/*
led控制
led控制
*/
/*Includes -----------------------------------------*/
#include "device/led.h"
@ -23,7 +23,7 @@ int8_t BSP_LED_Set(char sign,DEVICE_LED_t ch,bool value,float duty_cycle)
case 'p':
case 'P':
if (duty_cycle < 0.0f || duty_cycle > 1.0f) {
return DEVICE_ERR_NULL; // 错误：占空比超出范围
return DEVICE_ERR_NULL; // 错误：占空比超出范围
}
uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
BSP_PWM_Start(LED_Map.channel);
@ -35,7 +35,7 @@ int8_t BSP_LED_Set(char sign,DEVICE_LED_t ch,bool value,float duty_cycle)
BSP_GPIO_WritePin(LED_Map.gpio,value);
break;
default:
return DEVICE_ERR_INITED; // 错误：无效的控制方式
return DEVICE_ERR_INITED; // 错误：无效的控制方式
}
return DEVICE_OK;
}

8
assets/User_code/device/motor_lk.c
View File

@ -44,8 +44,8 @@ static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private functions -------------------------------------------------------- */
static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
switch (module) {
case MOTOR_MF9025:
case MOTOR_MF9035:
case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035:
return LK_CURR_LSB_MF;
default:
return LK_CURR_LSB_MG; // 默认使用MG的分辨率
@ -89,8 +89,8 @@ static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
// 根据电机类型解析电流或功率
switch (motor->param.module) {
case MOTOR_MF9025:
case MOTOR_MF9035:
case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035:
// MF/MG电机：转矩电流值
motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
break;

4
assets/User_code/device/motor_lk.h
View File

@ -15,8 +15,8 @@ extern "C" {
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_MF9025,
MOTOR_MF9035,
MOTOR_LK_MF9025,
MOTOR_LK_MF9035,
} MOTOR_LK_Module_t;

48
assets/User_code/device/motor_lz.c
View File

@ -187,20 +187,20 @@ static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
// 解析数据区
// Byte0~1: 当前角度
uint16_t raw_angle = (uint16_t)((msg->data[1] << 8) | msg->data[0]);
// Byte0~1: 当前角度 (高字节在前，低字节在后)
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
motor->lz_feedback.current_angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
// Byte2~3: 当前角速度
uint16_t raw_velocity = (uint16_t)((msg->data[3] << 8) | msg->data[2]);
// Byte2~3: 当前角速度 (高字节在前，低字节在后)
uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
motor->lz_feedback.current_velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
// Byte4~5: 当前力矩
uint16_t raw_torque = (uint16_t)((msg->data[5] << 8) | msg->data[4]);
// Byte4~5: 当前力矩 (高字节在前，低字节在后)
uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
motor->lz_feedback.current_torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
// Byte6~7: 当前温度 (温度*10)
uint16_t raw_temp = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
// Byte6~7: 当前温度 (温度*10) (高字节在前，低字节在后)
uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
// 更新通用电机反馈信息
@ -334,25 +334,25 @@ int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *m
// 准备数据
uint8_t data[8];
// Byte0~1: 目标角度
// Byte0~1: 目标角度 (高字节在前，低字节在后)
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(motion_param->target_angle, LZ_ANGLE_RANGE_RAD);
data[0] = raw_angle & 0xFF;
data[1] = (raw_angle >> 8) & 0xFF;
data[0] = (raw_angle >> 8) & 0xFF; // 高字节
data[1] = raw_angle & 0xFF; // 低字节
// Byte2~3: 目标角速度
// Byte2~3: 目标角速度 (高字节在前，低字节在后)
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(motion_param->target_velocity, LZ_VELOCITY_RANGE_RAD_S);
data[2] = raw_velocity & 0xFF;
data[3] = (raw_velocity >> 8) & 0xFF;
data[2] = (raw_velocity >> 8) & 0xFF; // 高字节
data[3] = raw_velocity & 0xFF; // 低字节
// Byte4~5: Kp
// Byte4~5: Kp (高字节在前，低字节在后)
uint16_t raw_kp = MOTOR_LZ_FloatToRaw(motion_param->kp, LZ_KP_MAX);
data[4] = raw_kp & 0xFF;
data[5] = (raw_kp >> 8) & 0xFF;
data[4] = (raw_kp >> 8) & 0xFF; // 高字节
data[5] = raw_kp & 0xFF; // 低字节
// Byte6~7: Kd
// Byte6~7: Kd (高字节在前，低字节在后)
uint16_t raw_kd = MOTOR_LZ_FloatToRaw(motion_param->kd, LZ_KD_MAX);
data[6] = raw_kd & 0xFF;
data[7] = (raw_kd >> 8) & 0xFF;
data[6] = (raw_kd >> 8) & 0xFF; // 高字节
data[7] = raw_kd & 0xFF; // 低字节
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
@ -423,3 +423,11 @@ int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
}
return DEVICE_ERR_NO_DEV;
}
MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor && motor->motor.header.online) {
return &motor->lz_feedback;
}
return NULL;
}