TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +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"
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
// #define ONE_BOARD // 单板控制整车
#define CHASSIS_BOARD //底盘板
#define ONE_BOARD // 单板控制整车
// #define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据

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

@@ -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(&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)
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]);
}
}
}

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

@@ -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

2
modules/motor/motor_def.h
View File

@@ -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电机的结构体,各类电机通用 */