diff --git a/.DS_Store b/.DS_Store index 1d425fc..20d5f5c 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/assets/.DS_Store b/assets/.DS_Store index 739f81d..90d92e1 100644 Binary files a/assets/.DS_Store and b/assets/.DS_Store differ diff --git a/assets/User_code/.DS_Store b/assets/User_code/.DS_Store index ee966f4..0b7a85f 100644 Binary files a/assets/User_code/.DS_Store and b/assets/User_code/.DS_Store differ diff --git a/assets/User_code/bsp/pwm.c b/assets/User_code/bsp/pwm.c index 550d7a4..3183e4c 100644 --- a/assets/User_code/bsp/pwm.c +++ b/assets/User_code/bsp/pwm.c @@ -67,4 +67,33 @@ int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) { HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel); return BSP_OK; +} + +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].tim->Init.AutoReloadPreload; +} + +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) { + return PWM_Map[ch].tim; +} + + +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].channel; +} + +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length); + return BSP_OK; +} + +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; } \ No newline at end of file diff --git a/assets/User_code/bsp/pwm.h b/assets/User_code/bsp/pwm.h index 03f6cf5..35f55d7 100644 --- a/assets/User_code/bsp/pwm.h +++ b/assets/User_code/bsp/pwm.h @@ -12,6 +12,9 @@ extern "C" { /* Exported constants ------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ +/* Forward declarations */ +typedef struct __TIM_HandleTypeDef TIM_HandleTypeDef; + /* PWM通道 */ typedef enum { /* AUTO GENERATED BSP_PWM_ENUM */ @@ -24,6 +27,11 @@ int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch); int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch); +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch); +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch); +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length); +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch); #ifdef __cplusplus } diff --git a/assets/User_code/device/.DS_Store b/assets/User_code/device/.DS_Store index 640682c..b711bf4 100644 Binary files a/assets/User_code/device/.DS_Store and b/assets/User_code/device/.DS_Store differ diff --git a/assets/User_code/device/buzzer.c b/assets/User_code/device/buzzer.c new file mode 100644 index 0000000..89f616b --- /dev/null +++ b/assets/User_code/device/buzzer.c @@ -0,0 +1,44 @@ +#include "device/buzzer.h" + + +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) { + if (buzzer == NULL) return DEVICE_ERR; + + buzzer->channel = channel; + buzzer->header.online = true; + + BUZZER_Stop(buzzer); + + return DEVICE_OK ; +} + +int8_t BUZZER_Start(BUZZER_t *buzzer) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ? + DEVICE_OK : DEVICE_ERR; +} + +int8_t BUZZER_Stop(BUZZER_t *buzzer) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ? + DEVICE_OK : DEVICE_ERR; +} + +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + int result = DEVICE_OK ; + + if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK) + result = DEVICE_ERR; + + if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK) + result = DEVICE_ERR; + + return result; +} diff --git a/assets/User_code/device/buzzer.h b/assets/User_code/device/buzzer.h new file mode 100644 index 0000000..7553070 --- /dev/null +++ b/assets/User_code/device/buzzer.h @@ -0,0 +1,36 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device.h" +#include "bsp/pwm.h" +#include + +/* Exported constants ------------------------------------------------------- */ + +/* Exported types ----------------------------------------------------------- */ +typedef struct { + DEVICE_Header_t header; + BSP_PWM_Channel_t channel; +} BUZZER_t; + +/* Exported functions prototypes -------------------------------------------- */ + + +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel); + + +int8_t BUZZER_Start(BUZZER_t *buzzer); + + +int8_t BUZZER_Stop(BUZZER_t *buzzer); + + +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle); + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/config.yaml b/assets/User_code/device/config.yaml index 72be95c..6ff6630 100644 --- a/assets/User_code/device/config.yaml +++ b/assets/User_code/device/config.yaml @@ -107,4 +107,19 @@ devices: description: "通用电机驱动" files: header: "motor.h" - source: "motor.c" \ No newline at end of file + source: "motor.c" + + ws2812: + name: "WS2812 LED 灯" + description: "WS2812 RGB LED 灯驱动" + dependencies: + bsp: ["pwm", "time"] + component: [] + bsp_requirements: + - type: "pwm" + var_name: "BSP_PWM_WS2812" + description: "用于WS2812数据传输的PWM通道" + thread_signals: [] + files: + header: "ws2812.h" + source: "ws2812.c" \ No newline at end of file diff --git a/assets/User_code/device/led.c b/assets/User_code/device/led.c new file mode 100644 index 0000000..a19d58a --- /dev/null +++ b/assets/User_code/device/led.c @@ -0,0 +1,47 @@ +/* + led +*/ +/*Includes -----------------------------------------*/ +#include "device/led.h" +#include "bsp/gpio.h" +#include "bsp/pwm.h" +#include "device.h" + + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ + +DEVICE_LED_t LED_Map={ + BSP_GPIO_BLUE, +BSP_PWM_TIM5_CH1, +}; + +int8_t BSP_LED_Set(char sign,DEVICE_LED_t ch,bool value,float duty_cycle) +{ + switch(sign){ + case 'p': + case 'P': + if (duty_cycle < 0.0f || duty_cycle > 1.0f) { + return DEVICE_ERR_NULL; // ռձȳΧ + } + uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX); + BSP_PWM_Start(LED_Map.channel); + BSP_PWM_SetComp(LED_Map.channel, pulse); + break; + + case 'g': + case 'G': + BSP_GPIO_WritePin(LED_Map.gpio,value); + break; + default: + return DEVICE_ERR_INITED; // ЧĿƷʽ + } + return DEVICE_OK; +} + + + + + + diff --git a/assets/User_code/device/led.h b/assets/User_code/device/led.h new file mode 100644 index 0000000..39fa98b --- /dev/null +++ b/assets/User_code/device/led.h @@ -0,0 +1,33 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/gpio.h" +#include "bsp/pwm.h" +#include "bsp/bsp.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + + +/* LED ??? */ +typedef struct { +BSP_GPIO_t gpio; +BSP_PWM_Channel_t channel; +} DEVICE_LED_t; + + + extern DEVICE_LED_t LED_Map; +/* Exported functions prototypes -------------------------------------------- */ + + +int8_t BSP_LED_Set(char sign,DEVICE_LED_t ch,bool value,float duty_cycle); + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/motor_lk.c b/assets/User_code/device/motor_lk.c new file mode 100644 index 0000000..1527f66 --- /dev/null +++ b/assets/User_code/device/motor_lk.c @@ -0,0 +1,310 @@ +/* + LK电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lk.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ +#define LK_CTRL_ID_BASE (0x140) +#define LK_FB_ID_BASE (0x240) + +// LK电机命令字节定义 +#define LK_CMD_FEEDBACK (0x9C) // 反馈命令字节 +#define LK_CMD_MOTOR_OFF (0x80) // 电机关闭命令 +#define LK_CMD_MOTOR_ON (0x88) // 电机运行命令 +#define LK_CMD_TORQUE_CTRL (0xA1) // 转矩闭环控制命令 + +// LK电机参数定义 +#define LK_CURR_LSB_MF (33.0f / 4096.0f) // MF电机转矩电流分辨率 A/LSB +#define LK_CURR_LSB_MG (66.0f / 4096.0f) // MG电机转矩电流分辨率 A/LSB +#define LK_POWER_RANGE_MS (1000) // MS电机功率范围 ±1000 +#define LK_TORQUE_RANGE (2048) // 转矩控制值范围 ±2048 +#define LK_TORQUE_CURRENT_MF (16.5f) // MF电机最大转矩电流 A +#define LK_TORQUE_CURRENT_MG (33.0f) // MG电机最大转矩电流 A + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +// 编码器分辨率定义 +#define LK_ENC_14BIT_MAX (16383) // 14位编码器最大值 +#define LK_ENC_15BIT_MAX (32767) // 15位编码器最大值 +#define LK_ENC_16BIT_MAX (65535) // 16位编码器最大值 + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +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: + return LK_CURR_LSB_MF; + default: + return LK_CURR_LSB_MG; // 默认使用MG的分辨率 + } +} + +static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) { + // 根据电机型号返回编码器最大值，这里假设都使用16位编码器 + // 实际使用时需要根据具体电机型号配置 + return LK_ENC_16BIT_MAX; +} + +static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) { + // 检查命令字节是否为反馈命令 + if (msg->data[0] != LK_CMD_FEEDBACK) { + return; + } + + // 解析温度 (DATA[1]) + motor->motor.feedback.temp = (int8_t)msg->data[1]; + + // 解析转矩电流值或功率值 (DATA[2], DATA[3]) + int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]); + + // 根据电机类型解析电流或功率 + switch (motor->param.module) { + case MOTOR_MF9025: + case MOTOR_MF9035: + // MF/MG电机：转矩电流值 + motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module); + break; + default: + // MS电机：功率值（范围-1000~1000） + motor->motor.feedback.torque_current = (float)raw_current_or_power; // 将功率存储在torque_current字段中 + break; + } + + // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB + motor->motor.feedback.rotor_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]); + + // 解析编码器值 (DATA[6], DATA[7]) + uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]); + uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module); + + // 将编码器值转换为弧度 (0 ~ 2π) + motor->motor.feedback.rotor_abs_angle = (float)raw_encoder / (float)encoder_max * M_2PI; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_LK_CANManager_t *manager = MOTOR_LK_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 >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR; + + // 创建新电机实例 + MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t)); + if (new_motor == NULL) return DEVICE_ERR; + + memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + + // 计算反馈ID（假设为控制ID + 0x40） + uint16_t feedback_id = param->id + 0x40; + + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, feedback_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 MOTOR_LK_Update(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + // 计算反馈ID + uint16_t feedback_id = param->id + 0x100; + + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, feedback_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_LK_Decode(motor, &rx_msg); + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_LK_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + // 限制输出值范围 + if (value > 1.0f) value = 1.0f; + if (value < -1.0f) value = -1.0f; + + MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 转矩闭环控制命令 - 将输出值转换为转矩控制值 + int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE); + + // 构建CAN帧 + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 设置转矩闭环控制命令数据 + tx_frame.data[0] = LK_CMD_TORQUE_CTRL; // 命令字节 + tx_frame.data[1] = 0x00; // NULL + tx_frame.data[2] = 0x00; // NULL + tx_frame.data[3] = 0x00; // NULL + tx_frame.data[4] = (uint8_t)(torque_control & 0xFF); // 转矩电流控制值低字节 + tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); // 转矩电流控制值高字节 + tx_frame.data[6] = 0x00; // NULL + tx_frame.data[7] = 0x00; // NULL + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) { + // 对于LK电机，每次设置输出时就直接发送控制命令 + // 这个函数可以用于发送其他控制命令，如电机开启/关闭 + return DEVICE_OK; +} + +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机运行命令 + tx_frame.data[0] = LK_CMD_MOTOR_ON; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机关闭命令 + tx_frame.data[0] = LK_CMD_MOTOR_OFF; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) { + if (param == NULL) return NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) { + return MOTOR_LK_SetOutput(param, 0.0f); +} + +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) { + MOTOR_LK_t *motor = MOTOR_LK_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_lk.h b/assets/User_code/device/motor_lk.h new file mode 100644 index 0000000..ada3f41 --- /dev/null +++ b/assets/User_code/device/motor_lk.h @@ -0,0 +1,119 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_LK_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_MF9025, + MOTOR_MF9035, +} MOTOR_LK_Module_t; + + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + MOTOR_LK_Module_t module; + bool reverse; +} MOTOR_LK_Param_t; + +/*电机实例*/ +typedef struct{ + MOTOR_LK_Param_t param; + MOTOR_t motor; +} MOTOR_LK_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_LK_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个LK电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值，范围[-1.0, 1.0] + * @return + */ +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value); + +/** + * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机开启命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机关闭命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param); + +/** + * @brief + * @param + * @return + */ +int8_t MOTOR_LK_UpdateAll(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/motor_lz.c b/assets/User_code/device/motor_lz.c new file mode 100644 index 0000000..8c7afc7 --- /dev/null +++ b/assets/User_code/device/motor_lz.c @@ -0,0 +1,425 @@ +/* + 灵足电机驱动 + + 灵足电机通信协议： + - CAN 2.0通信接口，波特率1Mbps + - 采用扩展帧格式(29位ID) + - ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址) +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lz.h" +#include +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ +// 灵足电机协议参数 +#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */ +#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */ +#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */ +#define LZ_KP_MAX (5000.0f) /* Kp最大值 */ +#define LZ_KD_MAX (100.0f) /* Kd最大值 */ + +#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */ +#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */ + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private function prototypes ---------------------------------------------- */ +static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can); +static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can); +static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg); +static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id); +static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value); +static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value); +static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc); +static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); + +/* Private functions -------------------------------------------------------- */ + +/** + * @brief 获取CAN管理器 + */ +static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +/** + * @brief 创建CAN管理器 + */ +static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +/** + * @brief 构建扩展ID + */ +static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) { + uint32_t ext_id = 0; + ext_id |= ((uint32_t)cmd_type & 0x1F) << 24; // Bit28~24: 通信类型 + ext_id |= ((uint32_t)data2 & 0xFFFF) << 8; // Bit23~8: 数据区2 + ext_id |= ((uint32_t)target_id & 0xFF); // Bit7~0: 目标地址 + return ext_id; +} + +/** + * @brief 浮点值转换为原始值 + */ +static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) { + // 限制范围 + if (value > max_value) value = max_value; + if (value < -max_value) value = -max_value; + + // 转换为0~65535范围，对应-max_value~max_value + return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX); +} + +/** + * @brief 原始值转换为浮点值 + */ +static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) { + // 将0~65535范围转换为-max_value~max_value + return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value; +} + +/** + * @brief 发送扩展帧 + */ +static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) { + BSP_CAN_ExtDataFrame_t tx_frame; + tx_frame.id = ext_id; + tx_frame.dlc = dlc; + if (data != NULL) { + memcpy(tx_frame.data, data, dlc); + } else { + memset(tx_frame.data, 0, dlc); + } + + return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 灵足电机ID解析器 + * @param original_id 原始CAN ID（29位扩展帧） + * @param frame_type 帧类型 + * @return 解析后的ID（用于队列匹配） + * + * 灵足电机扩展ID格式： + * Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位) + * Bit23~8: 数据区2 (根据通信类型而定) + * Bit7~0: 目标地址 (目标电机CAN ID) + */ +static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) { + // 只处理扩展数据帧 + if (frame_type != BSP_CAN_FRAME_EXT_DATA) { + return original_id; // 非扩展帧直接返回原始ID + } + + // 解析扩展ID各个字段 + uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型 + uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2 + uint8_t target_id = original_id & 0xFF; // Bit7~0: 目标地址 + + // 对于反馈数据帧，我们使用特殊的解析规则 + if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) { + // 反馈数据的data2字段包含： + // Bit8~15: 当前电机CAN ID + // Bit16~21: 故障信息 + // Bit22~23: 模式状态 + uint8_t motor_can_id = data2 & 0xFF; + + // 返回格式化的ID，便于匹配 + // 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=目标ID) + return (0x02000000) | ((data2 & 0xFF00) << 8) | (motor_can_id << 8) | target_id; + } + + // 对于其他命令类型，直接返回原始ID + return original_id; +} + +/** + * @brief 解码灵足电机反馈数据 + */ +static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) { + if (motor == NULL || msg == NULL) return; + + // 检查是否为反馈数据帧 (通信类型2) + uint8_t cmd_type = (msg->parsed_id >> 24) & 0x1F; + if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return; + + // 解析ID中的信息 + uint16_t id_data2 = (msg->parsed_id >> 8) & 0xFFFF; + uint8_t motor_can_id = id_data2 & 0xFF; // Bit8~15: 当前电机CAN ID + uint8_t fault_info = (id_data2 >> 8) & 0x3F; // Bit16~21: 故障信息 + uint8_t mode_state = (id_data2 >> 14) & 0x03; // Bit22~23: 模式状态 + + // 更新电机CAN ID + motor->lz_feedback.motor_can_id = motor_can_id; + + // 解析故障信息 + motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0; // bit16 + motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0; // bit17 + motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0; // bit18 + motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0; // bit19 + motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0; // bit20 + motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0; // bit21 + + // 解析模式状态 + 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]); + 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]); + 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]); + 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]); + motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE; + + // 更新通用电机反馈信息 + motor->motor.feedback.rotor_abs_angle = motor->lz_feedback.current_angle; + motor->motor.feedback.rotor_speed = motor->lz_feedback.current_velocity * 180.0f / M_PI * 6.0f; // 转换为RPM + motor->motor.feedback.torque_current = motor->lz_feedback.current_torque; // 使用力矩作为电流反馈 + motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature; + + // 更新在线状态 + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Init(void) { + // 注册灵足电机专用的ID解析器 + return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 反初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_DeInit(void) { + // 注销ID解析器 + return BSP_CAN_UnregisterIdParser() == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_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.motor_id == param->motor_id) { + return DEVICE_ERR; // 已注册 + } + } + + // 检查数量 + if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR; + + // 创建新电机实例 + MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t)); + if (new_motor == NULL) return DEVICE_ERR; + + memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t)); + memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t)); + + new_motor->motor.reverse = param->reverse; + + // 注册CAN接收ID - 使用解析后的反馈数据ID + // 构建原始扩展ID + uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->host_id, param->motor_id); + // 通过ID解析器得到解析后的ID + uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA); + + if (BSP_CAN_RegisterId(param->can, parsed_feedback_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 MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor && motor->param.motor_id == param->motor_id) { + // 获取反馈数据 - 使用解析后的ID + uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->host_id, param->motor_id); + uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA); + BSP_CAN_Message_t msg; + + while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) { + MOTOR_LZ_Decode(motor, &msg); + } + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_LZ_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor) { + if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) { + if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 更新运控参数 + memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t)); + + // 构建扩展ID + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, 0, param->motor_id); + + // 准备数据 + uint8_t data[8]; + + // 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; + + // 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; + + // 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; + + // 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; + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 使能命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id); + + // 数据区清零 + uint8_t data[8] = {0}; + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 停止命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id); + + // 数据区 + uint8_t data[8] = {0}; + if (clear_fault) { + data[0] = 1; // Byte[0]=1时清故障 + } + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 设置零位命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id); + + // 数据区 - Byte[0]=1 + uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0}; + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) { + if (param == NULL) return NULL; + + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor && motor->param.motor_id == param->motor_id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) { + return MOTOR_LZ_Disable(param, false); +} + +int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) { + MOTOR_LZ_t *motor = MOTOR_LZ_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_lz.h b/assets/User_code/device/motor_lz.h new file mode 100644 index 0000000..1b65ced --- /dev/null +++ b/assets/User_code/device/motor_lz.h @@ -0,0 +1,193 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_LZ_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_LZ_RSO0, + MOTOR_LZ_RSO1, + MOTOR_LZ_RSO2, + MOTOR_LZ_RSO3, + MOTOR_LZ_RSO4, + MOTOR_LZ_RSO5, + MOTOR_LZ_RSO6, +} MOTOR_LZ_Module_t; + +/* 灵足电机控制模式 */ +typedef enum { + MOTOR_LZ_MODE_MOTION = 0x1, /* 运控模式 */ + MOTOR_LZ_MODE_CURRENT = 0x2, /* 电流模式 */ + MOTOR_LZ_MODE_VELOCITY = 0x3, /* 速度模式 */ + MOTOR_LZ_MODE_POSITION = 0x4, /* 位置模式 */ +} MOTOR_LZ_ControlMode_t; + +/* 灵足电机通信类型 */ +typedef enum { + MOTOR_LZ_CMD_MOTION = 0x1, /* 运控模式控制 */ + MOTOR_LZ_CMD_FEEDBACK = 0x2, /* 电机反馈数据 */ + MOTOR_LZ_CMD_ENABLE = 0x3, /* 电机使能运行 */ + MOTOR_LZ_CMD_DISABLE = 0x4, /* 电机停止运行 */ + MOTOR_LZ_CMD_SET_ZERO = 0x6, /* 设置电机机械零位 */ +} MOTOR_LZ_CmdType_t; + +/* 灵足电机运行状态 */ +typedef enum { + MOTOR_LZ_STATE_RESET = 0, /* Reset模式[复位] */ + MOTOR_LZ_STATE_CALI = 1, /* Cali模式[标定] */ + MOTOR_LZ_STATE_MOTOR = 2, /* Motor模式[运行] */ +} MOTOR_LZ_State_t; + +/* 灵足电机故障信息 */ +typedef struct { + bool uncalibrated; /* bit21: 未标定 */ + bool stall_overload; /* bit20: 堵转过载故障 */ + bool encoder_fault; /* bit19: 磁编码故障 */ + bool over_temp; /* bit18: 过温 */ + bool driver_fault; /* bit17: 驱动故障 */ + bool under_voltage; /* bit16: 欠压故障 */ +} MOTOR_LZ_Fault_t; + +/* 灵足电机运控参数 */ +typedef struct { + float target_angle; /* 目标角度 (-12.57f~12.57f rad) */ + float target_velocity; /* 目标角速度 (-20~20 rad/s) */ + float kp; /* 位置增益 (0.0~5000.0) */ + float kd; /* 微分增益 (0.0~100.0) */ + float torque; /* 力矩 (-60~60 Nm) */ +} MOTOR_LZ_MotionParam_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; /* CAN总线 */ + uint8_t motor_id; /* 电机CAN ID */ + uint8_t host_id; /* 主机CAN ID */ + MOTOR_LZ_Module_t module; /* 电机型号 */ + bool reverse; /* 是否反向 */ + MOTOR_LZ_ControlMode_t mode; /* 控制模式 */ +} MOTOR_LZ_Param_t; + +/*电机反馈信息扩展*/ +typedef struct { + float current_angle; /* 当前角度 (-12.57f~12.57f rad) */ + float current_velocity; /* 当前角速度 (-20~20 rad/s) */ + float current_torque; /* 当前力矩 (-60~60 Nm) */ + float temperature; /* 当前温度 (摄氏度) */ + MOTOR_LZ_State_t state; /* 运行状态 */ + MOTOR_LZ_Fault_t fault; /* 故障信息 */ + uint8_t motor_can_id; /* 当前电机CAN ID */ +} MOTOR_LZ_Feedback_t; + +/*电机实例*/ +typedef struct { + MOTOR_LZ_Param_t param; + MOTOR_t motor; + MOTOR_LZ_Feedback_t lz_feedback; /* 灵足电机特有反馈信息 */ + MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */ +} MOTOR_LZ_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_LZ_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Init(void); + +/** + * @brief 反初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_DeInit(void); + +/** + * @brief 注册一个灵足电机 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param); + +/** + * @brief 更新所有电机数据 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_UpdateAll(void); + +/** + * @brief 运控模式控制电机 + * @param param 电机参数 + * @param motion_param 运控参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param); + +/** + * @brief 电机使能运行 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param); + +/** + * @brief 电机停止运行 + * @param param 电机参数 + * @param clear_fault 是否清除故障 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault); + +/** + * @brief 设置电机机械零位 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return 电机实例指针，失败返回NULL + */ +MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param); + +/** + * @brief 使电机松弛（发送停止命令） + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/servo.c b/assets/User_code/device/servo.c new file mode 100644 index 0000000..85cdcd7 --- /dev/null +++ b/assets/User_code/device/servo.c @@ -0,0 +1,39 @@ +/* + pwmƶ +*/ + +/*Includes -----------------------------------------*/ + +#include "bsp/pwm.h" +#include "servo.h" + +#define SERVO_MIN_DUTY 0.025f +#define SERVO_MAX_DUTY 0.125f + +/** + * @brief + * @param + * @retval BSP_OK / BSP_ERR + */ + +int8_t SERVO_Init(SERVO_t *servo) { + if (servo == NULL) return BSP_ERR; + return BSP_PWM_Start(servo->pwm_ch); +} + +int8_t SERVO_SetAngle(SERVO_t *servo, float angle) { + if (servo == NULL) return BSP_ERR; + + /*ƽǶȷΧ*/ + if (angle < 0.0f) angle = 0.0f; + if (angle > 180.0f) angle = 180.0f; + /*Ƕӳ䵽ռձ*/ + float duty = servo->min_duty + (angle / 180.0f) * (servo->max_duty - servo->min_duty); + + return BSP_PWM_Set(servo->pwm_ch, duty); +} + +int8_t SERVO_Stop(SERVO_t *servo) { + if (servo == NULL) return BSP_ERR; + return BSP_PWM_Stop(servo->pwm_ch); +} \ No newline at end of file diff --git a/assets/User_code/device/servo.h b/assets/User_code/device/servo.h new file mode 100644 index 0000000..9957ef6 --- /dev/null +++ b/assets/User_code/device/servo.h @@ -0,0 +1,52 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp\pwm.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/** + * @brief ??????? + */ +typedef struct { + BSP_PWM_Channel_t pwm_ch; + float min_duty; + float max_duty; +} SERVO_t; + +/** + * @brief + * @param servo + * @retval BSP_OK / BSP_ERR + */ + +int8_t SERVO_Init(SERVO_t *servo); + +/** + * @brief + * @param servo + * @param angle + * @retval BSP_OK / BSP_ERR + */ +int8_t SERVO_SetAngle(SERVO_t *servo, float angle); + +/** + * @brief + * @param servo + * @retval BSP_OK / BSP_ERR + */ + +int8_t SERVO_Stop(SERVO_t *servo); + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/ws2812.c b/assets/User_code/device/ws2812.c new file mode 100644 index 0000000..45747b9 --- /dev/null +++ b/assets/User_code/device/ws2812.c @@ -0,0 +1,96 @@ +/* Includes ----------------------------------------------------------------- */ +#include "ws2812.h" +#include "device.h" + +#include "bsp/pwm.h" +#include +/* Private define ----------------------------------------------------------- */ +#define DEVICE_WS2812_T1H (uint16_t)(BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.56) // High-level width of logic-1 pulse +#define DEVICE_WS2812_T0H (BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.29) // High-level width of logic-0 pulse +#define DEVICE_WS2812_WS_REST 40 // Number of reset pulses (low level) after data stream +#define DEVICE_WS2812_DATA_LEN 24 // WS2812 data length: 24 bits (GRB) per LED +#define DEVICE_WS2812_RST_NUM 50 // Extra reset pulses reserved at the end of the buffer +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static uint16_t DEVICE_WS2812_LED_NUM; // Total number of LEDs +static uint16_t *DEVICE_WS2812_RGB_Buff = NULL;// PWM duty buffer for DMA +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +/** + * Set color of a single WS2812 LED + * @param num LED index (1-based) + * @param R Red value (0-255) + * @param G Green value (0-255) + * @param B Blue value (0-255) + * @return DEVICE_OK on success, DEVICE_ERR if num is invalid + */ +uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B) +{ + if(num<1 || num>DEVICE_WS2812_LED_NUM) return DEVICE_ERR; + uint32_t indexx = (num-1) * DEVICE_WS2812_DATA_LEN; + + /* WS2812 uses GRB order, MSB first */ + for (uint8_t i = 0; i < 8; i++) { + // G + DEVICE_WS2812_RGB_Buff[indexx + i] = (G & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H; + // R + DEVICE_WS2812_RGB_Buff[indexx + i + 8] = (R & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H; + // B + DEVICE_WS2812_RGB_Buff[indexx + i + 16] = (B & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H; + } + return DEVICE_OK; +} + +/** + * Initialize WS2812 driver + * @param ledNum Number of LEDs in the strip + * @return DEVICE_OK on success, DEVICE_ERR if memory allocation or PWM setup fails + */ +uint8_t DEVICE_WS2812_Init(uint16_t ledNum) +{ + DEVICE_WS2812_LED_NUM = ledNum; + + if (DEVICE_WS2812_RGB_Buff != NULL) + { + free(DEVICE_WS2812_RGB_Buff); + DEVICE_WS2812_RGB_Buff = NULL; + } + + /* Allocate new buffer: 24 PWM samples per LED + reset pulses */ + size_t bufLen = ledNum * DEVICE_WS2812_DATA_LEN + DEVICE_WS2812_RST_NUM; + DEVICE_WS2812_RGB_Buff = (uint16_t *)malloc(bufLen * sizeof(uint16_t)); + if (DEVICE_WS2812_RGB_Buff == NULL) + return DEVICE_ERR; + + /* Initialize all LEDs to dim green */ + for (int i = 1; i <= ledNum; i++) + DEVICE_WS2812_Set(i, 0, 20, 0); + + /* Configure PWM frequency to 800 kHz and start DMA */ + if (BSP_PWM_SetFreq(BSP_PWM_WS2812, 800000) == DEVICE_OK) + BSP_PWM_Start_DMA( + BSP_PWM_WS2812, + (uint32_t *)DEVICE_WS2812_RGB_Buff, + bufLen); + else + return DEVICE_ERR; + + return DEVICE_OK; +} + +/** + * De-initialize WS2812 driver + * Frees the DMA buffer and stops PWM + */ +void DEVICE_WS2812_DeInit() +{ + for (int i = 1; i <= DEVICE_WS2812_LED_NUM; i++) + DEVICE_WS2812_Set(i, 0, 0, 0); + if (DEVICE_WS2812_RGB_Buff != NULL) + { + free(DEVICE_WS2812_RGB_Buff); + DEVICE_WS2812_RGB_Buff = NULL; + } + BSP_PWM_Stop_DMA(BSP_PWM_WS2812); +} \ No newline at end of file diff --git a/assets/User_code/device/ws2812.h b/assets/User_code/device/ws2812.h new file mode 100644 index 0000000..9a26cfa --- /dev/null +++ b/assets/User_code/device/ws2812.h @@ -0,0 +1,19 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +uint8_t DEVICE_WS2812_Init(uint16_t led_num); +uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B); +void DEVICE_WS2812_DeInit(); + +#ifdef __cplusplus +} +#endif \ No newline at end of file