diff --git a/assets/User_code/device/motor_odrive.c b/assets/User_code/device/motor_odrive.c new file mode 100644 index 0000000..6394600 --- /dev/null +++ b/assets/User_code/device/motor_odrive.c @@ -0,0 +1,328 @@ +/* + CAN总线上的设备 + 将所有CAN总线上挂载的设备抽象成一个设备进行配置和控制 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_odrive.h" +#include +#include +#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; +} diff --git a/assets/User_code/device/motor_odrive.h b/assets/User_code/device/motor_odrive.h new file mode 100644 index 0000000..ffc5282 --- /dev/null +++ b/assets/User_code/device/motor_odrive.h @@ -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 \ No newline at end of file diff --git a/assets/User_code/device/motor_vesc.c b/assets/User_code/device/motor_vesc.c new file mode 100644 index 0000000..1947b62 --- /dev/null +++ b/assets/User_code/device/motor_vesc.c @@ -0,0 +1,250 @@ +/* + CAN总线上的设备 + 将所有CAN总线上挂载的设备抽象成一个设备进行配置和控制 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_vesc.h" +#include +#include +#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; +} \ No newline at end of file diff --git a/assets/User_code/device/motor_vesc.h b/assets/User_code/device/motor_vesc.h new file mode 100644 index 0000000..eaea852 --- /dev/null +++ b/assets/User_code/device/motor_vesc.h @@ -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 \ No newline at end of file