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

完成了HTMotor和LKMotor模块,并为motor_def增加了前馈指令

Browse Source
This commit is contained in:
NeoZng
2022-12-14 13:33:19 +08:00
parent 67fbf9064d
commit 4637cb8297
16 changed files with 160 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
modules/motor/LK9025.c
View File

@@ -3,6 +3,8 @@
static uint8_t idx;
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
// 后续考虑兼容单电机和多电机指令.
static void LKMotorDecode(CANInstance *_instance)
{
@@ -13,11 +15,15 @@ static void LKMotorDecode(CANInstance *_instance)
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];
// 计算多圈角度
@@ -28,13 +34,6 @@ 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)
{
}
}
LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
{
lkmotor_instance[idx] = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
@@ -50,13 +49,77 @@ LKMotorInstance *LKMotroInit(Motor_Init_Config_s *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;
config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280;
lkmotor_instance[idx]->motor_can_ins = CANRegister(&config->can_init_config);
if (idx == 0)
sender_instance = lkmotor_instance[idx]->motor_can_ins;
LKMotorEnable(lkmotor_instance[idx]);
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;