From 817850a0a8661f262a87638a161acb5e1032a1ce Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Wed, 29 Nov 2023 21:48:07 +0800 Subject: [PATCH] add driver for DMMotor --- Makefile | 2 + modules/motor/DMmotor/dmmotor.c | 173 ++++++++++++++++++++++++++++++++ modules/motor/DMmotor/dmmotor.h | 76 ++++++++++++++ 3 files changed, 251 insertions(+) create mode 100644 modules/motor/DMmotor/dmmotor.c create mode 100644 modules/motor/DMmotor/dmmotor.h diff --git a/Makefile b/Makefile index a1e97ff..1f4f42f 100644 --- a/Makefile +++ b/Makefile @@ -135,6 +135,7 @@ modules/master_machine/seasky_protocol.c \ modules/motor/DJImotor/dji_motor.c \ modules/motor/HTmotor/HT04.c \ modules/motor/LKmotor/LK9025.c \ +modules/motor/DMmotor/dmmotor.c \ modules/motor/step_motor/step_motor.c \ modules/motor/servo_motor/servo_motor.c \ modules/motor/motor_task.c \ @@ -255,6 +256,7 @@ C_INCLUDES = \ -Imodules/motor/HTmotor \ -Imodules/motor/step_motor \ -Imodules/motor/servo_motor \ +-Imodules/motor/DMmotor \ -Imodules/motor \ -Imodules/oled \ -Imodules/referee \ diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c new file mode 100644 index 0000000..5fa2054 --- /dev/null +++ b/modules/motor/DMmotor/dmmotor.c @@ -0,0 +1,173 @@ +#include "dmmotor.h" +#include "memory.h" +#include "general_def.h" +#include "user_lib.h" +#include "cmsis_os.h" +#include "string.h" +#include "daemon.h" +#include "stdlib.h" +#include "bsp_log.h" + +static uint8_t idx; +static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT]; +static osThreadId 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_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff + motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id + CANTransmit(motor->motor_can_instace, 1); +} + +static void DMMotorDecode(CANInstance *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; + 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)); + + 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->can_init_config.can_module_callback = DMMotorDecode; + config->can_init_config.id = motor; + motor->motor_can_instace = CANRegister(&config->can_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 DMMotorTask(void const *argument) +{ + float pid_measure, pid_ref, set; + DMMotorInstance *motor = (DMMotorInstance *)argument; + DM_Motor_Measure_s *measure = &motor->measure; + Motor_Control_Setting_s *setting = &motor->motor_settings; + CANInstance *motor_can = motor->motor_can_instace; + uint16_t tmp; + DMMotor_Send_s motor_send_mailbox; + while (1) + { + 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_instace->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8); + motor->motor_can_instace->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des); + motor->motor_can_instace->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4); + motor->motor_can_instace->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8)); + motor->motor_can_instace->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp); + motor->motor_can_instace->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4); + motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8)); + motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des); + + CANTransmit(motor->motor_can_instace, 1); + + osDelay(2); + } +} +void DMMotorControlInit() +{ + char dm_task_name[5] = "dm"; + // 遍历所有电机实例,创建任务 + if (!idx) + return; + for (size_t i = 0; i < idx; i++) + { + char dm_id_buff[2] = {0}; + __itoa(i, dm_id_buff, 10); + strcat(dm_task_name, dm_id_buff); + osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128); + dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]); + } +} \ No newline at end of file diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h new file mode 100644 index 0000000..f653998 --- /dev/null +++ b/modules/motor/DMmotor/dmmotor.h @@ -0,0 +1,76 @@ +#ifndef DMMOTOR_H +#define DMMOTOR_H +#include +#include "bsp_can.h" +#include "controller.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 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; + CANInstance *motor_can_instace; + DaemonInstance* motor_daemon; + uint32_t lost_cnt; +}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(); +#endif // !DMMOTOR \ No newline at end of file