From 483d5f0ac4fd520e336432a13b6384ce920ea9ea Mon Sep 17 00:00:00 2001 From: TuxMonkey Date: Thu, 5 Mar 2026 02:06:41 +0800 Subject: [PATCH] add DM motor drv --- .../motor/can_motors/dm_motor/dm_motor.c | 274 ++++++++++++++++++ .../motor/can_motors/dm_motor/dm_motor.h | 96 ++++++ .../motor/can_motors/dm_motor/dm_motor.md | 95 +++++- 3 files changed, 464 insertions(+), 1 deletion(-) create mode 100644 User_Code/module/motor/can_motors/dm_motor/dm_motor.c create mode 100644 User_Code/module/motor/can_motors/dm_motor/dm_motor.h diff --git a/User_Code/module/motor/can_motors/dm_motor/dm_motor.c b/User_Code/module/motor/can_motors/dm_motor/dm_motor.c new file mode 100644 index 0000000..96bdd5c --- /dev/null +++ b/User_Code/module/motor/can_motors/dm_motor/dm_motor.c @@ -0,0 +1,274 @@ +#include "dm_motor.h" +#include "bsp_log.h" +#include "cmsis_os.h" +#include "daemon.h" +#include "general_def.h" +#include "memory.h" +#include "motor_def.h" +#include "stdlib.h" +#include "string.h" +#include "user_lib.h" + +static uint8_t idx; +static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT]; +static TaskHandle_t dm_task_handle[DM_MOTOR_CNT]; +/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */ +static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits) +{ + float span = x_max - x_min; + float offset = x_min; + return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span); +} +static float uint_to_float(int x_int, float x_min, float x_max, int bits) +{ + float span = x_max - x_min; + float offset = x_min; + return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset; +} + +static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) +{ + memset(motor->motor_can_instance->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff + motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id + CANTransmit(motor->motor_can_instance, 1); +} + +static void DMMotorDecode(FDCANInstance *motor_can) +{ + uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 + uint8_t *rxbuff = motor_can->rx_buff; + DMMotorInstance *motor = (DMMotorInstance *)motor_can->id; + DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针 + + DaemonReload(motor->motor_daemon); + + measure->last_position = measure->position; + + // 区分一控四模式和MIT模式的反馈解析 + if (motor->ctrl_mode == DM_CTRL_ONE_TO_FOUR) + { + // 标识符为0x300+电机ID时的解析逻辑 + // D[0], D[1] 为位置高/低8位，范围0-8191对应一圈位置 + tmp = (uint16_t)((rxbuff[0] << 8) | rxbuff[1]); + measure->position = (float)tmp; + + // D[2], D[3] 为速度高/低8位，单位rpm放大一百倍 + int16_t vel_tmp = (int16_t)((rxbuff[2] << 8) | rxbuff[3]); + measure->velocity = (float)vel_tmp / 100.0f; + + // D[4], D[5] 为扭矩电流高/低8位，单位mA + int16_t torq_tmp = (int16_t)((rxbuff[4] << 8) | rxbuff[5]); + measure->torque = (float)torq_tmp / 1000.0f; + + // D[6] 为电机线圈温度 + measure->T_Mos = (float)rxbuff[6]; + + // D[7] 为错误状态 + measure->state = rxbuff[7]; + } + else + { + // 原MIT反馈解析逻辑 + tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); + measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16); + + tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4); + measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12); + + tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]); + measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12); + + measure->T_Mos = (float)rxbuff[6]; + measure->T_Rotor = (float)rxbuff[7]; + } +} + +static void DMMotorLostCallback(void *motor_ptr) +{ +} +void DMMotorCaliEncoder(DMMotorInstance *motor) +{ + DMMotorSetMode(DM_CMD_ZERO_POSITION, motor); + DWT_Delay(0.1); +} +DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) +{ + DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance)); + memset(motor, 0, sizeof(DMMotorInstance)); + + // 默认初始化为MIT模式 + motor->ctrl_mode = DM_CTRL_MIT; + + motor->motor_settings = config->controller_setting_init_config; + PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID); + PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID); + PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID); + motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; + motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; + + config->fdcan_init_config.can_module_callback = DMMotorDecode; + config->fdcan_init_config.id = motor; + motor->motor_can_instance = CANRegister(&config->fdcan_init_config); + + Daemon_Init_Config_s conf = { + .callback = DMMotorLostCallback, + .owner_id = motor, + .reload_count = 10, + }; + motor->motor_daemon = DaemonRegister(&conf); + + DMMotorEnable(motor); + DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); + DWT_Delay(0.1); + DMMotorCaliEncoder(motor); + DWT_Delay(0.1); + dm_motor_instance[idx++] = motor; + return motor; +} + +void DMMotorSetRef(DMMotorInstance *motor, float ref) +{ + motor->pid_ref = ref; +} + +void DMMotorEnable(DMMotorInstance *motor) +{ + motor->stop_flag = MOTOR_ENALBED; +} + +void DMMotorStop(DMMotorInstance *motor)//不使用使能模式是因为需要收到反馈 +{ + motor->stop_flag = MOTOR_STOP; +} + +void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type) +{ + motor->motor_settings.outer_loop_type = type; +} + +void DMMotorSetCtrlMode(DMMotorInstance *motor, DMMotor_Ctrl_Mode_e mode) +{ + motor->ctrl_mode = mode; +} + +// 一控四下发指令：支持1帧控制4个电机，由外部统一调用，不要在DMMotorTask中高频调用此函数 +void DMMotorSendOneToFourGroup(FDCANInstance *can_instance, uint8_t group, float i1, float i2, float i3, float i4) +{ + // 根据电机ID组配置对应的报文ID：[1,4]为0x3FE, [5,8]为0x4FE + uint32_t tx_id = (group == 1) ? 0x3FE : 0x4FE; + + // 控制电流为标幺值，采用力位混控i_des相同的16位映射机制进行量化 + uint16_t cur1 = float_to_uint(i1, DM_T_MIN, DM_T_MAX, 16); + uint16_t cur2 = float_to_uint(i2, DM_T_MIN, DM_T_MAX, 16); + uint16_t cur3 = float_to_uint(i3, DM_T_MIN, DM_T_MAX, 16); + uint16_t cur4 = float_to_uint(i4, DM_T_MIN, DM_T_MAX, 16); + + uint32_t old_id = can_instance->tx_id; + can_instance->tx_id = tx_id; + + // 数据段填充：先低8位，再高8位 + can_instance->tx_buff[0] = (uint8_t)(cur1 & 0xFF); + can_instance->tx_buff[1] = (uint8_t)(cur1 >> 8); + can_instance->tx_buff[2] = (uint8_t)(cur2 & 0xFF); + can_instance->tx_buff[3] = (uint8_t)(cur2 >> 8); + can_instance->tx_buff[4] = (uint8_t)(cur3 & 0xFF); + can_instance->tx_buff[5] = (uint8_t)(cur3 >> 8); + can_instance->tx_buff[6] = (uint8_t)(cur4 & 0xFF); + can_instance->tx_buff[7] = (uint8_t)(cur4 >> 8); + + CANTransmit(can_instance, 1); + + can_instance->tx_id = old_id; // 恢复旧有配置 +} + +// 一控四模式下特殊清零指令 +void DMMotorSetZeroOneToFour(FDCANInstance *can_instance, uint16_t target_can_id) +{ + uint32_t old_id = can_instance->tx_id; + can_instance->tx_id = 0x7FF; // 零点设置特殊指令报文ID + + // 依序填入CANID和固定魔法字 + can_instance->tx_buff[0] = (uint8_t)(target_can_id & 0xFF); + can_instance->tx_buff[1] = (uint8_t)(target_can_id >> 8); + can_instance->tx_buff[2] = 0x55; + can_instance->tx_buff[3] = 0x50; + can_instance->tx_buff[4] = 0x00; + can_instance->tx_buff[5] = 0x00; + can_instance->tx_buff[6] = 0x00; + can_instance->tx_buff[7] = 0x00; + + CANTransmit(can_instance, 1); + can_instance->tx_id = old_id; +} + + +//@Todo: MIT模式目前只实现了力控，更多位控PID等请自行添加 +void DMMotorTask(void *argument) +{ + float pid_ref, set; + DMMotorInstance *motor = (DMMotorInstance *)argument; + Motor_Control_Setting_s *setting = &motor->motor_settings; + DMMotor_Send_s motor_send_mailbox; + while (1) + { + // 若当前实例设为了一控四模式，不应由单独的电机Task发送报文 + // 需要在用户外部的任务里定期调用 DMMotorSendOneToFourGroup() + if (motor->ctrl_mode == DM_CTRL_ONE_TO_FOUR) + { + osDelay(2); + continue; + } + + pid_ref = motor->pid_ref; + + set = pid_ref; + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + set *= -1; + + LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX); + motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16); + motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12); + motor_send_mailbox.torque_des = float_to_uint(pid_ref, DM_T_MIN, DM_T_MAX, 12); + motor_send_mailbox.Kp = 0; + motor_send_mailbox.Kd = 0; + + if(motor->stop_flag == MOTOR_STOP) + motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12); + + motor->motor_can_instance->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8); + motor->motor_can_instance->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des); + motor->motor_can_instance->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4); + motor->motor_can_instance->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8)); + motor->motor_can_instance->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp); + motor->motor_can_instance->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4); + motor->motor_can_instance->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8)); + motor->motor_can_instance->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des); + + CANTransmit(motor->motor_can_instance, 1); + + osDelay(2); + } +} + +void DMMotorControlInit() +{ + // 遍历所有电机实例,创建任务 + if (!idx) + return; + + // 注意：CMSIS-RTOS V2的osThreadDef不支持动态生成的名称 + // 我们需要为每个电机创建独立的线程定义或使用不同的方法 + + // 方案1：使用循环和预定义的线程定义（如果电机数量固定） + // 这里改为直接使用FreeRTOS原生API创建线程，更加灵活可靠 + + for (size_t i = 0; i < idx; i++) + { + // 使用FreeRTOS原生API创建线程 + // 参数：线程函数、线程名称、堆栈大小、参数、优先级、线程句柄 + if (xTaskCreate(DMMotorTask, "DMMotorTask", 128, dm_motor_instance[i], osPriorityNormal, &dm_task_handle[i]) != pdPASS) + { + LOGERROR("[DM_Motor] Failed to create motor thread for motor %d", i); + } + } +} \ No newline at end of file diff --git a/User_Code/module/motor/can_motors/dm_motor/dm_motor.h b/User_Code/module/motor/can_motors/dm_motor/dm_motor.h new file mode 100644 index 0000000..12ddee8 --- /dev/null +++ b/User_Code/module/motor/can_motors/dm_motor/dm_motor.h @@ -0,0 +1,96 @@ +#ifndef DM_MOTOR_H +#define DM_MOTOR_H +#include +#include "bsp_fdcan.h" +#include "pid.h" +#include "motor_def.h" +#include "daemon.h" + +#define DM_MOTOR_CNT 4 + +#define DM_P_MIN (-12.5f) +#define DM_P_MAX 12.5f +#define DM_V_MIN (-45.0f) +#define DM_V_MAX 45.0f +#define DM_T_MIN (-18.0f) +#define DM_T_MAX 18.0f + +// 新增：电机控制模式枚举 +typedef enum { + DM_CTRL_MIT = 0,//MIT模式，默认的单电机控制模式 + DM_CTRL_ONE_TO_FOUR = 1, // 一控四模式 +} DMMotor_Ctrl_Mode_e; + +typedef struct +{ + uint8_t id; + uint8_t state; + float velocity; + float last_position; + float position; + float torque; + float T_Mos; + float T_Rotor; + int32_t total_round; +}DM_Motor_Measure_s; + +typedef struct +{ + uint16_t position_des; + uint16_t velocity_des; + uint16_t torque_des; + uint16_t Kp; + uint16_t Kd; +}DMMotor_Send_s; + +typedef struct +{ + DM_Motor_Measure_s measure; + Motor_Control_Setting_s motor_settings; + PIDInstance current_PID; + PIDInstance speed_PID; + PIDInstance angle_PID; + float *other_angle_feedback_ptr; + float *other_speed_feedback_ptr; + float *speed_feedforward_ptr; + float *current_feedforward_ptr; + float pid_ref; + Motor_Working_Type_e stop_flag; + FDCANInstance *motor_can_instance; + Daemon_Instance* motor_daemon; + uint32_t lost_cnt; + + // 新增：当前电机的控制模式标志 + DMMotor_Ctrl_Mode_e ctrl_mode; +}DMMotorInstance; + +typedef enum +{ + DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令 + DM_CMD_RESET_MODE = 0xfd, // 停止 + DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位 + DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误 +}DMMotor_Mode_e; + +DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config); + +void DMMotorSetRef(DMMotorInstance *motor, float ref); + +void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type); + +void DMMotorEnable(DMMotorInstance *motor); + +void DMMotorStop(DMMotorInstance *motor); +void DMMotorCaliEncoder(DMMotorInstance *motor); +void DMMotorControlInit(); + +// 新增：设置电机控制模式 +void DMMotorSetCtrlMode(DMMotorInstance *motor, DMMotor_Ctrl_Mode_e mode); + +// 新增：一控四模式下，单控制帧发送4个电机的电流 +void DMMotorSendOneToFourGroup(FDCANInstance *can_instance, uint8_t group, float i1, float i2, float i3, float i4); + +// 新增：一控四模式下的零点设置 +void DMMotorSetZeroOneToFour(FDCANInstance *can_instance, uint16_t target_can_id); + +#endif // !DMMOTOR \ No newline at end of file diff --git a/User_Code/module/motor/can_motors/dm_motor/dm_motor.md b/User_Code/module/motor/can_motors/dm_motor/dm_motor.md index 1588e09..a3ab9d8 100644 --- a/User_Code/module/motor/can_motors/dm_motor/dm_motor.md +++ b/User_Code/module/motor/can_motors/dm_motor/dm_motor.md @@ -1 +1,94 @@ -# 达妙电机 \ No newline at end of file +# 达妙电机 + +在 `gimbal.c` 中进行达妙电机的初始化配置并使用“一拖四”模式发送指令，你可以按照以下结构来组织代码。 + +### 1. 初始化配置与指定ID + +在初始化阶段，你需要先配置好 `Motor_Init_Config_s`，并通过刚才新增的 `DMMotorSetCtrlMode` 函数将实例切换为一拖四模式。电机的 **ID** 是在 `can_init_config.tx_id` 中指定的。 + +```c +#include "DMmotor.h" + +// 1. 定义电机配置结构体 (以ID为1的电机为例) +static Motor_Init_Config_s gimbal_dm_config_id1 = { + .can_init_config = { + .can_handle = &hcan1, // 指定使用的CAN外设句柄，例如 hcan1 或 hcan2 + .tx_id = 0x01, // 【指定电机ID】这里填入电机的实际ID，如 1 + }, + .controller_param_init_config = { + // 虽然一控四主要是直接下发电流，但为了结构完整性或外环计算，可配置相关PID + .current_PID = { + .Kp = 6.0f, + .Ki = 0.0f, + .Kd = 0.495f, + .MaxOut = 45.0f, + }, + }, + .controller_setting_init_config = { + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + } +}; + +// 2. 声明电机实例指针 +DMMotorInstance *gimbal_motor_1; +// 如果同一条总线上有另外三个电机，你需要分别为它们声明实例并配置 tx_id = 2, 3, 4 + +void Gimbal_Init(void) +{ + // 3. 调用Init函数完成底层初始化与实例分配 + gimbal_motor_1 = DMMotorInit(&gimbal_dm_config_id1); + + // 4. 【关键步骤】将该电机控制模式切换为一控四模式 + DMMotorSetCtrlMode(gimbal_motor_1, DM_CTRL_ONE_TO_FOUR); + + // (同理，对ID为2、3、4的电机执行相同的Init和SetCtrlMode操作) +} + +``` + +### 2. 使用一拖四模式发送指令 + +在控制任务（例如 FreeRTOS 的 `Gimbal_Task`）中，你不再需要让每个电机单独发送报文，而是通过刚才新增的 `DMMotorSendOneToFourGroup` 函数**统一打包下发**。 + +```c +void Gimbal_Task(void const * argument) +{ + // 假设通过你的控制器（如LQR或MPC等）计算得出了4个电机的目标电流 + // 单位与你设定DM_T_MIN、DM_T_MAX的量纲一致 + float target_i1 = 1.5f; + float target_i2 = -0.5f; + float target_i3 = 2.0f; + float target_i4 = 0.0f; + + while(1) + { + // ... (各种控制算法计算过程) ... + + // 调用一控四发送函数打包下发控制帧 + // 参数1: can_instance -> 传入挂载在该CAN总线上的任一电机实例的CAN指针即可 + // 参数2: group -> 1 表示控制电机 ID[1~4] (对应报文 0x3FE) + // 2 表示控制电机 ID[5~8] (对应报文 0x4FE) + // 参数3~6: 分别对应这4个电机的电流值 + DMMotorSendOneToFourGroup(gimbal_motor_1->motor_can_instace, 1, + target_i1, target_i2, target_i3, target_i4); + + osDelay(2); + } +} + +``` + +### 3. 一拖四模式下的零点设置 (附加) + +如果你在调试时需要将某台电机当前的位置设置为编码器零位，可以调用对应的零点校准函数，传入目标电机的ID： + +```c +void Gimbal_Set_Zero(void) +{ + // 将总线上 ID = 1 的电机当前位置设为零点 + DMMotorSetZeroOneToFour(gimbal_motor_1->motor_can_instace, 0x01); +} + +``` + +按照这种方式组织 `gimbal.c`，底层的CAN发送与接收解析就会被彻底隔离开，既保证了多电机联合控制的同步性，又能极大节省 CAN 总线的带宽。 \ No newline at end of file