重构项目：移除JY901传感器，添加CAN总线支持，集成电机/传感器模块库
This commit is contained in:
CloudPlant Team
101
modules/device/motor/mf4010v2.c
Normal file
101
modules/device/motor/mf4010v2.c
Normal file
@@ -0,0 +1,101 @@
|
||||
/**
|
||||
* @file mf4010v2.c
|
||||
* @brief MF4010V2 无刷电机驱动程序
|
||||
* @note 改进版本：
|
||||
* - 添加错误处理和返回值
|
||||
* - 添加 CAN 超时机制
|
||||
* - 使用安全的内联函数替代危险宏
|
||||
* - 使用 HAL 层解耦硬件依赖
|
||||
*/
|
||||
|
||||
#include "mf4010v2.h"
|
||||
#include "bsp_can.h"
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
/**
|
||||
* @brief 初始化电机结构体
|
||||
*/
|
||||
mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id)
|
||||
{
|
||||
// 参数验证
|
||||
if(motor == NULL) {
|
||||
return MF4010_ERR_INVALID_PARAM;
|
||||
}
|
||||
|
||||
// CAN ID 范围验证 (0x000-0x7FF for standard frame)
|
||||
if(id > 0x7FF) {
|
||||
printf("Error: Invalid CAN ID 0x%03X (must be 0x000-0x7FF)\r\n", id);
|
||||
return MF4010_ERR_INVALID_ID;
|
||||
}
|
||||
|
||||
// 初始化结构体
|
||||
motor->id = id;
|
||||
memset(motor->last_response, 0, sizeof(motor->last_response));
|
||||
motor->last_comm_time = 0;
|
||||
motor->error_count = 0;
|
||||
motor->success_count = 0;
|
||||
motor->flags = MF4010_FLAG_NONE;
|
||||
motor->ops.init = mf4010v2_init;
|
||||
motor->ops.send_command = mf4010v2_send_command;
|
||||
return MF4010_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送命令并接收响应（带超时）
|
||||
*/
|
||||
mf4010_status_t mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8], uint32_t timeout_ms)
|
||||
{
|
||||
// 参数验证
|
||||
if(motor == NULL || command == NULL) {
|
||||
return MF4010_ERR_INVALID_PARAM;
|
||||
}
|
||||
// 验证命令格式（首字节应该是命令字）
|
||||
if((command[0] & 0x80) == 0 && command[0] < 0x30) {
|
||||
printf("Warning: Invalid command format 0x%02X\r\n", command[0]);
|
||||
// 不返回错误，继续执行
|
||||
}
|
||||
|
||||
// 发送 CAN 消息
|
||||
if (can_send(motor->id, command, 8) != HAL_OK)
|
||||
{
|
||||
motor->error_count++;
|
||||
motor->flags &= ~MF4010_FLAG_CONNECTED;
|
||||
return MF4010_ERR_TIMEOUT;
|
||||
}
|
||||
|
||||
// 接收响应
|
||||
uint16_t rx_id;
|
||||
uint8_t len;
|
||||
if (MF4010V2_HAL_Receive(&rx_id, motor->last_response, &len, timeout_ms) != HAL_OK)
|
||||
{
|
||||
motor->error_count++;
|
||||
motor->flags &= ~MF4010_FLAG_CONNECTED;
|
||||
return MF4010_ERR_NO_RESPONSE;
|
||||
}
|
||||
|
||||
// 验证响应 ID
|
||||
if (rx_id != motor->id)
|
||||
{
|
||||
return MF4010_ERR_CRC;
|
||||
}
|
||||
|
||||
motor->last_comm_time = get_system_tick();
|
||||
motor->success_count++;
|
||||
motor->flags |= MF4010_FLAG_CONNECTED;
|
||||
|
||||
// 解析错误状态
|
||||
if (motor->last_response[0] & 0x80)
|
||||
{
|
||||
uint8_t error_code = motor->last_response[0] & 0x7F;
|
||||
printf("Motor error from 0x%03X: 0x%02X\r\n", motor->id, error_code);
|
||||
motor->error_count++;
|
||||
motor->flags |= MF4010_FLAG_FAULT;
|
||||
return MF4010_ERR_CRC;
|
||||
}
|
||||
|
||||
motor->flags &= ~MF4010_FLAG_FAULT;
|
||||
return MF4010_OK;
|
||||
}
|
||||
|
||||
338
modules/device/motor/mf4010v2.h
Normal file
338
modules/device/motor/mf4010v2.h
Normal file
@@ -0,0 +1,338 @@
|
||||
#ifndef __MF4010V2__H__
|
||||
#define __MF4010V2__H__
|
||||
|
||||
/**
|
||||
* @file mf4010v2.h
|
||||
* @brief MF4010V2 无刷电机驱动程序
|
||||
* @note 改进版本：
|
||||
* - 添加错误处理和返回值
|
||||
* - 添加 CAN 超时机制
|
||||
* - 使用安全的内联函数替代危险宏
|
||||
* - 使用 HAL 层解耦硬件依赖
|
||||
*
|
||||
* 电机 CAN ID: 0x141 (默认)
|
||||
* CAN 波特率：1 Mbps
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* ============================================================================
|
||||
* 状态码和错误定义
|
||||
* ============================================================================ */
|
||||
|
||||
/**
|
||||
* @brief MF4010V2 状态码
|
||||
*/
|
||||
typedef enum {
|
||||
MF4010_OK = 0, /**< 成功 */
|
||||
MF4010_ERR_TIMEOUT = -1, /**< CAN 通信超时 */
|
||||
MF4010_ERR_INVALID_ID = -2, /**< 无效的 CAN ID */
|
||||
MF4010_ERR_NO_RESPONSE = -3, /**< 无响应 */
|
||||
MF4010_ERR_INVALID_PARAM = -4, /**< 无效参数 */
|
||||
MF4010_ERR_BUS_OFF = -5, /**< CAN 总线关闭 */
|
||||
MF4010_ERR_CRC = -6, /**< CRC 校验错误 */
|
||||
MF4010_ERR_HARDWARE = -7 /**< 硬件错误 */
|
||||
} mf4010_status_t;
|
||||
|
||||
/**
|
||||
* @brief 电机状态标志
|
||||
*/
|
||||
typedef enum {
|
||||
MF4010_FLAG_NONE = 0x00, /**< 无标志 */
|
||||
MF4010_FLAG_RUNNING = 0x01, /**< 电机运行中 */
|
||||
MF4010_FLAG_ENABLED = 0x02, /**< 电机已使能 */
|
||||
MF4010_FLAG_FAULT = 0x04, /**< 故障状态 */
|
||||
MF4010_FLAG_CONNECTED = 0x08 /**< 已连接 */
|
||||
} mf4010_flag_t;
|
||||
|
||||
/* ============================================================================
|
||||
* 电机结构体定义
|
||||
* ============================================================================ */
|
||||
|
||||
/**
|
||||
* @brief 电机结构体（优化版）
|
||||
*/
|
||||
struct __mf4010v2_ops_t;
|
||||
typedef struct {
|
||||
struct __mf4010v2_ops_t ops; /**< 操作函数指针 */
|
||||
uint16_t id; /**< CAN ID (0x000-0x7FF) */
|
||||
uint8_t last_response[8]; /**< 最后一次响应数据 */
|
||||
uint32_t last_comm_time; /**< 最后通信时间戳 (ms) */
|
||||
uint16_t error_count; /**< 通信错误计数 */
|
||||
uint16_t success_count; /**< 成功通信计数 */
|
||||
uint8_t flags; /**< 状态标志 (@ref mf4010_flag_t) */
|
||||
} mf4010v2_t;
|
||||
|
||||
struct __mf4010v2_ops_t {
|
||||
mf4010_status_t (*init)(mf4010v2_t* self, uint16_t id);
|
||||
mf4010_status_t (*send_command)(mf4010v2_t* self, const uint8_t command[8], uint32_t timeout_ms);
|
||||
};
|
||||
|
||||
typedef struct __mf4010v2_ops_t mf4010v2_ops_t;
|
||||
|
||||
|
||||
/* ============================================================================
|
||||
* 核心 API 函数
|
||||
* ============================================================================ */
|
||||
|
||||
/**
|
||||
* @brief 初始化电机结构体
|
||||
* @param motor: 电机结构体指针
|
||||
* @param id: 电机 CAN ID (范围 0x000-0x7FF，如 0x141)
|
||||
* @retval 成功返回 MF4010_OK，失败返回错误码
|
||||
*/
|
||||
mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id);
|
||||
|
||||
/**
|
||||
* @brief 发送命令并接收响应（带超时）
|
||||
* @param motor: 电机结构体指针
|
||||
* @param command: 8 字节命令数据
|
||||
* @param timeout_ms: 超时时间 (ms)，0 表示无限等待
|
||||
* @retval 成功返回 MF4010_OK，失败返回错误码
|
||||
*
|
||||
* @note 命令格式参考下面的宏定义
|
||||
* @example
|
||||
* // 关闭电机
|
||||
* uint8_t cmd[8] = COMMAND_MOTOR_CLOSE;
|
||||
* mf4010v2_send_command(&motor, cmd, 1000);
|
||||
*
|
||||
* // 速度控制 (100 DPS)
|
||||
* uint8_t cmd[8];
|
||||
* cmd_speed_control(cmd, 10000); // 100 * 100 (0.01 DPS/LSB)
|
||||
* mf4010v2_send_command(&motor, cmd, 1000);
|
||||
*/
|
||||
mf4010_status_t mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8], uint32_t timeout_ms);
|
||||
|
||||
|
||||
/* ============================================================================
|
||||
* 命令宏定义（8 字节 CAN 命令帧）
|
||||
* ============================================================================ */
|
||||
|
||||
/**
|
||||
* @brief 电机关闭命令
|
||||
* @note 命令字：0x80
|
||||
*/
|
||||
#define COMMAND_MOTOR_CLOSE {0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
|
||||
|
||||
/**
|
||||
* @brief 电机运行命令
|
||||
* @note 命令字：0x88
|
||||
*/
|
||||
#define COMMAND_MOTOR_RUNNING {0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
|
||||
|
||||
/**
|
||||
* @brief 电机停止命令
|
||||
* @note 命令字：0x81
|
||||
*/
|
||||
#define COMMAND_MOTOR_STOP {0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
|
||||
|
||||
/**
|
||||
* @brief 读取 PID 参数命令
|
||||
* @note 命令字：0x30
|
||||
*/
|
||||
#define COMMAND_READ_PID_PARAMS {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
|
||||
|
||||
/* ============================================================================
|
||||
* 安全命令生成函数（使用内联函数避免指针问题）
|
||||
* ============================================================================ */
|
||||
|
||||
/**
|
||||
* @brief 转矩闭环控制命令
|
||||
* @param buf: 8 字节缓冲区（输出）
|
||||
* @param iq: 转矩控制量 (-2048 ~ 2048)
|
||||
* @note 分辨率：1 LSB = 1 单位
|
||||
*/
|
||||
static inline void cmd_torque_control(uint8_t* buf, int16_t iq)
|
||||
{
|
||||
buf[0] = 0xA1;
|
||||
buf[1] = 0x00;
|
||||
buf[2] = 0x00;
|
||||
buf[3] = 0x00;
|
||||
buf[4] = (uint8_t)(iq & 0xFF);
|
||||
buf[5] = (uint8_t)((iq >> 8) & 0xFF);
|
||||
buf[6] = 0x00;
|
||||
buf[7] = 0x00;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 速度闭环控制命令
|
||||
* @param buf: 8 字节缓冲区（输出）
|
||||
* @param speed: 速度控制量
|
||||
* @note 分辨率：0.01 DPS/LSB
|
||||
* 范围：-204800 ~ +204800 (对应 -2048 ~ +2048 DPS)
|
||||
* @example
|
||||
* uint8_t cmd[8];
|
||||
* cmd_speed_control(cmd, 10000); // 100 DPS
|
||||
*/
|
||||
static inline void cmd_speed_control(uint8_t* buf, int32_t speed)
|
||||
{
|
||||
buf[0] = 0xA2;
|
||||
buf[1] = 0x00;
|
||||
buf[2] = 0x00;
|
||||
buf[3] = 0x00;
|
||||
buf[4] = (uint8_t)(speed & 0xFF);
|
||||
buf[5] = (uint8_t)((speed >> 8) & 0xFF);
|
||||
buf[6] = (uint8_t)((speed >> 16) & 0xFF);
|
||||
buf[7] = (uint8_t)((speed >> 24) & 0xFF);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 单圈位置闭环控制命令
|
||||
* @param buf: 8 字节缓冲区（输出）
|
||||
* @param direction: 旋转方向 (0x00=顺时针，0x01=逆时针)
|
||||
* @param angle: 目标角度 (单位：0.01°, 如 36000=360°)
|
||||
*/
|
||||
static inline void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle)
|
||||
{
|
||||
buf[0] = 0xA5;
|
||||
buf[1] = direction & 0x01;
|
||||
buf[2] = 0x00;
|
||||
buf[3] = 0x00;
|
||||
buf[4] = (uint8_t)(angle & 0xFF);
|
||||
buf[5] = (uint8_t)((angle >> 8) & 0xFF);
|
||||
buf[6] = (uint8_t)((angle >> 16) & 0xFF);
|
||||
buf[7] = (uint8_t)((angle >> 24) & 0xFF);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 增量位置闭环控制命令（带最大速度）
|
||||
* @param buf: 8 字节缓冲区（输出）
|
||||
* @param direction: 旋转方向 (0x00=顺时针，0x01=逆时针)
|
||||
* @param max_speed: 最大速度 (单位：DPS, 如 360=360 DPS)
|
||||
* @param angle: 目标角度 (单位：0.01°, 如 36000=360°)
|
||||
*/
|
||||
static inline void cmd_incr_pos_with_speed(uint8_t* buf, uint8_t direction,
|
||||
uint16_t max_speed, uint32_t angle)
|
||||
{
|
||||
buf[0] = 0xA6;
|
||||
buf[1] = direction & 0x01;
|
||||
buf[2] = (uint8_t)(max_speed & 0xFF);
|
||||
buf[3] = (uint8_t)((max_speed >> 8) & 0xFF);
|
||||
buf[4] = (uint8_t)(angle & 0xFF);
|
||||
buf[5] = (uint8_t)((angle >> 8) & 0xFF);
|
||||
buf[6] = (uint8_t)((angle >> 16) & 0xFF);
|
||||
buf[7] = (uint8_t)((angle >> 24) & 0xFF);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 增量位置闭环控制命令
|
||||
* @param buf: 8 字节缓冲区（输出）
|
||||
* @param angle_increment: 角度增量 (单位：0.01°, 如 36000=360°)
|
||||
* @note 正值 = 逆时针，负值 = 顺时针
|
||||
*/
|
||||
static inline void cmd_position_add(uint8_t* buf, int32_t angle_increment)
|
||||
{
|
||||
buf[0] = 0xA7;
|
||||
buf[1] = 0x00;
|
||||
buf[2] = 0x00;
|
||||
buf[3] = 0x00;
|
||||
buf[4] = (uint8_t)(angle_increment & 0xFF);
|
||||
buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF);
|
||||
buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF);
|
||||
buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief PID 参数写入命令
|
||||
* @param buf: 8 字节缓冲区（输出）
|
||||
* @param location: 写入位置 (0x31=RAM, 0x32=ROM)
|
||||
* @param angle_kp: 角度环 Kp (0-255)
|
||||
* @param angle_ki: 角度环 Ki (0-255)
|
||||
* @param speed_kp: 速度环 Kp (0-255)
|
||||
* @param speed_ki: 速度环 Ki (0-255)
|
||||
* @param iq_kp: 电流环 Kp (0-255)
|
||||
* @param iq_ki: 电流环 Ki (0-255)
|
||||
*/
|
||||
static inline void cmd_write_pid_params(uint8_t* buf, uint8_t location,
|
||||
uint8_t angle_kp, uint8_t angle_ki,
|
||||
uint8_t speed_kp, uint8_t speed_ki,
|
||||
uint8_t iq_kp, uint8_t iq_ki)
|
||||
{
|
||||
buf[0] = location;
|
||||
buf[1] = 0x00;
|
||||
buf[2] = angle_kp;
|
||||
buf[3] = angle_ki;
|
||||
buf[4] = speed_kp;
|
||||
buf[5] = speed_ki;
|
||||
buf[6] = iq_kp;
|
||||
buf[7] = iq_ki;
|
||||
}
|
||||
|
||||
/* ============================================================================
|
||||
* 辅助函数和宏
|
||||
* ============================================================================ */
|
||||
|
||||
/**
|
||||
* @brief 检查电机是否在线
|
||||
*/
|
||||
static inline bool mf4010v2_is_connected(mf4010v2_t* motor)
|
||||
{
|
||||
return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 检查电机是否运行
|
||||
*/
|
||||
static inline bool mf4010v2_is_running(mf4010v2_t* motor)
|
||||
{
|
||||
return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取电机错误计数
|
||||
*/
|
||||
static inline uint16_t mf4010v2_get_error_count(mf4010v2_t* motor)
|
||||
{
|
||||
return (motor != NULL) ? motor->error_count : 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 重置错误计数
|
||||
*/
|
||||
static inline void mf4010v2_reset_errors(mf4010v2_t* motor)
|
||||
{
|
||||
if(motor != NULL) {
|
||||
motor->error_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取响应中的命令字节
|
||||
*/
|
||||
#define MF4010_GET_RESPONSE_CMD(motor) ((motor)->last_response[0])
|
||||
|
||||
/**
|
||||
* @brief 获取响应中的数据字节
|
||||
*/
|
||||
#define MF4010_GET_RESPONSE_DATA(motor, index) ((motor)->last_response[(index) + 1])
|
||||
|
||||
/**
|
||||
* @brief 检查响应是否包含错误
|
||||
* @note 如果响应字节 0 的最高位为 1，表示有错误
|
||||
*/
|
||||
static inline bool mf4010v2_response_has_error(mf4010v2_t* motor)
|
||||
{
|
||||
return (motor != NULL) && ((motor->last_response[0] & 0x80) != 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取错误码（从响应中）
|
||||
*/
|
||||
static inline uint8_t mf4010v2_get_error_code(mf4010v2_t* motor)
|
||||
{
|
||||
return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0;
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __MF4010V2__H__ */
|
||||
Reference in New Issue
Block a user