ArmDev/motor-controller
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[add] add app level code

Browse Source
This commit is contained in:
robinson
2026-04-18 11:02:40 +08:00
parent a8b7c716f3
commit dcb33f8a99
34 changed files with 1265 additions and 678 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
modules/device/can/can_device.c Normal file
View File

@@ -0,0 +1,48 @@
#include "can_device.h"
#include "can_if.h"
int can_device_init(can_device_t *dev, int ch)
{
if (!dev) return -1;
dev->ch = ch;
dev->initialized = 0;
int ret = can_if_init(ch);
if (ret == 0) {
dev->initialized = 1;
}
return ret;
}
int can_device_send(can_device_t *dev, uint32_t id, const uint8_t *data, uint8_t len)
{
if (!dev || !data || len > 8) return -1;
if (!dev->initialized) return -1;
can_message_t msg;
msg.id = id;
msg.length = len;
for (int i = 0; i < len; i++) {
msg.data[i] = data[i];
}
return can_if_send(dev->ch, &msg);
}
int can_device_recv(can_device_t *dev, can_message_t *msg)
{
if (!dev || !msg) return -1;
if (!dev->initialized) return -1;
return can_if_recv(dev->ch, msg);
}
int can_device_set_filter(can_device_t *dev, uint8_t filter_id, uint32_t id, uint32_t mask)
{
if (!dev || filter_id > 13) return -1;
if (!dev->initialized) return -1;
return can_if_filter_config(dev->ch, filter_id, id, mask);
}

18
modules/device/can/can_device.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef CAN_DEVICE_H
#define CAN_DEVICE_H
#include "can_if.h"
typedef struct {
int ch; // CAN 通道
can_message_t tx_msg; // 发送消息
can_message_t rx_msg; // 接收消息
int initialized; // 初始化标志
} can_device_t;
int can_device_init(can_device_t *dev, int ch);
int can_device_send(can_device_t *dev, uint32_t id, const uint8_t *data, uint8_t len);
int can_device_recv(can_device_t *dev, can_message_t *msg);
int can_device_set_filter(can_device_t *dev, uint8_t filter_id, uint32_t id, uint32_t mask);
#endif

99
modules/device/motor/mf4010v2.c
View File

@@ -9,7 +9,7 @@
*/
#include "mf4010v2.h"
#include "bsp_can.h"
#include "can_if.h"
#include <stdint.h>
#include <stdio.h>
#include <string.h>
@@ -17,21 +17,21 @@
/**
* @brief 初始化电机结构体
*/
mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id)
int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch)
{
// 参数验证
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->can_ch = can_ch;
motor->id = id;
motor->target_angle = 0;
motor->current_angle = 0;
memset(motor->last_response, 0, sizeof(motor->last_response));
motor->last_comm_time = 0;
motor->error_count = 0;
@@ -43,8 +43,89 @@ mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id)
/**
* @brief 发送命令并接收响应（带超时）
*/
mf4010_status_t mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8], uint32_t timeout_ms)
int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8])
{
can_message_t msg;
msg.id = motor->id;
memcpy(msg.data, command, 8);
msg.length = 8;
int ret = can_if_send(motor->can_ch, &msg);
if (ret != 0) {
motor->error_count++;
printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id);
return MF4010_ERR_CAN_SEND;
}
motor->success_count++;
return MF4010_OK;
}
/**
* @brief 设置电机目标角度 (位置闭环)
* @param motor 电机结构体
* @param angle_0_01deg 目标角度，单位 0.01 度
*/
int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg)
{
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8];
// 使用增量位置控制 (A7 命令)
// 计算角度增量
int32_t delta = angle_0_01deg - motor->current_angle;
// 限制单次增量在合理范围内
if (delta > 18000) delta = 18000; // 最多 +180 度
if (delta < -18000) delta = -18000; // 最多 -180 度
cmd_position_add(cmd, delta);
motor->target_angle = angle_0_01deg;
return mf4010v2_send_command(motor, motor->can_ch, cmd);
}
/**
* @brief 设置电机速度 (速度闭环)
* @param motor 电机结构体
* @param speed_0_01dps 目标速度，单位 0.01 DPS
*/
int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps)
{
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8];
cmd_speed_control(cmd, speed_0_01dps);
return mf4010v2_send_command(motor, motor->can_ch, cmd);
}
/**
* @brief 启动电机
*/
void mf4010v2_run(mf4010v2_t* motor)
{
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_RUNNING;
mf4010v2_send_command(motor, motor->can_ch, cmd);
motor->flags |= MF4010_FLAG_RUNNING;
}
/**
* @brief 停止电机
*/
void mf4010v2_stop(mf4010v2_t* motor)
{
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_STOP;
mf4010v2_send_command(motor, motor->can_ch, cmd);
motor->flags &= ~MF4010_FLAG_RUNNING;
}
/**
* @brief 获取当前速度 (返回目标速度，因为实际速度需要从编码器反馈)
*/
int32_t mf4010v2_get_speed(mf4010v2_t* motor)
{
if (!motor) return 0;
uint8_t cmd[8] =
mf4010v2_send_command(motor, motor->can_ch, cmd);
return motor->target_angle; // 占位返回
}

37
modules/device/motor/mf4010v2.h
View File

@@ -39,7 +39,8 @@ typedef enum {
MF4010_ERR_INVALID_PARAM = -4, /**< 无效参数 */
MF4010_ERR_BUS_OFF = -5, /**< CAN 总线关闭 */
MF4010_ERR_CRC = -6, /**< CRC 校验错误 */
MF4010_ERR_HARDWARE = -7 /**< 硬件错误 */
MF4010_ERR_HARDWARE = -7, /**< 硬件错误 */
MF4010_ERR_CAN_SEND = -8 /**< CAN 发送失败 */
} mf4010_status_t;
/**
@@ -60,21 +61,18 @@ typedef enum {
/**
* @brief 电机结构体（优化版）
*/
struct __mf4010v2_t;
typedef struct __mf4010v2_t mf4010v2_t; // 前向声明
typedef struct {
mf4010_status_t (*init)();
mf4010_status_t (*send_command)();
}mf4010v2_ops_t;
struct __mf4010v2_t {
mf4010v2_ops_t ops; /**< 操作函数指针 */
int can_ch; /**< CAN 通道 (0 或 1) */
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) */
int32_t target_angle; /**< 目标角度 (0.01 度) */
int32_t current_angle; /**< 当前角度 (0.01 度) - 从编码器读取 */
};
@@ -89,31 +87,26 @@ struct __mf4010v2_t {
* @brief 初始化电机结构体
* @param motor: 电机结构体指针
* @param id: 电机 CAN ID (范围 0x000-0x7FF，如 0x141)
* @param can_ch: CAN 通道 (0=CAN1, 1=CAN2)
* @retval 成功返回 MF4010_OK，失败返回错误码
*/
mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id);
int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch);
/**
* @brief 发送命令并接收响应（带超时）
* @param motor: 电机结构体指针
* @param can_ch: CAN 通道
* @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);
int mf4010v2_send_command(mf4010v2_t* motor, int can_ch, const uint8_t command[8]);
void mf4010v2_run(mf4010v2_t* motor);
void mf4010v2_stop(mf4010v2_t* motor);
int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps);
int32_t mf4010v2_get_speed(mf4010v2_t* motor);
/* ============================================================================
* 命令宏定义（8 字节 CAN 命令帧）
* ============================================================================ */