From 70ff2815f347b0254ab969006f5191624081e65d Mon Sep 17 00:00:00 2001 From: TuxMonkey Date: Mon, 20 Apr 2026 15:28:26 +0800 Subject: [PATCH] changed damiao controller --- application/robot_def.h | 4 +- modules/motor/DMmotor/dmmotor.c | 130 ++++++++++++++++++++++---------- modules/motor/DMmotor/dmmotor.h | 37 +++++---- modules/motor/motor_def.h | 2 + 4 files changed, 115 insertions(+), 58 deletions(-) diff --git a/application/robot_def.h b/application/robot_def.h index 776457a..171c62a 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -17,8 +17,8 @@ #include "stdint.h" /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ -// #define ONE_BOARD // 单板控制整车 -#define CHASSIS_BOARD //底盘板 +#define ONE_BOARD // 单板控制整车 +// #define CHASSIS_BOARD //底盘板 // #define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index fad8221..0873988 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -1,5 +1,4 @@ #include "dmmotor.h" -#include "memory.h" #include "general_def.h" #include "user_lib.h" #include "cmsis_os.h" @@ -8,16 +7,19 @@ #include "stdlib.h" #include "bsp_log.h" +#define DM_SEND_DELAY 0.2 + 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; @@ -25,53 +27,64 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits) return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset; } -static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) +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 + memset(motor->motor_can_instace->tx_buff, 0xff, 7); + motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; CANTransmit(motor->motor_can_instace, 1); + DWT_Delay(DM_SEND_DELAY); } static void DMMotorDecode(CANInstance *motor_can) { - uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 + uint16_t tmp; uint8_t *rxbuff = motor_can->rx_buff; DMMotorInstance *motor = (DMMotorInstance *)motor_can->id; - DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针 + DM_Motor_Measure_s *measure = &(motor->measure); DaemonReload(motor->motor_daemon); - + measure->id = rxbuff[0]; + measure->state = (rxbuff[0] >> 4) & 0xf; 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); - + measure->angle_single_round = measure->position / (2 * PI) * 360.0f; + if (measure->position - measure->last_position > PI) + measure->total_round--; + else if (measure->position - measure->last_position < -PI) + measure->total_round++; + measure->total_angle = measure->total_round * 360.0f + measure->position / (2 * PI) * 360.0f; 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) { + DMMotorInstance *motor = (DMMotorInstance *)motor_ptr; + DMMotorEnable(motor); + DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); } + 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_type = config->motor_type; 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); + PIDInit(&motor->torque_PID, &config->controller_param_init_config.torque_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; @@ -84,13 +97,8 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) .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); + motor->motor_daemon = DaemonRegister(&conf); dm_motor_instance[idx++] = motor; return motor; } @@ -105,7 +113,7 @@ void DMMotorEnable(DMMotorInstance *motor) motor->stop_flag = MOTOR_ENALBED; } -void DMMotorStop(DMMotorInstance *motor)//不使用使能模式是因为需要收到反馈 +void DMMotorStop(DMMotorInstance *motor) { motor->stop_flag = MOTOR_STOP; } @@ -115,33 +123,60 @@ void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type) motor->motor_settings.outer_loop_type = type; } - -//@Todo: 目前只实现了力控，更多位控PID等请自行添加 void DMMotorTask(void const *argument) { - float pid_ref, set; + float 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; + DM_Motor_Measure_s *measure; + Motor_Control_Setting_s *setting; DMMotor_Send_s motor_send_mailbox; + portTickType currentTime; + const portTickType xFrequency = pdMS_TO_TICKS(4); while (1) { - pid_ref = motor->pid_ref; - - set = pid_ref; + currentTime = xTaskGetTickCount(); + set = motor->pid_ref; + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; - - LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX); + + measure = &motor->measure; + setting = &motor->motor_settings; + if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP)) + { + if (setting->angle_feedback_source == OTHER_FEED) + { + set = PIDCalculate(&motor->angle_PID, *motor->other_angle_feedback_ptr, set); + } + else if (setting->angle_feedback_source == MOTOR_FEED) + { + set = PIDCalculate(&motor->angle_PID, measure->position, set);; + } + } + if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (SPEED_LOOP | ANGLE_LOOP))) + { + if (setting->speed_feedback_source == OTHER_FEED) + { + set = PIDCalculate(&motor->speed_PID, *motor->other_speed_feedback_ptr, set); + } + else if (setting->speed_feedback_source == MOTOR_FEED) + { + set = PIDCalculate(&motor->speed_PID, measure->velocity, set); + } + } + if ((setting->close_loop_type & TORQUE_LOOP) && (setting->outer_loop_type & (TORQUE_LOOP | SPEED_LOOP | ANGLE_LOOP))) + { + set = PIDCalculate(&motor->torque_PID, 0, set); + } + pid_ref = set; + LIMIT_MIN_MAX(pid_ref, 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) + 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); @@ -153,23 +188,38 @@ void DMMotorTask(void const *argument) 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); + CANTransmit(motor->motor_can_instace, 1); - osDelay(2); + osDelayUntil(¤tTime, 4); // 4ms 的绝对延时 } } -void DMMotorControlInit() + +// /** +// * @brief Keil的runtime中的stdlib.h没有提供__itoa()，此处为该函数的手动实现 +// * @param n 源数据 +// * @param s 目标字符串 +// * @param b 目标进制 有效范围2-36 +// * @attention 没加特判，注意整数溢出/字符串数组越界/进制范围限制 +// **/ +// void __itoa(int n,char *s,int b) +// { +// char *p = s; int sg = n < 0; n = sg ? -n : n; +// do { *p++ = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[n % b];} while(n /= b); +// sg ? (*p++ = '-') : 0; +// *p = '\0'; +// for(char *h = s,*t = p-1; h < t; h++, t--)(*h ^= *t,*t ^= *h,*h ^= *t); +// } + +void DMMotorControlInit(void) { - char dm_task_name[5] = "dm"; - // 遍历所有电机实例,创建任务 if (!idx) return; for (size_t i = 0; i < idx; i++) { - char dm_id_buff[2] = {0}; + char dm_task_name[5] = "dm", 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 index 8f03e28..d4e77a9 100644 --- a/modules/motor/DMmotor/dmmotor.h +++ b/modules/motor/DMmotor/dmmotor.h @@ -6,7 +6,7 @@ #include "motor_def.h" #include "daemon.h" -#define DM_MOTOR_CNT 4 +#define DM_MOTOR_CNT 5 #define DM_P_MIN (-12.5f) #define DM_P_MAX 12.5f @@ -15,18 +15,22 @@ #define DM_T_MIN (-18.0f) #define DM_T_MAX 18.0f -typedef struct +#define DM_REDUCE 9 + +typedef struct { uint8_t id; uint8_t state; float velocity; float last_position; float position; + float angle_single_round; + float total_angle; float torque; float T_Mos; float T_Rotor; int32_t total_round; -}DM_Motor_Measure_s; +} DM_Motor_Measure_s; typedef struct { @@ -35,12 +39,14 @@ typedef struct uint16_t torque_des; uint16_t Kp; uint16_t Kd; -}DMMotor_Send_s; -typedef struct +} DMMotor_Send_s; +typedef struct { + Motor_Type_e motor_type; DM_Motor_Measure_s measure; Motor_Control_Setting_s motor_settings; PIDInstance current_PID; + PIDInstance torque_PID; PIDInstance speed_PID; PIDInstance angle_PID; float *other_angle_feedback_ptr; @@ -50,27 +56,26 @@ typedef struct float pid_ref; Motor_Working_Type_e stop_flag; CANInstance *motor_can_instace; - DaemonInstance* motor_daemon; + DaemonInstance *motor_daemon; uint32_t lost_cnt; -}DMMotorInstance; +} DMMotorInstance; typedef enum { - DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令 - DM_CMD_RESET_MODE = 0xfd, // 停止 + DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令 + DM_CMD_RESET_MODE = 0xfd, // 停止 DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位 - DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误 -}DMMotor_Mode_e; + 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 DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e closeloop_type); void DMMotorEnable(DMMotorInstance *motor); - void DMMotorStop(DMMotorInstance *motor); void DMMotorCaliEncoder(DMMotorInstance *motor); -void DMMotorControlInit(); +DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config); +void DMMotorControlInit(void); +void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor); #endif // !DMMOTOR \ No newline at end of file diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 8bd3cd2..eb60c69 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -27,6 +27,7 @@ typedef enum CURRENT_LOOP = 0b0001, SPEED_LOOP = 0b0010, ANGLE_LOOP = 0b0100, + TORQUE_LOOP = 0b1000, // only for checking SPEED_AND_CURRENT_LOOP = 0b0011, @@ -125,6 +126,7 @@ typedef struct PID_Init_Config_s current_PID; PID_Init_Config_s speed_PID; PID_Init_Config_s angle_PID; + PID_Init_Config_s torque_PID; } Motor_Controller_Init_s; /* 用于初始化CAN电机的结构体,各类电机通用 */