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

能动了！（小板凳）但是还是很抖

Browse Source
This commit is contained in:
TuxMonkey
2026-07-15 13:40:54 +08:00
parent 8a13444e48
commit b3da53745d
5 changed files with 74 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
modules/algorithm/QuaternionEKF.c
View File

@@ -199,7 +199,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
// 利用四元数反解欧拉角
QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
QEKF_INS.Pitch = -asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
// get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)

85
modules/motor/LKmotor/LK9025.c
View File

@@ -1,5 +1,6 @@
#include "LK9025.h"
#include "stdlib.h"
#include "string.h"
#include "general_def.h"
#include "daemon.h"
#include "bsp_dwt.h"
@@ -7,33 +8,30 @@
static uint8_t idx;
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
// 后续考虑兼容单电机和多电机指令.
/**
* @brief 电机反馈报文解析
*
* @param _instance 发生中断的caninstance
*/
static uint32_t LKMotorSingleCanID(uint32_t id)
{
return id < 0x140 ? 0x140 + id : id;
}
static void LKMotorDecode(CANInstance *_instance)
{
LKMotorInstance *motor = (LKMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
LKMotorInstance *motor = (LKMotorInstance *)_instance->id;
LKMotor_Measure_t *measure = &motor->measure;
uint8_t *rx_buff = _instance->rx_buff;
DaemonReload(motor->daemon); // 喂狗
DaemonReload(motor->daemon);
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
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_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads +
DEGREE_2_RAD * 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->real_current = (int16_t)((1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2])));
measure->temperature = rx_buff[1];
@@ -41,7 +39,7 @@ static void LKMotorDecode(CANInstance *_instance)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -32768)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
measure->total_angle = measure->total_round * 360.0f + measure->angle_single_round;
}
static void LKMotorLostCallback(void *motor_ptr)
@@ -53,7 +51,6 @@ static void LKMotorLostCallback(void *motor_ptr)
LKMotorInstance *LKMotorInit(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;
@@ -65,16 +62,10 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
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 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280
config->can_init_config.tx_id = LKMotorSingleCanID(config->can_init_config.tx_id);
config->can_init_config.rx_id = config->can_init_config.tx_id;
motor->motor_can_ins = CANRegister(&config->can_init_config);
if (idx == 0) // 用第一个电机的can instance发送数据
{
sender_instance = motor->motor_can_ins;
sender_instance->tx_id = 0x280; // 修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用
}
LKMotorEnable(motor);
DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
lkmotor_instance[idx++] = motor;
@@ -82,14 +73,29 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
Daemon_Init_Config_s daemon_config = {
.callback = LKMotorLostCallback,
.owner_id = motor,
.reload_count = 50, // 50ms
.reload_count = 50,
};
motor->daemon = DaemonRegister(&daemon_config);
return motor;
}
/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */
static void LKMotorSendTorque(LKMotorInstance *motor, int16_t set)
{
uint16_t raw_set = (uint16_t)set;
uint8_t *tx_buff = motor->motor_can_ins->tx_buff;
tx_buff[0] = 0xA1;
tx_buff[1] = 0x00;
tx_buff[2] = 0x00;
tx_buff[3] = 0x00;
tx_buff[4] = (uint8_t)(raw_set & 0xff);
tx_buff[5] = (uint8_t)(raw_set >> 8);
tx_buff[6] = 0x00;
tx_buff[7] = 0x00;
CANTransmit(motor->motor_can_ins, 0.2);
}
void LKMotorControl()
{
float pid_measure, pid_ref;
@@ -105,48 +111,41 @@ void LKMotorControl()
setting = &motor->motor_settings;
pid_ref = motor->pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
pid_ref *= -1;
pid_ref *= -1.0f;
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_measure = measure->total_angle;
pid_ref = PIDCalculate(&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->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (setting->angle_feedback_source == OTHER_FEED)
if (setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_rads;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
pid_ref = PIDCalculate(&motor->speed_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 = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
}
set = (int16_t)pid_ref;
if (pid_ref > I_MAX)
pid_ref = I_MAX;
else if (pid_ref < I_MIN)
pid_ref = I_MIN;
// 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t));
}
set = motor->stop_flag == MOTOR_STOP ? 0 : (int16_t)pid_ref;
LKMotorSendTorque(motor, set);
}
if (idx) // 如果有电机注册了
CANTransmit(sender_instance, 0.2);
}
void LKMotorStop(LKMotorInstance *motor)