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

完成了LKmotor模块的重构，优化了DJIMotor的反馈计算

Browse Source
This commit is contained in:
NeoZng
2022-12-11 20:48:24 +08:00
parent 78cc27ee1a
commit 2f41e67de0
15 changed files with 274 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
modules/motor/HT04.c
View File

@@ -1,7 +1,7 @@
#include "HT04.h"
#include "memory.h"
joint_instance *joint_motor_info[HT_MOTOR_CNT];
HKMotor_Measure_t *joint_motor_info[HT_MOTOR_CNT];
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
@@ -36,15 +36,15 @@ static void DecodeJoint(CANInstance *motor_instance)
}
}
joint_instance *HTMotorInit(CAN_Init_Config_s config)
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config)
{
static uint8_t idx;
joint_motor_info[idx] = (joint_instance *)malloc(sizeof(joint_instance));
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++];
}
void JointControl(joint_instance *_instance, float current)
void JointControl(HKMotor_Measure_t *_instance, float current)
{
uint16_t tmp;
LIMIT_MIN_MAX(current, T_MIN, T_MAX);
@@ -54,7 +54,7 @@ void JointControl(joint_instance *_instance, float current)
CANTransmit(_instance->motor_can_instace);
}
void SetJointMode(joint_mode cmd, joint_instance *_instance)
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7] = (uint8_t)cmd;

8
modules/motor/HT04.h
View File

@@ -24,7 +24,7 @@ typedef struct // HT04
PIDInstance pid;
CANInstance *motor_can_instace;
} joint_instance;
} HKMotor_Measure_t;
typedef enum
{
@@ -33,10 +33,10 @@ typedef enum
CMD_ZERO_POSITION = 0xfe
} joint_mode;
joint_instance *HTMotorInit(CAN_Init_Config_s config);
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config);
void JointControl(joint_instance *_instance, float current);
void JointControl(HKMotor_Measure_t *_instance, float current);
void SetJointMode(joint_mode cmd, joint_instance *_instance);
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance);
#endif // !HT04_H#define HT04_H

90
modules/motor/LK9025.c
View File

@@ -1,47 +1,75 @@
#include "LK9025.h"
#include "stdlib.h"
static driven_instance *driven_motor_info[LK_MOTOR_CNT];
static uint8_t idx;
static LKMotorInstance* lkmotor_instance[LK_MOTOR_MX_CNT]={NULL};
static void DecodeDriven(CANInstance *_instance)
static void LKMotorDecode(CANInstance *_instance)
{
for (size_t i = 0; i < LK_MOTOR_CNT; i++)
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];
//计算多圈角度
if (measure->ecd - measure->last_ecd > 32678)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -32678)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
void LKMotorControl()
{
for (size_t i = 0; i < idx; i++)
{
if (driven_motor_info[i]->motor_can_instance == _instance)
{
driven_motor_info[i]->last_ecd = driven_motor_info[i]->ecd;
driven_motor_info[i]->ecd = (uint16_t)((_instance->rx_buff[7] << 8) | _instance->rx_buff[6]);
driven_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);
driven_motor_info[i]->real_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);
driven_motor_info[i]->temperate = _instance->rx_buff[1];
break;
}
}
}
driven_instance *LKMotroInit(CAN_Init_Config_s config)
LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
{
static uint8_t idx;
driven_motor_info[idx] = (driven_instance *)malloc(sizeof(driven_instance));
config.can_module_callback = DecodeDriven;
driven_motor_info[idx]->motor_can_instance = CANRegister(&config);
return driven_motor_info[idx++];
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;
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 DrivenControl(int16_t motor1_current, int16_t motor2_current)
void LKMotorStop(LKMotorInstance *motor)
{
LIMIT_MIN_MAX(motor1_current, I_MIN, I_MAX);
LIMIT_MIN_MAX(motor2_current, I_MIN, I_MAX);
driven_motor_info[0]->motor_can_instance->tx_buff[0] = motor1_current;
driven_motor_info[0]->motor_can_instance->tx_buff[1] = motor1_current >> 8;
driven_motor_info[0]->motor_can_instance->tx_buff[2] = motor2_current;
driven_motor_info[0]->motor_can_instance->tx_buff[3] = motor2_current >> 8;
CANTransmit(driven_motor_info[0]->motor_can_instance);
motor->stop_flag=MOTOR_STOP;
}
void SetDrivenMode(driven_mode cmd, uint16_t motor_id)
void LKMotorEnable(LKMotorInstance *motor)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
// code goes here ...
// CANTransmit(driven_mode)
motor->stop_flag=MOTOR_ENALBED;
}
void LKMotorSetRef(LKMotorInstance *motor, float ref)
{
motor->pid_ref=ref;
}

58
modules/motor/LK9025.h
View File

@@ -6,32 +6,60 @@
#include "controller.h"
#include "motor_def.h"
#define LK_MOTOR_CNT 2
#define LK_MOTOR_MX_CNT 4
#define I_MIN -2000
#define I_MAX 2000
#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)
typedef struct // 9025
{
uint16_t last_ecd;
uint16_t ecd;
int16_t speed_rpm;
int16_t real_current;
uint8_t temperate;
uint16_t last_ecd;// 上一次读取的编码器值
uint16_t ecd; //
float angle_single_round; // 单圈角度
float speed_aps; // speed angle per sec(degree:°)
int16_t real_current; // 实际电流
uint8_t temperate; //温度,C°
PIDInstance *pid;
CANInstance *motor_can_instance;
float total_angle; // 总角度
int32_t total_round; //总圈数
} driven_instance;
} LKMotor_Measure_t;
typedef enum
typedef struct
{
unused = 0,
} driven_mode;
LKMotor_Measure_t measure;
driven_instance *LKMotroInit(CAN_Init_Config_s config);
Motor_Control_Setting_s motor_settings;
void DrivenControl(int16_t motor1_current, int16_t motor2_current);
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float pid_ref;
Motor_Working_Type_e stop_flag; // 启停标志
CANInstance* motor_can_ins;
}LKMotorInstance;
LKMotorInstance *LKMotroInit(Motor_Init_Config_s* config);
void LKMotorSetRef(LKMotorInstance* motor,float ref);
void LKMotorControl();
void LKMotorStop(LKMotorInstance *motor);
void LKMotorEnable(LKMotorInstance *motor);
void LKMotorSetRef(LKMotorInstance *motor,float ref);
void SetDrivenMode(driven_mode cmd, uint16_t motor_id);
#endif // LK9025_H

44
modules/motor/dji_motor.c
View File

@@ -123,33 +123,26 @@ static void DecodeDJIMotor(CANInstance *_instance)
// 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取
static uint8_t *rxbuff;
static DJI_Motor_Measure_s *measure;
rxbuff = _instance->rx_buff;
measure = &((DJIMotorInstance*)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i]->motor_can_instance == _instance)
{
rxbuff = _instance->rx_buff;
measure = &dji_motor_info[i]->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->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];
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->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];
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
break;
}
}
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
// 电机初始化,返回一个电机实例
@@ -174,6 +167,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)
dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config);
DJIMotorEnable(dji_motor_info[idx]);

4
modules/motor/dji_motor.h
View File

@@ -22,8 +22,8 @@
#define DJI_MOTOR_CNT 12
/* 滤波系数设置为1的时候即关闭滤波 */
#define SPEED_SMOOTH_COEF 0.9f // better to be greater than 0.85
#define CURRENT_SMOOTH_COEF 0.98f // this coef *must* be greater than 0.95
#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,将编码器值转化为角度制
/* DJI电机CAN反馈信息*/