TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 19:47:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

changed damiao controller

Browse Source
This commit is contained in:
TuxMonkey
2026-04-20 15:28:26 +08:00
parent ccfb03f3b7
commit 70ff2815f3
4 changed files with 115 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
application/robot_def.h
View File

@@ -17,8 +17,8 @@
#include "stdint.h" #include "stdint.h"
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
// #define ONE_BOARD // 单板控制整车 #define ONE_BOARD // 单板控制整车
#define CHASSIS_BOARD //底盘板 // #define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板 // #define GIMBAL_BOARD //云台板
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据

122
modules/motor/DMmotor/dmmotor.c
View File

@@ -1,5 +1,4 @@
#include "dmmotor.h" #include "dmmotor.h"
#include "memory.h"
#include "general_def.h" #include "general_def.h"
#include "user_lib.h" #include "user_lib.h"
#include "cmsis_os.h" #include "cmsis_os.h"
@@ -8,16 +7,19 @@
#include "stdlib.h" #include "stdlib.h"
#include "bsp_log.h" #include "bsp_log.h"
#define DM_SEND_DELAY 0.2
static uint8_t idx; static uint8_t idx;
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT]; static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
static osThreadId dm_task_handle[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) static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{ {
float span = x_max - x_min; float span = x_max - x_min;
float offset = x_min; float offset = x_min;
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span); 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) static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{ {
float span = x_max - x_min; 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; 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 memset(motor->motor_can_instace->tx_buff, 0xff, 7);
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd;
CANTransmit(motor->motor_can_instace, 1); CANTransmit(motor->motor_can_instace, 1);
DWT_Delay(DM_SEND_DELAY);
} }
static void DMMotorDecode(CANInstance *motor_can) static void DMMotorDecode(CANInstance *motor_can)
{ {
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 uint16_t tmp;
uint8_t *rxbuff = motor_can->rx_buff; uint8_t *rxbuff = motor_can->rx_buff;
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id; 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); DaemonReload(motor->motor_daemon);
measure->id = rxbuff[0];
measure->state = (rxbuff[0] >> 4) & 0xf;
measure->last_position = measure->position; measure->last_position = measure->position;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16); 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); tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12); measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]); tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12); measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
measure->T_Mos = (float)rxbuff[6]; measure->T_Mos = (float)rxbuff[6];
measure->T_Rotor = (float)rxbuff[7]; measure->T_Rotor = (float)rxbuff[7];
} }
static void DMMotorLostCallback(void *motor_ptr) static void DMMotorLostCallback(void *motor_ptr)
{ {
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
DMMotorEnable(motor);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
} }
void DMMotorCaliEncoder(DMMotorInstance *motor) void DMMotorCaliEncoder(DMMotorInstance *motor)
{ {
DMMotorSetMode(DM_CMD_ZERO_POSITION, motor); DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
DWT_Delay(0.1);
} }
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
{ {
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance)); DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
memset(motor, 0, sizeof(DMMotorInstance)); memset(motor, 0, sizeof(DMMotorInstance));
motor->motor_type = config->motor_type;
motor->motor_settings = config->controller_setting_init_config; motor->motor_settings = config->controller_setting_init_config;
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID); PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_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->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_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; 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, .owner_id = motor,
.reload_count = 10, .reload_count = 10,
}; };
motor->motor_daemon = DaemonRegister(&conf);
DMMotorEnable(motor); motor->motor_daemon = DaemonRegister(&conf);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
DWT_Delay(0.1);
DMMotorCaliEncoder(motor);
DWT_Delay(0.1);
dm_motor_instance[idx++] = motor; dm_motor_instance[idx++] = motor;
return motor; return motor;
} }
@@ -105,7 +113,7 @@ void DMMotorEnable(DMMotorInstance *motor)
motor->stop_flag = MOTOR_ENALBED; motor->stop_flag = MOTOR_ENALBED;
} }
void DMMotorStop(DMMotorInstance *motor)//不使用使能模式是因为需要收到反馈 void DMMotorStop(DMMotorInstance *motor)
{ {
motor->stop_flag = MOTOR_STOP; motor->stop_flag = MOTOR_STOP;
} }
@@ -115,33 +123,60 @@ void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
motor->motor_settings.outer_loop_type = type; motor->motor_settings.outer_loop_type = type;
} }
//@Todo: 目前只实现了力控，更多位控PID等请自行添加
void DMMotorTask(void const *argument) void DMMotorTask(void const *argument)
{ {
float pid_ref, set; float pid_ref, set;
DMMotorInstance *motor = (DMMotorInstance *)argument; DMMotorInstance *motor = (DMMotorInstance *)argument;
//DM_Motor_Measure_s *measure = &motor->measure; DM_Motor_Measure_s *measure;
Motor_Control_Setting_s *setting = &motor->motor_settings; Motor_Control_Setting_s *setting;
//CANInstance *motor_can = motor->motor_can_instace;
//uint16_t tmp;
DMMotor_Send_s motor_send_mailbox; DMMotor_Send_s motor_send_mailbox;
portTickType currentTime;
const portTickType xFrequency = pdMS_TO_TICKS(4);
while (1) while (1)
{ {
pid_ref = motor->pid_ref; currentTime = xTaskGetTickCount();
set = motor->pid_ref;
set = pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1; 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.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.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.torque_des = float_to_uint(pid_ref, DM_T_MIN, DM_T_MAX, 12);
motor_send_mailbox.Kp = 0; motor_send_mailbox.Kp = 0;
motor_send_mailbox.Kd = 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_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[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
@@ -153,20 +188,35 @@ 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[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); 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(&currentTime, 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) if (!idx)
return; return;
for (size_t i = 0; i < idx; i++) 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); __itoa(i, dm_id_buff, 10);
strcat(dm_task_name, dm_id_buff); strcat(dm_task_name, dm_id_buff);
osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128); osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);

33
modules/motor/DMmotor/dmmotor.h
View File

@@ -6,7 +6,7 @@
#include "motor_def.h" #include "motor_def.h"
#include "daemon.h" #include "daemon.h"
#define DM_MOTOR_CNT 4 #define DM_MOTOR_CNT 5
#define DM_P_MIN (-12.5f) #define DM_P_MIN (-12.5f)
#define DM_P_MAX 12.5f #define DM_P_MAX 12.5f
@@ -15,6 +15,8 @@
#define DM_T_MIN (-18.0f) #define DM_T_MIN (-18.0f)
#define DM_T_MAX 18.0f #define DM_T_MAX 18.0f
#define DM_REDUCE 9
typedef struct typedef struct
{ {
uint8_t id; uint8_t id;
@@ -22,11 +24,13 @@ typedef struct
float velocity; float velocity;
float last_position; float last_position;
float position; float position;
float angle_single_round;
float total_angle;
float torque; float torque;
float T_Mos; float T_Mos;
float T_Rotor; float T_Rotor;
int32_t total_round; int32_t total_round;
}DM_Motor_Measure_s; } DM_Motor_Measure_s;
typedef struct typedef struct
{ {
@@ -35,12 +39,14 @@ typedef struct
uint16_t torque_des; uint16_t torque_des;
uint16_t Kp; uint16_t Kp;
uint16_t Kd; uint16_t Kd;
}DMMotor_Send_s; } DMMotor_Send_s;
typedef struct typedef struct
{ {
Motor_Type_e motor_type;
DM_Motor_Measure_s measure; DM_Motor_Measure_s measure;
Motor_Control_Setting_s motor_settings; Motor_Control_Setting_s motor_settings;
PIDInstance current_PID; PIDInstance current_PID;
PIDInstance torque_PID;
PIDInstance speed_PID; PIDInstance speed_PID;
PIDInstance angle_PID; PIDInstance angle_PID;
float *other_angle_feedback_ptr; float *other_angle_feedback_ptr;
@@ -50,27 +56,26 @@ typedef struct
float pid_ref; float pid_ref;
Motor_Working_Type_e stop_flag; Motor_Working_Type_e stop_flag;
CANInstance *motor_can_instace; CANInstance *motor_can_instace;
DaemonInstance* motor_daemon; DaemonInstance *motor_daemon;
uint32_t lost_cnt; uint32_t lost_cnt;
}DMMotorInstance; } DMMotorInstance;
typedef enum typedef enum
{ {
DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令 DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
DM_CMD_RESET_MODE = 0xfd, // 停止 DM_CMD_RESET_MODE = 0xfd, // 停止
DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位 DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误 DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
}DMMotor_Mode_e; } DMMotor_Mode_e;
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config); DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
void DMMotorSetRef(DMMotorInstance *motor, float ref); 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 DMMotorEnable(DMMotorInstance *motor);
void DMMotorStop(DMMotorInstance *motor); void DMMotorStop(DMMotorInstance *motor);
void DMMotorCaliEncoder(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 #endif // !DMMOTOR

2
modules/motor/motor_def.h
View File

@@ -27,6 +27,7 @@ typedef enum
CURRENT_LOOP = 0b0001, CURRENT_LOOP = 0b0001,
SPEED_LOOP = 0b0010, SPEED_LOOP = 0b0010,
ANGLE_LOOP = 0b0100, ANGLE_LOOP = 0b0100,
TORQUE_LOOP = 0b1000,
// only for checking // only for checking
SPEED_AND_CURRENT_LOOP = 0b0011, SPEED_AND_CURRENT_LOOP = 0b0011,
@@ -125,6 +126,7 @@ typedef struct
PID_Init_Config_s current_PID; PID_Init_Config_s current_PID;
PID_Init_Config_s speed_PID; PID_Init_Config_s speed_PID;
PID_Init_Config_s angle_PID; PID_Init_Config_s angle_PID;
PID_Init_Config_s torque_PID;
} Motor_Controller_Init_s; } Motor_Controller_Init_s;
/* 用于初始化CAN电机的结构体,各类电机通用 */ /* 用于初始化CAN电机的结构体,各类电机通用 */