mirror of
https://github.com/goldenfishs/MRobot.git
synced 2025-11-04 05:23:10 +08:00
497 lines
14 KiB
C
497 lines
14 KiB
C
#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \
|
||
((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1))))
|
||
|
||
#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \
|
||
(((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits))))
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "device/motor_dm.h"
|
||
#include "bsp/mm.h"
|
||
#include "bsp/time.h"
|
||
#include "component/user_math.h"
|
||
#include "string.h"
|
||
#include "math.h"
|
||
|
||
/* Private constants -------------------------------------------------------- */
|
||
/* DM电机数据映射范围 */
|
||
#define DM_P_MIN (-12.56637f)
|
||
#define DM_P_MAX (12.56637f)
|
||
#define DM_V_MIN (-30.0f)
|
||
#define DM_V_MAX (30.0f)
|
||
#define DM_T_MIN (-12.0f)
|
||
#define DM_T_MAX (12.0f)
|
||
#define DM_KP_MIN (0.0f)
|
||
#define DM_KP_MAX (500.0f)
|
||
#define DM_KD_MIN (0.0f)
|
||
#define DM_KD_MAX (5.0f)
|
||
|
||
/* CAN ID偏移量 */
|
||
#define DM_CAN_ID_OFFSET_POS_VEL 0x100
|
||
#define DM_CAN_ID_OFFSET_VEL 0x200
|
||
|
||
/* Private macro ------------------------------------------------------------ */
|
||
#define FLOAT_TO_UINT(x, x_min, x_max, bits) \
|
||
(uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min))
|
||
|
||
#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \
|
||
((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min)
|
||
|
||
|
||
/* Private variables -------------------------------------------------------- */
|
||
static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
|
||
|
||
static int float_to_uint(float x_float, float x_min, float x_max, int bits)
|
||
{
|
||
/* Converts a float to an unsigned int, given range and number of bits */
|
||
float span = x_max - x_min;
|
||
float offset = x_min;
|
||
return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
|
||
}
|
||
|
||
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
||
{
|
||
/* converts unsigned int to float, given range and number of bits */
|
||
float span = x_max - x_min;
|
||
float offset = x_min;
|
||
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
|
||
}
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data);
|
||
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output);
|
||
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel);
|
||
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel);
|
||
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can);
|
||
|
||
/* Private functions -------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 解析DM电机反馈帧
|
||
* @param motor 电机实例
|
||
* @param data CAN数据
|
||
* @return 解析结果
|
||
*/
|
||
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data) {
|
||
if (motor == NULL || data == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
uint16_t p_int=(data[1]<<8)|data[2];
|
||
motor->motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5)
|
||
uint16_t v_int=(data[3]<<4)|(data[4]>>4);
|
||
motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0)
|
||
uint16_t t_int=((data[4]&0xF)<<8)|data[5];
|
||
motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0)
|
||
motor->motor.feedback.temp = (float)(data[6]);
|
||
|
||
while (motor->motor.feedback.rotor_abs_angle < 0) {
|
||
motor->motor.feedback.rotor_abs_angle += M_2PI;
|
||
}
|
||
while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
|
||
motor->motor.feedback.rotor_abs_angle -= M_2PI;
|
||
}
|
||
|
||
if (motor->param.reverse) {
|
||
motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
|
||
motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
|
||
motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
|
||
}
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 发送MIT模式控制命令
|
||
* @param motor 电机实例
|
||
* @param output MIT控制参数
|
||
* @return 发送结果
|
||
*/
|
||
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) {
|
||
if (motor == NULL || output == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
uint8_t data[8];
|
||
uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
|
||
uint16_t id = motor->param.can_id;
|
||
|
||
pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
|
||
vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
|
||
kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12);
|
||
kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12);
|
||
tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
|
||
|
||
/* 打包数据 */
|
||
data[0] = (pos_tmp >> 8);
|
||
data[1] = pos_tmp;
|
||
data[2] = (vel_tmp >> 4);
|
||
data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
|
||
data[4] = kp_tmp;
|
||
data[5] = (kd_tmp >> 4);
|
||
data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
|
||
data[7] = tor_tmp;
|
||
|
||
/* 发送CAN消息 */
|
||
BSP_CAN_StdDataFrame_t frame;
|
||
frame.id = motor->param.can_id;
|
||
frame.dlc = 8;
|
||
memcpy(frame.data, data, 8);
|
||
|
||
|
||
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||
}
|
||
|
||
/**
|
||
* @brief 发送位置速度模式控制命令
|
||
* @param motor 电机实例
|
||
* @param pos 目标位置
|
||
* @param vel 目标速度
|
||
* @return 发送结果
|
||
*/
|
||
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
uint8_t data[8] = {0};
|
||
|
||
|
||
/* 直接发送浮点数数据 */
|
||
memcpy(&data[0], &pos, 4); // 位置，低位在前
|
||
memcpy(&data[4], &vel, 4); // 速度，低位在前
|
||
|
||
/* 发送CAN消息，ID为原ID+0x100 */
|
||
uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id;
|
||
BSP_CAN_StdDataFrame_t frame;
|
||
frame.id = can_id;
|
||
frame.dlc = 8;
|
||
memcpy(frame.data, data, 8);
|
||
|
||
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||
}
|
||
|
||
/**
|
||
* @brief 发送速度模式控制命令
|
||
* @param motor 电机实例
|
||
* @param vel 目标速度
|
||
* @return 发送结果
|
||
*/
|
||
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
uint8_t data[4] = {0};
|
||
|
||
/* 直接发送浮点数数据 */
|
||
memcpy(&data[0], &vel, 4); // 速度，低位在前
|
||
|
||
/* 发送CAN消息，ID为原ID+0x200 */
|
||
uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id;
|
||
BSP_CAN_StdDataFrame_t frame;
|
||
frame.id = can_id;
|
||
frame.dlc = 4;
|
||
memcpy(frame.data, data, 4);
|
||
|
||
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取指定CAN总线的管理器
|
||
* @param can CAN总线
|
||
* @return CAN管理器指针
|
||
*/
|
||
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) {
|
||
if (can >= BSP_CAN_NUM) {
|
||
return NULL;
|
||
}
|
||
|
||
return can_managers[can];
|
||
}
|
||
|
||
/**
|
||
* @brief 创建CAN管理器
|
||
* @param can CAN总线
|
||
* @return 创建结果
|
||
*/
|
||
static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) {
|
||
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
|
||
if (can_managers[can] != NULL) return DEVICE_OK;
|
||
|
||
can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t));
|
||
if (can_managers[can] == NULL) return DEVICE_ERR;
|
||
|
||
memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t));
|
||
can_managers[can]->can = can;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 注册一个DM电机
|
||
* @param param 电机参数
|
||
* @return 注册结果
|
||
*/
|
||
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) {
|
||
if (param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
/* 创建CAN管理器 */
|
||
if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
/* 获取CAN管理器 */
|
||
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
|
||
if (manager == NULL) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
/* 检查是否已注册 */
|
||
for (int i = 0; i < manager->motor_count; i++) {
|
||
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
|
||
return DEVICE_ERR_INITED;
|
||
}
|
||
}
|
||
|
||
/* 检查是否已达到最大数量 */
|
||
if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
/* 分配内存 */
|
||
MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t));
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
/* 初始化电机 */
|
||
memset(motor, 0, sizeof(MOTOR_DM_t));
|
||
memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t));
|
||
motor->motor.header.online = false;
|
||
motor->motor.reverse = param->reverse;
|
||
|
||
/* 注册CAN接收ID - DM电机使用Master ID接收反馈 */
|
||
uint16_t feedback_id = param->master_id;
|
||
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
|
||
BSP_Free(motor);
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
/* 添加到管理器 */
|
||
manager->motors[manager->motor_count] = motor;
|
||
manager->motor_count++;
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新指定电机数据
|
||
* @param param 电机参数
|
||
* @return 更新结果
|
||
*/
|
||
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) {
|
||
if (param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
|
||
if (manager == NULL) {
|
||
return DEVICE_ERR_NO_DEV;
|
||
}
|
||
|
||
/* 查找电机 */
|
||
MOTOR_DM_t *motor = NULL;
|
||
for (int i = 0; i < manager->motor_count; i++) {
|
||
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
|
||
motor = manager->motors[i];
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR_NO_DEV;
|
||
}
|
||
|
||
/* 主动接收CAN消息 */
|
||
uint16_t feedback_id = param->master_id;
|
||
BSP_CAN_Message_t rx_msg;
|
||
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
|
||
uint64_t now_time = BSP_TIME_Get();
|
||
if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒
|
||
motor->motor.header.online = false;
|
||
}
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
motor->motor.header.online = true;
|
||
motor->motor.header.last_online_time = BSP_TIME_Get();
|
||
MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data);
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新所有电机数据
|
||
* @return 更新结果
|
||
*/
|
||
int8_t MOTOR_DM_UpdateAll(void) {
|
||
int8_t ret = DEVICE_OK;
|
||
for (int can = 0; can < BSP_CAN_NUM; can++) {
|
||
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can);
|
||
if (manager == NULL) continue;
|
||
|
||
for (int i = 0; i < manager->motor_count; i++) {
|
||
MOTOR_DM_t *motor = manager->motors[i];
|
||
if (motor != NULL) {
|
||
if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) {
|
||
ret = DEVICE_ERR;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return ret;
|
||
}
|
||
|
||
/**
|
||
* @brief MIT模式控制
|
||
* @param param 电机参数
|
||
* @param output MIT控制参数
|
||
* @return 控制结果
|
||
*/
|
||
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) {
|
||
if (param == NULL || output == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR_NO_DEV;
|
||
}
|
||
|
||
return MOTOR_DM_SendMITCmd(motor, output);
|
||
}
|
||
|
||
/**
|
||
* @brief 位置速度模式控制
|
||
* @param param 电机参数
|
||
* @param target_pos 目标位置
|
||
* @param target_vel 目标速度
|
||
* @return 控制结果
|
||
*/
|
||
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) {
|
||
if (param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR_NO_DEV;
|
||
}
|
||
|
||
return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel);
|
||
}
|
||
|
||
/**
|
||
* @brief 速度模式控制
|
||
* @param param 电机参数
|
||
* @param target_vel 目标速度
|
||
* @return 控制结果
|
||
*/
|
||
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) {
|
||
if (param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR_NO_DEV;
|
||
}
|
||
|
||
return MOTOR_DM_SendVelCmd(motor, target_vel);
|
||
}
|
||
|
||
/**
|
||
* @brief 获取指定电机的实例指针
|
||
* @param param 电机参数
|
||
* @return 电机实例指针
|
||
*/
|
||
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) {
|
||
if (param == NULL) {
|
||
return NULL;
|
||
}
|
||
|
||
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
|
||
if (manager == NULL) {
|
||
return NULL;
|
||
}
|
||
|
||
/* 查找对应的电机 */
|
||
for (int i = 0; i < manager->motor_count; i++) {
|
||
MOTOR_DM_t *motor = manager->motors[i];
|
||
if (motor && motor->param.can == param->can &&
|
||
motor->param.master_id == param->master_id) {
|
||
return motor;
|
||
}
|
||
}
|
||
|
||
return NULL;
|
||
}
|
||
|
||
|
||
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){
|
||
if (param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR_NO_DEV;
|
||
}
|
||
|
||
BSP_CAN_StdDataFrame_t frame;
|
||
frame.id = motor->param.can_id;
|
||
frame.dlc = 8;
|
||
frame.data[0] = 0XFF;
|
||
frame.data[1] = 0xFF;
|
||
frame.data[2] = 0xFF;
|
||
frame.data[3] = 0xFF;
|
||
frame.data[4] = 0xFF;
|
||
frame.data[5] = 0xFF;
|
||
frame.data[6] = 0xFF;
|
||
frame.data[7] = 0xFC;
|
||
|
||
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||
}
|
||
|
||
/**
|
||
* @brief 使电机松弛（设置输出为0）
|
||
* @param param 电机参数
|
||
* @return 操作结果
|
||
*/
|
||
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) {
|
||
if (param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
MOTOR_MIT_Output_t output = {0};
|
||
return MOTOR_DM_MITCtrl(param, &output);
|
||
}
|
||
|
||
/**
|
||
* @brief 使电机离线（设置在线状态为false）
|
||
* @param param 电机参数
|
||
* @return 操作结果
|
||
*/
|
||
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) {
|
||
if (param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
|
||
if (motor == NULL) {
|
||
return DEVICE_ERR_NO_DEV;
|
||
}
|
||
|
||
motor->motor.header.online = false;
|
||
return DEVICE_OK;
|
||
}
|