robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

上ai

Browse Source
This commit is contained in:
Robofish 2026-01-14 10:10:12 +08:00
parent bfb2368082
commit 5abd099f6a
8 changed files with 8202 additions and 8153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

16288
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

2
User/module/balance_chassis.c
View File

@ -222,7 +222,7 @@ static float Chassis_CalcSpringForce(float leg_length)
const float L_AC = 0.042f; // 弹簧上端高度
const float L_BD = 0.050f; // 杆BD长度
const float L_BE = 0.254f; // 杆BE长度
const float Fs = 360.0f *(360.0f/220.0f); // 弹簧恒力 (N)
const float Fs = 360.0f; // 弹簧恒力 (N)
// 通过余弦定理计算∠ABE的余弦值
float cos_theta = (L_AB*L_AB + L_BE*L_BE - leg_length*leg_length) / (2.0f * L_AB * L_BE);

4
User/module/config.c
View File

@ -24,9 +24,9 @@ Config_RobotParam_t robot_config = {
.gimbal_param = {
.pid = {
.yaw_omega = {
.k = 2.0f,
.k = 0.25f,
.p = 1.0f,
.i = 0.3f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,

44
User/module/gimbal.c
View File

@ -223,43 +223,55 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
g->out.pit = 0.0f;
break;
case GIMBAL_MODE_AI_CONTROL:
if (g_ai != NULL && g_ai->ctrl) {
if (g_ai != NULL && g_ai->ctrl) {
g->setpoint.eulr.yaw = g_ai->yaw;
g->setpoint.eulr.pit = g_ai->pit;
g->setpoint.eulr.pit = -g_ai->pit;
/* 限位处理 - 使用圆周误差保持一致性 */
/* 限位处理 - 需要考虑电机和IMU之间的偏差 */
if (g->param->travel.yaw > 0) {
if (CircleError(g->setpoint.eulr.yaw, g->limit.yaw.max, M_2PI) > 0) {
g->setpoint.eulr.yaw = g->limit.yaw.max;
float yaw_motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
if (yaw_motor_imu_offset > M_PI) yaw_motor_imu_offset -= M_2PI;
if (yaw_motor_imu_offset < -M_PI) yaw_motor_imu_offset += M_2PI;
/* 将IMU setpoint转换为电机角度后进行限位检查 */
float yaw_motor_target = g->setpoint.eulr.yaw + yaw_motor_imu_offset;
if (CircleError(yaw_motor_target, g->limit.yaw.max, M_2PI) > 0) {
g->setpoint.eulr.yaw = g->limit.yaw.max - yaw_motor_imu_offset;
}
if (CircleError(g->setpoint.eulr.yaw, g->limit.yaw.min, M_2PI) < 0) {
g->setpoint.eulr.yaw = g->limit.yaw.min;
if (CircleError(yaw_motor_target, g->limit.yaw.min, M_2PI) < 0) {
g->setpoint.eulr.yaw = g->limit.yaw.min - yaw_motor_imu_offset;
}
}
if (g->param->travel.pit > 0) {
if (CircleError(g->setpoint.eulr.pit, g->limit.pit.max, M_2PI) > 0) {
g->setpoint.eulr.pit = g->limit.pit.max;
float pit_motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
if (pit_motor_imu_offset > M_PI) pit_motor_imu_offset -= M_2PI;
if (pit_motor_imu_offset < -M_PI) pit_motor_imu_offset += M_2PI;
/* 将IMU setpoint转换为电机角度后进行限位检查 */
float pit_motor_target = g->setpoint.eulr.pit + pit_motor_imu_offset;
if (CircleError(pit_motor_target, g->limit.pit.max, M_2PI) > 0) {
g->setpoint.eulr.pit = g->limit.pit.max - pit_motor_imu_offset;
}
if (CircleError(g->setpoint.eulr.pit, g->limit.pit.min, M_2PI) < 0) {
g->setpoint.eulr.pit = g->limit.pit.min;
if (CircleError(pit_motor_target, g->limit.pit.min, M_2PI) < 0) {
g->setpoint.eulr.pit = g->limit.pit.min - pit_motor_imu_offset;
}
}
}
/* fallthrough - AI控制模式也需要执行PID计算 */
case GIMBAL_MODE_ABSOLUTE:
case GIMBAL_MODE_RELATIVE:
/* AI 控制模式：直接使用 AI 下发的目标角度 */
/* PID 控制计算 - AI 和手动控制都需要执行 */
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.0f, g->dt);
g->feedback.imu.gyro.z, 0.f, g->dt);
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.0f, g->dt);
g->feedback.imu.gyro.y, 0.f, g->dt);
break;
}

12
User/task/ai.c
View File

@ -4,10 +4,12 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/fdcan.h"
#include "module/config.h"
#include "module/gimbal.h"
#include "device/vision_bridge.h"
/* USER INCLUDE END */
@ -18,6 +20,7 @@
/* USER STRUCT BEGIN */
AI_cmd_t cmd_ai;
AI_t ai;
Gimbal_AI_t ai_for_gimbal;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -41,6 +44,15 @@ void Task_ai(void *argument) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
AI_ParseCmdFromCan( &ai,&cmd_ai);
if (cmd_ai.mode != 0){
ai_for_gimbal.ctrl = true;
} else {
ai_for_gimbal.ctrl = false;
}
ai_for_gimbal.yaw = cmd_ai.gimbal.setpoint.yaw;
ai_for_gimbal.pit = cmd_ai.gimbal.setpoint.pit;
osMessageQueueReset(task_runtime.msgq.gimbal.ai_cmd);
osMessageQueuePut(task_runtime.msgq.gimbal.ai_cmd, &ai_for_gimbal, 0, 0);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

4
User/task/ctrl_gimbal.c
View File

@ -49,13 +49,13 @@ void Task_ctrl_gimbal(void *argument) {
Gimbal_UpdateIMU(&gimbal);
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.ai_cmd, &gimbal_ai, NULL, 0);
Gimbal_UpdateFeedback(&gimbal);
osMessageQueueReset(task_runtime.msgq.chassis.yaw); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.ai_cmd, &gimbal_ai, NULL, 0);
Gimbal_Control(&gimbal, &gimbal_cmd, &gimbal_ai);
Gimbal_Output(&gimbal);

1
User/task/init.c
View File

@ -49,6 +49,7 @@ void Task_Init(void *argument) {
task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核