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

增加了LK电机和HT电机的基本支持,待编写控制

Browse Source
This commit is contained in:
NeoZng
2022-12-13 19:40:03 +08:00
parent 2f41e67de0
commit 11329aaddb
21 changed files with 280 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
modules/motor/HT04.c
View File

@@ -1,15 +1,31 @@
#include "HT04.h"
#include "memory.h"
#include "general_def.h"
HKMotor_Measure_t *joint_motor_info[HT_MOTOR_CNT];
static uint8_t idx;
HTMotorInstance *ht_motor_info[HT_MOTOR_CNT];
/**
* @brief
*
* @param cmd
* @param motor
*/
static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7] = (uint8_t)cmd;
memcpy(motor->motor_can_instace->tx_buff, buf, sizeof(buf));
CANTransmit(motor->motor_can_instace);
}
/* 两个用于将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;
@@ -17,47 +33,84 @@ 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 DecodeJoint(CANInstance *motor_instance)
/**
* @brief 解析电机反馈值
*
* @param motor_can 收到
*/
static void HTMotorDecode(CANInstance *motor_can)
{
uint16_t tmp;
for (size_t i = 0; i < HT_MOTOR_CNT; i++)
static uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
static HTMotor_Measure_t *measure;
static uint8_t *rxbuff;
rxbuff = motor_can->rx_buff;
measure = &((HTMotorInstance *)motor_can->id)->motor_measure;
measure->last_angle = measure->total_angle;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->total_angle = RAD_2_ANGLE * uint_to_float(tmp, P_MAX, P_MIN, 16);
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
measure->speed_aps = RAD_2_ANGLE * SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MAX, V_MIN, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_aps;
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MAX, T_MIN, 12) +
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
}
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
{
ht_motor_info[idx] = (HTMotorInstance *)malloc(sizeof(HTMotorInstance));
memset(ht_motor_info[idx], 0, sizeof(HTMotorInstance));
ht_motor_info[idx]->motor_settings = config->controller_setting_init_config;
PID_Init(&ht_motor_info[idx]->current_PID, &config->controller_param_init_config.current_PID);
PID_Init(&ht_motor_info[idx]->speed_PID, &config->controller_param_init_config.speed_PID);
PID_Init(&ht_motor_info[idx]->angle_PID, &config->controller_param_init_config.angle_PID);
ht_motor_info[idx]->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
ht_motor_info[idx]->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
config->can_init_config.can_module_callback = HTMotorDecode;
config->can_init_config.id = ht_motor_info[idx];
ht_motor_info[idx]->motor_can_instace = CANRegister(&config->can_init_config);
HTMotorEnable(ht_motor_info[idx]);
return ht_motor_info[idx++];
}
void HTMotorSetRef(HTMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
}
void HTMotorControl()
{
static uint16_t tmp;
for (size_t i = 0; i < idx; i++)
{
if (joint_motor_info[i]->motor_can_instace == motor_instance)
{
tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2];
joint_motor_info[i]->last_ecd = joint_motor_info[i]->ecd;
joint_motor_info[i]->ecd = uint_to_float(tmp, P_MAX, P_MIN, 16);
tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4);
joint_motor_info[i]->speed_rpm = uint_to_float(tmp, V_MAX, V_MIN, 12);
tmp = ((motor_instance->rx_buff[4] & 0xf) << 8) | motor_instance->rx_buff[5];
joint_motor_info[i]->real_current = uint_to_float(tmp, T_MAX, T_MIN, 12);
break;
}
// tmp=float_to_uint()
// _instance->motor_can_instace->rx_buff[6] = tmp >> 8;
// _instance->motor_can_instace->rx_buff[7] = tmp & 0xff;
// CANTransmit(_instance->motor_can_instace);
}
}
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config)
void HTMotorStop(HTMotorInstance *motor)
{
static uint8_t idx;
joint_motor_info[idx] = (HKMotor_Measure_t *)malloc(sizeof(HKMotor_Measure_t));
joint_motor_info[idx]->motor_can_instace = CANRegister(&config);
return joint_motor_info[idx++];
HTMotorSetMode(CMD_RESET_MODE, motor);
}
void JointControl(HKMotor_Measure_t *_instance, float current)
void HTMotorEnable(HTMotorInstance *motor)
{
uint16_t tmp;
LIMIT_MIN_MAX(current, T_MIN, T_MAX);
tmp = float_to_uint(current, T_MIN, T_MAX, 12);
_instance->motor_can_instace->rx_buff[6] = tmp >> 8;
_instance->motor_can_instace->rx_buff[7] = tmp & 0xff;
CANTransmit(_instance->motor_can_instace);
HTMotorSetMode(CMD_MOTOR_MODE, motor);
}
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance)
void HTMotorCalibEncoder(HTMotorInstance *motor)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7] = (uint8_t)cmd;
memcpy(_instance->motor_can_instace->rx_buff, buf, 8 * sizeof(uint8_t));
CANTransmit(_instance->motor_can_instace);
HTMotorSetMode(CMD_ZERO_POSITION, motor);
}

86
modules/motor/HT04.h
View File

@@ -7,36 +7,94 @@
#include "motor_def.h"
#define HT_MOTOR_CNT 4
#define CURRENT_SMOOTH_COEF 0.9f
#define SPEED_SMOOTH_COEF 0.85f
#define P_MIN -95.5f // Radians
#define P_MAX 95.5f
#define V_MIN -45.0f // Rad/s
#define V_MAX 45.0f
#define T_MIN -18.0f
#define T_MIN -18.0f // N·m
#define T_MAX 18.0f
typedef struct // HT04
{
float last_ecd;
float ecd;
float speed_rpm;
{ // 角度为多圈角度,范围是-95.5~95.5,单位为rad
float last_angle;
float total_angle;
float speed_aps;
float real_current;
} HTMotor_Measure_t;
/* HT电机类型定义*/
typedef struct
{
HTMotor_Measure_t motor_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 pid_ref;
PIDInstance pid;
CANInstance *motor_can_instace;
} HKMotor_Measure_t;
} HTMotorInstance;
/* HT电机模式,初始化时自动进入CMD_MOTOR_MODE*/
typedef enum
{
CMD_MOTOR_MODE = 0xfc,
CMD_RESET_MODE = 0xfd,
CMD_ZERO_POSITION = 0xfe
} joint_mode;
CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
CMD_RESET_MODE = 0xfd, // 停止
CMD_ZERO_POSITION = 0xfe // 将当前的位置设置为编码器零位
} HTMotor_Mode_t;
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config);
/**
* @brief
*
* @param config
* @return HTMotorInstance*
*/
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config);
void JointControl(HKMotor_Measure_t *_instance, float current);
/**
* @brief 设定电机的参考值
*
* @param motor 要设定的电机
* @param current 设定值
*/
void HTMotorSetRef(HTMotorInstance *motor, float ref);
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance);
/**
* @brief 给所有的HT电机发送控制指令
*
*/
void HTMotorControl();
/**
* @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值
*
* @param motor
*/
void HTMotorStop(HTMotorInstance *motor);
/**
* @brief 启动电机
*
* @param motor 要启动的电机
*/
void HTMotorEnable(HTMotorInstance *motor);
/**
* @brief 校准电机编码器
* @attention 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
*
* @param motor
*/
void HTMotorCalibEncoder(HTMotorInstance *motor);
#endif // !HT04_H#define HT04_H

66
modules/motor/LK9025.c
View File

@@ -2,25 +2,25 @@
#include "stdlib.h"
static uint8_t idx;
static LKMotorInstance* lkmotor_instance[LK_MOTOR_MX_CNT]={NULL};
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
static void LKMotorDecode(CANInstance *_instance)
{
static LKMotor_Measure_t* measure;
static uint8_t* rx_buff;
rx_buff=_instance->rx_buff;
measure=&((LKMotorInstance*)_instance)->measure;
measure->last_ecd=measure->ecd;
measure->ecd=(uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
measure->angle_single_round=ECD_ANGLE_COEF*measure->ecd;
measure->speed_aps=(1-SPEED_SMOOTH_COEF)*measure->speed_aps+
SPEED_SMOOTH_COEF*(float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
measure->real_current=(1-CURRENT_SMOOTH_COEF)*measure->real_current+
CURRENT_SMOOTH_COEF*(float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
measure->temperate=rx_buff[1];
static LKMotor_Measure_t *measure;
static uint8_t *rx_buff;
rx_buff = _instance->rx_buff;
measure = &((LKMotorInstance *)_instance)->measure;
//计算多圈角度
measure->last_ecd = measure->ecd;
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
measure->speed_aps = (1 - SPEED_SMOOTH_COEF) * measure->speed_aps +
SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
measure->temperate = rx_buff[1];
// 计算多圈角度
if (measure->ecd - measure->last_ecd > 32678)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -32678)
@@ -28,48 +28,46 @@ static void LKMotorDecode(CANInstance *_instance)
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
void LKMotorControl()
{
for (size_t i = 0; i < idx; i++)
for (size_t i = 0; i < idx; ++i)
{
}
}
LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
{
lkmotor_instance[idx]=(LKMotorInstance*)malloc(sizeof(LKMotorInstance));
memset(lkmotor_instance[idx],0,sizeof(LKMotorInstance));
lkmotor_instance[idx] = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
memset(lkmotor_instance[idx], 0, sizeof(LKMotorInstance));
lkmotor_instance[idx]->motor_settings=config->controller_setting_init_config;
PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
lkmotor_instance[idx]->other_angle_feedback_ptr=config->controller_param_init_config.other_angle_feedback_ptr;
lkmotor_instance[idx]->other_speed_feedback_ptr=config->controller_param_init_config.other_speed_feedback_ptr;
lkmotor_instance[idx]->motor_settings = config->controller_setting_init_config;
PID_Init(&lkmotor_instance[idx]->current_PID, &config->controller_param_init_config.current_PID);
PID_Init(&lkmotor_instance[idx]->speed_PID, &config->controller_param_init_config.speed_PID);
PID_Init(&lkmotor_instance[idx]->angle_PID, &config->controller_param_init_config.angle_PID);
lkmotor_instance[idx]->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
lkmotor_instance[idx]->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
config->can_init_config.can_module_callback=LKMotorDecode;
config->can_init_config.rx_id=0x140+config->can_init_config.tx_id;
config->can_init_config.tx_id=config->can_init_config.tx_id+0x240;
lkmotor_instance[idx]->motor_can_ins=CANRegister(&config->can_init_config);
config->can_init_config.id = lkmotor_instance[idx];
config->can_init_config.can_module_callback = LKMotorDecode;
config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
config->can_init_config.tx_id = config->can_init_config.tx_id + 0x240;
lkmotor_instance[idx]->motor_can_ins = CANRegister(&config->can_init_config);
LKMotorEnable(lkmotor_instance[idx]);
return lkmotor_instance[idx++];
}
void LKMotorStop(LKMotorInstance *motor)
{
motor->stop_flag=MOTOR_STOP;
motor->stop_flag = MOTOR_STOP;
}
void LKMotorEnable(LKMotorInstance *motor)
{
motor->stop_flag=MOTOR_ENALBED;
motor->stop_flag = MOTOR_ENALBED;
}
void LKMotorSetRef(LKMotorInstance *motor, float ref)
{
motor->pid_ref=ref;
motor->pid_ref = ref;
}

2
modules/motor/LK9025.h
View File

@@ -13,7 +13,7 @@
#define CURRENT_SMOOTH_COEF 0.9f
#define SPEED_SMOOTH_COEF 0.85f
#define REDUCTION_RATIO_DRIVEN 1
#define ECD_ANGLE_COEF (360.0f/65536.0f)
#define ECD_ANGLE_COEF_LK (360.0f/65536.0f)
typedef struct // 9025
{

19
modules/motor/dji_motor.c
View File

@@ -76,7 +76,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
dji_motor_info[idx]->sender_group = motor_grouping;
// 检查是否发生id冲突
for (size_t i = 0; i < idx; i++)
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_info[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance->rx_id == config->rx_id)
IDcrash_Handler(i, idx);
@@ -100,7 +100,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
dji_motor_info[idx]->message_num = motor_send_num;
dji_motor_info[idx]->sender_group = motor_grouping;
for (size_t i = 0; i < idx; i++)
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_info[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance->rx_id == config->rx_id)
IDcrash_Handler(i, idx);
@@ -124,15 +124,14 @@ static void DecodeDJIMotor(CANInstance *_instance)
static uint8_t *rxbuff;
static DJI_Motor_Measure_s *measure;
rxbuff = _instance->rx_buff;
measure = &((DJIMotorInstance*)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销
measure = &((DJIMotorInstance *)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销
uint8_t nice;
// resolve data and apply filter to current and speed
measure->last_ecd = measure->ecd;
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
measure->angle_single_round = ECD_ANGLE_COEF * (float)measure->ecd;
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + RPM_2_ANGLE_PER_SEC *
SPEED_SMOOTH_COEF *(float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])) ;
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
measure->temperate = rxbuff[6];
@@ -167,7 +166,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
// register motor to CAN bus
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
config->can_init_config.id=dji_motor_info[idx]; //set id,eq to address(it is identity)
config->can_init_config.id = dji_motor_info[idx]; // set id,eq to address(it is identity)
dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config);
DJIMotorEnable(dji_motor_info[idx]);
@@ -220,7 +219,7 @@ void DJIMotorControl()
static DJI_Motor_Measure_s *motor_measure;
static float pid_measure, pid_ref;
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < idx; i++)
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_info[i])
{
@@ -279,7 +278,7 @@ void DJIMotorControl()
}
// 遍历flag,检查是否要发送这一帧报文
for (size_t i = 0; i < 6; i++)
for (size_t i = 0; i < 6; ++i)
{
if (sender_enable_flag[i])
{

2
modules/motor/dji_motor.h
View File

@@ -24,7 +24,7 @@
/* 滤波系数设置为1的时候即关闭滤波 */
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.85
#define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9
#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制
#define ECD_ANGLE_COEF_DJI (360.0f/8192.0f) // ,将编码器值转化为角度制
/* DJI电机CAN反馈信息*/
typedef struct

2
modules/motor/dji_motor.md
View File

@@ -387,7 +387,7 @@ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
```c
#define PI2 (3.141592f * 2)
#define ECD_ANGLE_COEF 3.835e-4 // ecd/8192*pi
#define ECD_ANGLE_COEF_DJI 3.835e-4 // ecd/8192*pi
```
这两个宏用于在电机反馈信息中的多圈角度计算，将编码器的0~8192转化为角度表示。

8
modules/motor/motor_def.h
View File

@@ -99,13 +99,15 @@ typedef enum
*/
typedef struct
{
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
float *other_angle_feedback_ptr; // 角度反馈数据指针,注意电机使用total_angle
float *other_speed_feedback_ptr; // 速度反馈数据指针,单位为angle per sec
float *speed_feedforward_ptr; // 速度前馈数据指针
float *current_feedforward_ptr; // 电流前馈数据指针
PID_Init_config_s current_PID;
PID_Init_config_s speed_PID;
PID_Init_config_s angle_PID;
} Motor_Controller_Init_s;
/* 用于初始化CAN电机的结构体,各类电机通用 */