mirror of
https://github.com/goldenfishs/MRobot.git
synced 2025-09-14 12:54:33 +08:00
修改了can，添加了4个device
This commit is contained in:
parent
d61dfa3634
commit
79f7671a9f
BIN
assets/.DS_Store
vendored
BIN
assets/.DS_Store
vendored
Binary file not shown.
BIN
assets/User_code/.DS_Store
vendored
BIN
assets/User_code/.DS_Store
vendored
Binary file not shown.
@ -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;
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
|
||||
BIN
assets/User_code/device/.DS_Store
vendored
BIN
assets/User_code/device/.DS_Store
vendored
Binary file not shown.
44
assets/User_code/device/buzzer.c
Normal file
44
assets/User_code/device/buzzer.c
Normal file
@ -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;
|
||||
}
|
||||
36
assets/User_code/device/buzzer.h
Normal file
36
assets/User_code/device/buzzer.h
Normal file
@ -0,0 +1,36 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "device.h"
|
||||
#include "bsp/pwm.h"
|
||||
#include <stddef.h>
|
||||
|
||||
/* 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
|
||||
@ -107,4 +107,19 @@ devices:
|
||||
description: "通用电机驱动"
|
||||
files:
|
||||
header: "motor.h"
|
||||
source: "motor.c"
|
||||
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"
|
||||
47
assets/User_code/device/led.c
Normal file
47
assets/User_code/device/led.c
Normal file
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
33
assets/User_code/device/led.h
Normal file
33
assets/User_code/device/led.h
Normal file
@ -0,0 +1,33 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <stdint.h>
|
||||
#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
|
||||
310
assets/User_code/device/motor_lk.c
Normal file
310
assets/User_code/device/motor_lk.c
Normal file
@ -0,0 +1,310 @@
|
||||
/*
|
||||
LK电机驱动
|
||||
*/
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "motor_lk.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 ----------------------------------------------------------- */
|
||||
#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;
|
||||
}
|
||||
119
assets/User_code/device/motor_lk.h
Normal file
119
assets/User_code/device/motor_lk.h
Normal file
@ -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
|
||||
425
assets/User_code/device/motor_lz.c
Normal file
425
assets/User_code/device/motor_lz.c
Normal file
@ -0,0 +1,425 @@
|
||||
/*
|
||||
灵足电机驱动
|
||||
|
||||
灵足电机通信协议：
|
||||
- CAN 2.0通信接口，波特率1Mbps
|
||||
- 采用扩展帧格式(29位ID)
|
||||
- ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
|
||||
*/
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "motor_lz.h"
|
||||
#include <stdint.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 ----------------------------------------------------------- */
|
||||
// 灵足电机协议参数
|
||||
#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;
|
||||
}
|
||||
193
assets/User_code/device/motor_lz.h
Normal file
193
assets/User_code/device/motor_lz.h
Normal file
@ -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
|
||||
39
assets/User_code/device/servo.c
Normal file
39
assets/User_code/device/servo.c
Normal file
@ -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);
|
||||
}
|
||||
52
assets/User_code/device/servo.h
Normal file
52
assets/User_code/device/servo.h
Normal file
@ -0,0 +1,52 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <cmsis_os2.h>
|
||||
|
||||
#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
|
||||
96
assets/User_code/device/ws2812.c
Normal file
96
assets/User_code/device/ws2812.c
Normal file
@ -0,0 +1,96 @@
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "ws2812.h"
|
||||
#include "device.h"
|
||||
|
||||
#include "bsp/pwm.h"
|
||||
#include <stdlib.h>
|
||||
/* 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);
|
||||
}
|
||||
19
assets/User_code/device/ws2812.h
Normal file
19
assets/User_code/device/ws2812.h
Normal file
@ -0,0 +1,19 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <stdint.h>
|
||||
/* 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
|
||||
Loading…
Reference in New Issue
Block a user