mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-25 11:57:46 +08:00
重新组织了文件的防止
This commit is contained in:
NeoZng
137
modules/motor/LKmotor/LK9025.c
Normal file
137
modules/motor/LKmotor/LK9025.c
Normal file
@@ -0,0 +1,137 @@
|
||||
#include "LK9025.h"
|
||||
#include "stdlib.h"
|
||||
|
||||
static uint8_t idx;
|
||||
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
|
||||
static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
|
||||
// 后续考虑兼容单电机和多电机指令.
|
||||
|
||||
static void LKMotorDecode(CANInstance *_instance)
|
||||
{
|
||||
static LKMotor_Measure_t *measure;
|
||||
static uint8_t *rx_buff;
|
||||
rx_buff = _instance->rx_buff;
|
||||
measure = &((LKMotorInstance *)_instance)->measure; // 通过caninstance保存的id获取对应的motorinstance
|
||||
|
||||
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)
|
||||
measure->total_round++;
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
|
||||
motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
|
||||
memset(motor, 0, sizeof(LKMotorInstance));
|
||||
|
||||
motor->motor_settings = config->controller_setting_init_config;
|
||||
PID_Init(&motor->current_PID, &config->controller_param_init_config.current_PID);
|
||||
PID_Init(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PID_Init(&motor->angle_PID, &config->controller_param_init_config.angle_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;
|
||||
|
||||
config->can_init_config.id = motor;
|
||||
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 + 0x280;
|
||||
motor->motor_can_ins = CANRegister(&config->can_init_config);
|
||||
|
||||
if (idx == 0)
|
||||
sender_instance = motor->motor_can_ins;
|
||||
|
||||
LKMotorEnable(motor);
|
||||
return lkmotor_instance[idx++];
|
||||
}
|
||||
|
||||
/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */
|
||||
void LKMotorControl()
|
||||
{
|
||||
static float pid_measure, pid_ref;
|
||||
static int16_t set;
|
||||
static LKMotorInstance *motor;
|
||||
static LKMotor_Measure_t *measure;
|
||||
static Motor_Control_Setting_s *setting;
|
||||
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{
|
||||
motor = lkmotor_instance[i];
|
||||
measure = &motor->measure;
|
||||
setting = &motor->motor_settings;
|
||||
pid_ref = motor->pid_ref;
|
||||
|
||||
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->real_current;
|
||||
pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor->speed_feedforward_ptr;
|
||||
}
|
||||
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_speed_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->speed_aps;
|
||||
pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor->current_feedforward_ptr;
|
||||
}
|
||||
|
||||
if (setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PID_Calculate(&motor->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
set = pid_ref;
|
||||
if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
set *= -1;
|
||||
// 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
|
||||
memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280 - 1) * 2, &set, sizeof(uint16_t));
|
||||
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
{ // 若该电机处于停止状态,直接将发送buff置零
|
||||
memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280 - 1) * 2, 0, sizeof(uint16_t));
|
||||
}
|
||||
}
|
||||
|
||||
if (idx) // 如果有电机注册了
|
||||
{
|
||||
CANTransmit(sender_instance);
|
||||
}
|
||||
}
|
||||
|
||||
void LKMotorStop(LKMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void LKMotorEnable(LKMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
void LKMotorSetRef(LKMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->pid_ref = ref;
|
||||
}
|
||||
67
modules/motor/LKmotor/LK9025.h
Normal file
67
modules/motor/LKmotor/LK9025.h
Normal file
@@ -0,0 +1,67 @@
|
||||
#ifndef LK9025_H
|
||||
#define LK9025_H
|
||||
|
||||
#include "stdint.h"
|
||||
#include "bsp_can.h"
|
||||
#include "controller.h"
|
||||
#include "motor_def.h"
|
||||
|
||||
#define LK_MOTOR_MX_CNT 4 // 最多允许4个LK电机使用多电机指令,挂载在一条总线上
|
||||
|
||||
#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_LK (360.0f/65536.0f)
|
||||
|
||||
typedef struct // 9025
|
||||
{
|
||||
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°
|
||||
|
||||
float total_angle; // 总角度
|
||||
int32_t total_round; //总圈数
|
||||
|
||||
} LKMotor_Measure_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
LKMotor_Measure_t measure;
|
||||
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
|
||||
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
|
||||
float *other_speed_feedback_ptr;
|
||||
float *speed_feedforward_ptr;
|
||||
float *current_feedforward_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);
|
||||
|
||||
|
||||
#endif // LK9025_H
|
||||
Reference in New Issue
Block a user