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

添加odrive和vesc

Browse Source
This commit is contained in:
Robofish 2025-08-18 03:20:20 +08:00
parent 7e92f32642
commit 41de38a146
4 changed files with 886 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

328
assets/User_code/device/motor_odrive.c Normal file
View File

@ -0,0 +1,328 @@
/*
CAN总线上的设备
将所有CAN总线上挂载的设备抽象成一个设备进行配置和控制
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_odrive.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static ODrive_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
// 获取指定CAN总线的电机管理器指针
static ODrive_CANManager_t* MOTOR_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
// 为指定CAN总线创建电机管理器
static int8_t MOTOR_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (ODrive_CANManager_t*)BSP_Malloc(sizeof(ODrive_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(ODrive_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
// 解析CAN报文，更新电机反馈信息
static void Motor_Decode(ODrive_t *motor, BSP_CAN_Message_t *msg)
{
uint8_t axis_id = (msg->original_id >> 5) & 0x3F; // 提取电机号（0~63）
uint8_t cmd_id = msg->original_id & 0x1F; // 提取命令 ID（低 5 位）
motor->param.id = axis_id; // 保存电机 ID
// 解析帧类型（数据帧或远程帧）
if (msg->frame_type == BSP_CAN_FRAME_STD_DATA) {
// 数据帧处理
switch (cmd_id)
{
case ODRIVE_HEARTBEAT_MESSAGE: // 0x001 ODrive心跳消息
// motor->motor.feedback.axis_error = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
// motor->motor.feedback.axis_state = msg->data[4];
// motor->motor.feedback.controller_status = msg->data[5];
break;
case ENCODER_ESTIMATES: // 0x009
{
uint32_t raw_pos = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
uint32_t raw_vel = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
memcpy(&motor->motor.feedback.rotor_abs_angle, &raw_pos, sizeof(float));
memcpy(&motor->motor.feedback.rotor_speed, &raw_vel, sizeof(float));
}
break;
case GET_ENCODER_COUNT: // 0x014
// motor->motor.feedback.encoder_shadow = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
// motor->motor.feedback.encoder_cpr = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
break;
case GET_BUS_VOLTAGE_CURRENT: // 0x017
{
uint32_t raw_vbus, raw_ibus;
raw_vbus = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
raw_ibus = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
// memcpy(&motor->motor.feedback.bus_voltage, &raw_vbus, sizeof(float));
memcpy(&motor->motor.feedback.torque_current, &raw_ibus, sizeof(float));
}
break;
case GET_IQ: // 0x018
{
uint32_t raw_iq_set, raw_iq_meas;
raw_iq_set = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
raw_iq_meas = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
// memcpy(&motor->motor.feedback.iq_setpoint, &raw_iq_set, sizeof(float));
// memcpy(&motor->motor.feedback.iq_measured, &raw_iq_meas, sizeof(float));
}
break;
default:
break;
}
}
}
/* Exported functions ------------------------------------------------------- */
// 注册一个新的电机实例到管理器
int8_t ODrive_Register(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= ODRIVE_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
ODrive_t *new_motor = (ODrive_t*)BSP_Malloc(sizeof(ODrive_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(ODrive_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
// 更新指定电机的反馈数据
int8_t ODrive_Update(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
ODrive_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_Decode(motor, &rx_msg);
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
// 更新所有CAN总线下所有电机的反馈数据
int8_t ODrive_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
ODrive_CANManager_t *manager = MOTOR_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
ODrive_t *motor = manager->motors[i];
if (motor != NULL) {
if (ODrive_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
// 获取指定参数对应的电机实例指针
ODrive_t* ODrive_GetMotor(ODrive_Param_t *param) {
if (param == NULL) return NULL;
ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
ODrive_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
// 设置指定电机的输出值
int8_t ODrive_SetOutput(ODrive_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
// 如果电机反转标志为 true，则反向值
if (param->reverse) {
value = -value;
}
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id;
uint8_t *pVal = (uint8_t *)&value;
// 选择命令 ID 和数据打包方式
switch (param->mode) {
case POSITION_CONTROL: {
command_id = SET_INPUT_POS;
float pos = value;
int16_t vel_ff = 0; // 可扩展为参数传入 0就行
int16_t torque_ff = 0; // 可扩展为参数传入 0就行
uint8_t *pPos = (uint8_t *)&pos;
uint8_t *pVel = (uint8_t *)&vel_ff;
uint8_t *pTor = (uint8_t *)&torque_ff;
memcpy(&tx_frame.data[0], pPos, 4);
memcpy(&tx_frame.data[4], pVel, 2);
memcpy(&tx_frame.data[6], pTor, 2);
tx_frame.dlc = 8;
break;
}
case VELOCITY_CONTROL: {
command_id = SET_INPUT_VEL;
float vel = value;
float torque_ff = 0.0f; // 可扩展为参数传入
uint8_t *pVel = (uint8_t *)&vel;
uint8_t *pTor = (uint8_t *)&torque_ff;
memcpy(&tx_frame.data[0], pVel, 4);
memcpy(&tx_frame.data[4], pTor, 4);
tx_frame.dlc = 8;
break;
}
case TORQUE_CONTROL: {
command_id = SET_INPUT_TORQUE;
memcpy(&tx_frame.data[0], pVal, 4);
tx_frame.dlc = 4;
break;
}
case VOLTAGE_CONTROL:
default:
return DEVICE_ERR; // 暂不支持电压模式
}
// 组装 CAN ID（标准帧）
tx_frame.id = (param->id << 5) | command_id;
// 标准数据帧
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 设置加速度和减速度
int8_t ODrive_SetAccel(ODrive_Param_t *param, float accel, float decel) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = SET_TRAJ_ACCEL_LIMITS;
uint8_t *pAccel = (uint8_t *)&accel;
uint8_t *pDecel = (uint8_t *)&decel;
memcpy(&tx_frame.data[0], pAccel, 4);
memcpy(&tx_frame.data[4], pDecel, 4);
tx_frame.dlc = 8;
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 获取位置和速度反馈
int8_t ODrive_RequestEncoderEstimates(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = ENCODER_ESTIMATES; // 请求编码器估计值命令
uint8_t zero_data[8] = {0}; // 发送全 0 数据（ODrive 协议要求）
memcpy(tx_frame.data, zero_data, 8);
tx_frame.dlc = 8;
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 设置轴请求状态（一般用来重启 ODrive 的某个轴）
// ODrive_SetAxisRequestedState(odrive_axis[0], CLOSED_LOOP_CONTROL);
int8_t ODrive_SetAxisRequestedState(ODrive_Param_t *param, Axis_State state) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = SET_AXIS_REQUESTED_STATE;
// 将 state 转为 4 字节
memcpy(tx_frame.data, &state, 4);
memset(&tx_frame.data[4], 0, 4);
tx_frame.dlc = 4;
// 组装 CAN ID
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 清除错误
int8_t ODrive_ClearErrors(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = CLEAR_ERRORS;
memset(tx_frame.data, 0, 8);
tx_frame.dlc = 0;
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
// 重启 ODrive
int8_t ODrive_Reboot(ODrive_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id = REBOOT_ODRIVE;
memset(tx_frame.data, 0, 8);
tx_frame.dlc = 0;
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}

162
assets/User_code/device/motor_odrive.h Normal file
View File

@ -0,0 +1,162 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Private define ----------------------------------------------------------- */
//ODrive型号根据实际情况调整
#define ODRIVE_MAX_MOTORS 2
//COMMAND ID
#define ODRIVE_HEARTBEAT_MESSAGE 0x001 // ODrive心跳消息
#define SET_AXIS_NODE_ID 0x006 // 设置电机节点ID
#define GET_ENCODER_ESTIMATES 0x008 // 获取编码器估计值
#define GET_ENCODER_COUNT 0x00A // 获取编码器计数
#define SET_AXIS_REQUESTED_STATE 0x007 // 设置电机请求状态
#define ENCODER_ESTIMATES 0x009 // 编码器估计值
#define GET_ENCODER_COUNT 0x00A // 获取编码器计数
#define SET_CONTROLLER_MODES 0x00B // 设置控制器模式
#define SET_INPUT_POS 0x00C // 设置输入位置
#define SET_INPUT_VEL 0x00D // 设置输入速度
#define SET_INPUT_TORQUE 0x00E // 设置输入转矩
#define SET_LIMITS 0x00F // 设置限制
#define GET_IQ 0x014 // 获取电流
#define REBOOT_ODRIVE 0x016 // 重启ODrive
#define GET_BUS_VOLTAGE_CURRENT 0x017 // 获取总线电压和电流
#define CLEAR_ERRORS 0x018 // 清除错误
#define SET_POSITION_GAIN 0x01A // 设置位置增益
#define SET_VEL_GAINS 0x01B // 设置速度增益
#define SET_TRAJ_ACCEL_LIMITS 0x012 // 设置轨迹加速度限制
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
//Axis States
typedef enum {
UNDEFINED = 0x0,
IDLE = 0x1,
STARTUP_SEQUENCE = 0x2,
FULL_CALIBRATION_SEQUENCE = 0x3,
MOTOR_CALIBRATION = 0x4,
ENCODER_INDEX_SEARCH = 0x6,
ENCODER_OFFSET_CALIBRATION = 0x7,
CLOSED_LOOP_CONTROL = 0x8,
LOCKIN_SPIN = 0x9,
ENCODER_DIR_FIND = 0xA,
HOMING = 0xB,
ENCODER_HALL_POLARITY_CALIBRATION = 0xC,
ENCODER_HALL_PHASE_CALIBRATION = 0xD
} Axis_State;
//Control Modes
typedef enum{
VOLTAGE_CONTROL = 0x0,
TORQUE_CONTROL = 0x1,
VELOCITY_CONTROL = 0x2,
POSITION_CONTROL = 0x3
} Control_Mode;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
uint16_t mode;
bool reverse;
} ODrive_Param_t;
/*电机实例*/
typedef struct ODrive_t {
ODrive_Param_t param;
MOTOR_t motor;
} ODrive_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
ODrive_t *motors[ODRIVE_MAX_MOTORS];
uint8_t motor_count;
} ODrive_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个odrive电机
* @param param 电机参数
* @return
*/
int8_t ODrive_Register(ODrive_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t ODrive_Update(ODrive_Param_t *param);
/** * @brief 更新所有ODrive电机状态
* @return
*/
int8_t ODrive_UpdateAll(void);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值
* @return
*/
int8_t ODrive_SetOutput(ODrive_Param_t *param, float value);
/** * @brief 设置电机加速度和减速度限制
* @param param 电机参数
* @param accel 加速度
* @param decel 减速度
* @return
*/
int8_t ODrive_SetAccel(ODrive_Param_t *param, float accel, float decel);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
ODrive_t* ODrive_GetMotor(ODrive_Param_t *param);
/** * @brief 获取指定电机的编码器估计值
* @param param 电机参数
* @return
*/
int8_t ODrive_RequestEncoderEstimates(ODrive_Param_t *param);
/** * @brief 设置轴请求状态（一般用来重启 ODrive 的某个轴）
* @param param 电机参数
* @return
*/
int8_t ODrive_SetAxisRequestedState(ODrive_Param_t *param, Axis_State state);
/** * @brief 清除错误
* @param param 电机参数
* @return
*/
int8_t ODrive_ClearErrors(ODrive_Param_t *param);
/** * @brief 重启 ODrive
* @param param 电机参数
* @return
*/
int8_t ODrive_Reboot(ODrive_Param_t *param);
#ifdef __cplusplus
}
#endif

250
assets/User_code/device/motor_vesc.c Normal file
View File

@ -0,0 +1,250 @@
/*
CAN总线上的设备
将所有CAN总线上挂载的设备抽象成一个设备进行配置和控制
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_vesc.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/**************************************
* 限幅函数
**************************************/
void assert_param_duty(float *duty){
// 如果 duty 是 -1.0 ~ 1.0，则最大值用 wtrcfg_VESC_COMMAND_DUTY_MAX / 100
float max_duty = wtrcfg_VESC_COMMAND_DUTY_MAX / 100.0f;
if (fabsf(*duty) > max_duty) {
*duty = (*duty > 0) ? max_duty : -max_duty;
}
}
void assert_param_current(float *current){
if( fabsf(*current) > wtrcfg_VESC_COMMAND_CURRENT_MAX )
*current = *current > 0 ? wtrcfg_VESC_COMMAND_CURRENT_MAX : - wtrcfg_VESC_COMMAND_CURRENT_MAX ;
}
void assert_param_rpm(float *rpm){
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
}
void assert_param_pos(float *pos){
if( fabsf(*pos) > wtrcfg_VESC_COMMAND_POS_MAX )
*pos = *pos > 0 ? wtrcfg_VESC_COMMAND_POS_MAX : - wtrcfg_VESC_COMMAND_POS_MAX ;
}
static VESC_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
// 获取指定CAN总线的电机管理器指针
static VESC_CANManager_t* MOTOR_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
// 为指定CAN总线创建电机管理器
static int8_t MOTOR_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (VESC_CANManager_t*)BSP_Malloc(sizeof(VESC_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(VESC_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
// 解析CAN报文，更新电机反馈信息
static void Motor_VESC_Decode(VESC_t *motor, BSP_CAN_Message_t *msg)
{
if (motor == NULL || msg == NULL) return;
motor->motor.feedback.rotor_speed =
((int32_t)msg->data[0] << 24) |
((int32_t)msg->data[1] << 16) |
((int32_t)msg->data[2] << 8) |
((int32_t)msg->data[3]);
// torque_current: 低 2 字节 (data[4], data[5])
int16_t raw_current = (int16_t)((msg->data[5] << 8) | msg->data[4]);
motor->motor.feedback.torque_current = raw_current / 1000.0f; // 从 0.1A -> A
// duty_cycle: 低 2 字节 (data[6], data[7])
int16_t raw_duty = (int16_t)((msg->data[7] << 8) | msg->data[6]);
//motor->motor.feedback.duty_cycle = raw_duty / 1000.0f; // 从千分之一 -> (-1.0 ~ 1.0)
}
/* Exported functions ------------------------------------------------------- */
// 注册一个新的电机实例到管理器
int8_t VESC_Register(VESC_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= VESC_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
VESC_t *new_motor = (VESC_t*)BSP_Malloc(sizeof(VESC_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(VESC_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
// 更新指定电机的反馈数据（扩展帧方式）
int8_t VESC_Update(VESC_Param_t *param)
{
if (param == NULL) return DEVICE_ERR_NULL;
VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
VESC_t *motor = NULL;
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
motor = manager->motors[i];
break;
}
}
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 根据电机 ID 获取对应扩展帧 ID
uint32_t ext_id = 0;
switch (param->id) {
case VESC_1: ext_id = CAN_VESC5065_M1_MSG1; break;
case VESC_2: ext_id = CAN_VESC5065_M2_MSG1; break;
case VESC_4: ext_id = CAN_VESC5065_M3_MSG1; break;
default: return DEVICE_ERR_NO_DEV;
}
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, ext_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_VESC_Decode(motor, &rx_msg);
return DEVICE_OK;
}
// 更新所有CAN总线下所有电机的反馈数据
int8_t VESC_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
VESC_CANManager_t *manager = MOTOR_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
VESC_t *motor = manager->motors[i];
if (motor != NULL) {
if (VESC_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
// 获取指定参数对应的电机实例指针
VESC_t* VESC_GetMotor(VESC_Param_t *param) {
if (param == NULL) return NULL;
VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
VESC_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
// 设置指定电机的输出值
int8_t VESC_SetOutput(VESC_Param_t *param, float value)
{
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t command_id;
if (param->reverse) {
value = -value;
}
switch (param->mode)
{
case DUTY_CONTROL: {
assert_param_duty(&value); // 调用你现有的限幅函数
command_id = CAN_PACKET_SET_DUTY;
int32_t duty_val = (int32_t)(value * 1e5f); // duty 放大 1e5
memcpy(&tx_frame.data[0], &duty_val, 4);
tx_frame.dlc = 4;
break;
}
case RPM_CONTROL: {
assert_param_rpm(&value);
command_id = CAN_PACKET_SET_RPM;
int32_t rpm_val = (int32_t)value;
memcpy(&tx_frame.data[0], &rpm_val, 4);
tx_frame.dlc = 4;
break;
}
case CURRENT_CONTROL: {
assert_param_current(&value);
command_id = CAN_PACKET_SET_CURRENT;
int32_t cur_val = (int32_t)(value * 1e3f); // A -> mA （0-50A）
memcpy(&tx_frame.data[0], &cur_val, 4);
tx_frame.dlc = 4;
break;
}
case POSITION_CONTROL: {
assert_param_pos(&value);
command_id = CAN_PACKET_SET_POS;
memcpy(&tx_frame.data[0], &value, 4);
tx_frame.dlc = 4;
break;
}
default:
return DEVICE_ERR;
}
tx_frame.id = (param->id << 5) | command_id;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t VESC_Relax(VESC_Param_t *param) {
return VESC_SetOutput(param, 0.0f);
}
int8_t VESC_Offine(VESC_Param_t *param) {
VESC_t *motor = VESC_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

146
assets/User_code/device/motor_vesc.h Normal file
View File

@ -0,0 +1,146 @@
#pragma once
#ifdef __cplusplus
extern "C"
{
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Private define ----------------------------------------------------------- */
#define wtrcfg_VESC_COMMAND_DUTY_MAX 100
#define wtrcfg_VESC_COMMAND_CURRENT_MAX 10
#define wtrcfg_VESC_COMMAND_POS_MAX 360
#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000
#define wtrcfg_VESC_UART_TIMEOUT 0xff
// VESC数量根据实际情况调整
#define VESC_MAX_MOTORS 4
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum
{
VESC_1 = 1,
VESC_2 = 2,
VESC_3 = 3,
VESC_4 = 4,
CAN_VESC5065_M1_MSG1 = 0x901, // vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
CAN_VESC5065_M2_MSG1 = 0x902,
CAN_VESC5065_M3_MSG1 = 0x903,
CAN_VESC5065_M4_MSG1 = 0x904,
}VESC_ID;
typedef enum
{
CAN_PACKET_SET_DUTY = 0,
CAN_PACKET_SET_CURRENT = 1,
CAN_PACKET_SET_CURRENT_BRAKE = 2,
CAN_PACKET_SET_RPM = 3,
CAN_PACKET_SET_POS = 4,
CAN_PACKET_FILL_RX_BUFFER = 5,
CAN_PACKET_FILL_RX_BUFFER_LONG = 6,
CAN_PACKET_PROCESS_RX_BUFFER = 7,
CAN_PACKET_PROCESS_SHORT_BUFFER = 8,
CAN_PACKET_STATUS = 9,
CAN_PACKET_SET_CURRENT_REL = 10,
CAN_PACKET_SET_CURRENT_BRAKE_REL = 11,
CAN_PACKET_SET_CURRENT_HANDBRAKE = 12,
CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13
} CAN_PACKET_ID;
// Control Modes
typedef enum
{
DUTY_CONTROL = 0x0,
RPM_CONTROL = 0x1,
CURRENT_CONTROL = 0x2,
POSITION_CONTROL = 0x3
} Control_Mode;
/*每个电机需要的参数*/
typedef struct
{
BSP_CAN_t can;
uint16_t id;
uint16_t mode;
bool reverse;
} VESC_Param_t;
/*电机实例*/
typedef struct ODrive_t
{
VESC_Param_t param;
MOTOR_t motor;
} VESC_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct
{
BSP_CAN_t can;
VESC_t *motors[VESC_MAX_MOTORS];
uint8_t motor_count;
} VESC_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个vesc电机
* @param param 电机参数
* @return
*/
int8_t VESC_Register(VESC_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t VESC_Update(VESC_Param_t *param);
/**
* @brief 更新所有电机数据
* @return
*/
int8_t VESC_UpdateAll(void);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值
* @return
*/
int8_t VESC_SetOutput(VESC_Param_t *param, float value);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
VESC_t* VESC_GetMotor(VESC_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t VESC_Relax(VESC_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t VESC_Offine(VESC_Param_t *param);
#ifdef __cplusplus
}
#endif