TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +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

11
application/chassis/balance.c
View File

@@ -28,10 +28,10 @@
#define STOOL_YAW_RATE_REF 3.5f
#define STOOL_ACC_REF 4.0f
#define STOOL_BRAKE_ACC_REF 8.0f
#define STOOL_PITCH_K 55.0f
#define STOOL_PITCH_W_K 7.0f
#define STOOL_VEL_K 10.0f
#define STOOL_STOP_DIST_K 8.0f
#define STOOL_PITCH_K 36.0f
#define STOOL_PITCH_W_K 10.0f
#define STOOL_VEL_K 6.0f
#define STOOL_STOP_DIST_K 3.0f
#define STOOL_VEL_LPF_RC 0.03f
#define STOOL_WHEEL_TORQUE_LIMIT 25.0f
#define STOOL_RC_DEADBAND 40
@@ -353,9 +353,6 @@ static void ControlSwitch()
if (DrivenMotorIsLost())
return;
if (!switch_is_up(rc_data[TEMP].rc.switch_right))
return;
chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE;
chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF;
chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;

6
application/cmd/robot_cmd.c
View File

@@ -330,8 +330,8 @@ static void EmergencyHandler()
if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下]或[中]
{
// 急停触发条件：遥控器离线 或者 右侧开关打到[下] 或者 robot_state 已经是 STOP
if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right) || robot_state == ROBOT_STOP)
// 急停触发条件：遥控器离线或者右侧开关打到[下]
if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right))
{
robot_state = ROBOT_STOP;
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
@@ -343,7 +343,7 @@ static void EmergencyHandler()
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步
gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
}
// 恢复正常运行条件：遥控器在线 且 右侧开关状态为[上]
// 恢复正常运行条件：遥控器在线且右侧开关状态为[上]或[中]
else if (switch_is_up(rc_data[TEMP].rc.switch_right) || switch_is_mid(rc_data[TEMP].rc.switch_right))
{
robot_state = ROBOT_READY;

35
bsp/can/bsp_can.c
View File

@@ -11,6 +11,11 @@
static CANInstance *can_instance[CAN_MX_REGISTER_CNT] = {NULL};
static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自增
static uint8_t CANBusIndex(CAN_HandleTypeDef *hcan)
{
return hcan == &hcan1 ? 0u : 1u;
}
/* ----------------two static function called by CANRegister()-------------------- */
/**
@@ -28,18 +33,26 @@ static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自
*/
static void CANAddFilter(CANInstance *_instance)
{
CAN_FilterTypeDef can_filter_conf;
static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; // 0-13给can1用,14-27给can2用
static uint8_t all_pass_filter_configured[DEVICE_CAN_CNT] = {0};
uint8_t bus = CANBusIndex(_instance->can_handle);
can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST; // 使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; // 使用16位id模式,即只有低16位有效
can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; // 奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
can_filter_conf.SlaveStartFilterBank = 14; // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机)
can_filter_conf.FilterIdLow = _instance->rx_id << 5; // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0
can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); // 根据can_handle判断是CAN1还是CAN2,然后自增
can_filter_conf.FilterActivation = CAN_FILTER_ENABLE; // 启用过滤器
if (all_pass_filter_configured[bus])
return;
HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf);
CAN_FilterTypeDef accept_all_filter = {0};
accept_all_filter.FilterActivation = CAN_FILTER_ENABLE;
accept_all_filter.FilterMode = CAN_FILTERMODE_IDMASK;
accept_all_filter.FilterScale = CAN_FILTERSCALE_32BIT;
accept_all_filter.FilterIdHigh = 0x0000;
accept_all_filter.FilterIdLow = 0x0000;
accept_all_filter.FilterMaskIdHigh = 0x0000;
accept_all_filter.FilterMaskIdLow = 0x0000;
accept_all_filter.FilterBank = _instance->can_handle == &hcan1 ? 0 : 14;
accept_all_filter.FilterFIFOAssignment = CAN_RX_FIFO0;
accept_all_filter.SlaveStartFilterBank = 14;
HAL_CAN_ConfigFilter(_instance->can_handle, &accept_all_filter);
all_pass_filter_configured[bus] = 1;
return;
}
/**
@@ -163,7 +176,7 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
memcpy(can_instance[i]->rx_buff, can_rx_buff, rxconf.DLC); // 消息拷贝到对应实例
can_instance[i]->can_module_callback(can_instance[i]); // 触发回调进行数据解析和处理
}
return;
break;
}
}
}

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)