ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

chore: 导入 Simba-OmniInfantry 基线(云台/底盘双工程)

Browse Source 
- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码
- 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等)
  与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局)
- core.autocrlf=false,避免固件源码在检出时被改动换行符
This commit is contained in:
LSMushui
2026-09-12 10:28:21 +08:00
commit a4da5ad17c
517 changed files with 503795 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
底盘/modules/chassis/chassis.c
+197
View File
@@ -0,0 +1,197 @@
/* =============================================================================
* chassis.c - 底盘模块:麦轮逆解 + 四轮转速 PID + 跟随云台(双 INS 世界系)
* 电机槽位(与 legacy 实际发送一致,已修正 legacy 后轮反馈交叉的 bug):
* 0x201 右前 0x202 左前 0x203 左后 0x204 右后
* 逆解(legacy Mecanum_PowerMoveControl 实际生效版本):
* w_wheel = (vx ± vy ± w*(rx+ry)/100) / R * 18143.66
* 跟随(2026-09 二次重写):改回 Yaw GM6020(0x205) 编码器多圈相对角——
* 编码器测的是"云台相对底盘"的机械角,零漂移(此前双 INS 方案两板 yaw
* 均无绝对参考,差值温漂,长时间工况不成立,弃用)。
* 符号全自动综合,无需手拨判据:
* 云台自检(0x1A9 flags)下发"tick 增向=云台 CCW/CW";
* 底盘自检 chassis_detect_follow_sign() 用小 w 脉冲+底盘 INS 测出
* "w 正向=底盘 CCW/CW";两者相乘即跟随环符号
* 功率: chassis_control() 的 power_scale 直接缩放四轮输出电流
* ========================================================================== */
#include "chassis.h"
#include "pid.h"
#include "filter.h"
#include "attitude.h"
#include "can.h"
#include "robot_param.h"
#include "c_comm.h"
dji_motor_t motor_wheel_rf; // 0x201 右前
dji_motor_t motor_wheel_lf; // 0x202 左前
dji_motor_t motor_wheel_lr; // 0x203 左后
dji_motor_t motor_wheel_rr; // 0x204 右后
dji_motor_t motor_yaw_follow; // Yaw GM6020 反馈,CAN2 0x205(跟随用,相对角零漂移)
static PID_PositionTypeDef wheel_speed_pid[4]; // 顺序:RF/LF/LR/RR(槽位序)
static PID_PositionTypeDef follow_pid;
/* 跟随环诊断导出(debug_task 打印) */
volatile float dbg_follow_err_deg = 0;
volatile float dbg_follow_out = 0;
volatile float dbg_follow_sign = 0; // 综合后的跟随符号(0=未定)
static float follow_target_tick = 0; // 跟随基准(编码器累计值)
static uint8_t follow_latched = 0;
static uint8_t follow_reset_request = 0;
void chassis_motor_init(void)
{
dji_motor_init(&motor_wheel_rf, &hcan1, 0x201, DJI_MOTOR_3508);
dji_motor_init(&motor_wheel_lf, &hcan1, 0x202, DJI_MOTOR_3508);
dji_motor_init(&motor_wheel_lr, &hcan1, 0x203, DJI_MOTOR_3508);
dji_motor_init(&motor_wheel_rr, &hcan1, 0x204, DJI_MOTOR_3508);
dji_motor_init(&motor_yaw_follow, &hcan2, 0x205, DJI_MOTOR_6020);
for (uint8_t i = 0; i < 4; i++)
{
PID_PositionStructureInit(&wheel_speed_pid[i], 0);
PID_PositionSetParameter(&wheel_speed_pid[i], MECANUM_WHEEL_SPEED_KP,
MECANUM_WHEEL_SPEED_KI, MECANUM_WHEEL_SPEED_KD);
PID_PositionSetOUTRange(&wheel_speed_pid[i], -MECANUM_WHEEL_SPEED_OUT_LIMIT,
MECANUM_WHEEL_SPEED_OUT_LIMIT);
}
PID_PositionStructureInit(&follow_pid, 0);
PID_PositionSetParameter(&follow_pid, CHASSIS_FOLLOW_KP, CHASSIS_FOLLOW_KI, CHASSIS_FOLLOW_KD);
PID_PositionSetOUTRange(&follow_pid, -CHASSIS_FOLLOW_OUT_LIMIT, CHASSIS_FOLLOW_OUT_LIMIT);
}
uint8_t chassis_motor_offline(void)
{
uint32_t now = HAL_GetTick();
return (motor_wheel_rf.fdb.last_online_tick == 0 || now - motor_wheel_rf.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_wheel_lf.fdb.last_online_tick == 0 || now - motor_wheel_lf.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_wheel_lr.fdb.last_online_tick == 0 || now - motor_wheel_lr.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_wheel_rr.fdb.last_online_tick == 0 || now - motor_wheel_rr.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS);
}
void chassis_control(float vx, float vy, float w, float power_scale)
{
/* 麦轮逆解(legacy Mecanum_PowerMoveControl 的实际生效版本)
* 换算:w_wheel = (vx±vy±w*(rx+ry)/100)/R * 18143.66 */
float rear = w * (MECANUM_RX + MECANUM_RY) / 100.0f;
float rf = (vx - vy - rear) / MECANUM_WHEEL_RADIUS * MECANUM_SPEED_FACTOR;
float lf = (-vx - vy - rear) / MECANUM_WHEEL_RADIUS * MECANUM_SPEED_FACTOR;
float lr = (vx + vy - rear) / MECANUM_WHEEL_RADIUS * MECANUM_SPEED_FACTOR;
float rr = (-vx + vy - rear) / MECANUM_WHEEL_RADIUS * MECANUM_SPEED_FACTOR;
wheel_speed_pid[0].Need_Value = rf;
wheel_speed_pid[1].Need_Value = lf;
wheel_speed_pid[2].Need_Value = lr;
wheel_speed_pid[3].Need_Value = rr;
PID_PositionCalc(&wheel_speed_pid[0], motor_wheel_rf.fdb.rotor_speed);
PID_PositionCalc(&wheel_speed_pid[1], motor_wheel_lf.fdb.rotor_speed);
PID_PositionCalc(&wheel_speed_pid[2], motor_wheel_lr.fdb.rotor_speed);
PID_PositionCalc(&wheel_speed_pid[3], motor_wheel_rr.fdb.rotor_speed);
/* 简易功率限制:按 power_scale 缩放输出电流 */
int16_t out[4];
out[0] = (int16_t)(wheel_speed_pid[0].OUT * power_scale);
out[1] = (int16_t)(wheel_speed_pid[1].OUT * power_scale);
out[2] = (int16_t)(wheel_speed_pid[2].OUT * power_scale);
out[3] = (int16_t)(wheel_speed_pid[3].OUT * power_scale);
dji_motor_send_current_low(&hcan1, out[0], out[1], out[2], out[3]);
}
void chassis_follow_reset(void)
{
follow_reset_request = 1; // 下个跟随周期重锁基准并清积分
PID_PositionClean(&follow_pid);
}
/* 底盘自检:给四轮一个小 w 脉冲,用底盘 INS yaw 响应方向测出
* "w 正向 = 底盘 CCW(+1)/CW(-1)"。阻塞约 200ms,落地时整车会轻转一下。
* 返回 1-成功 0-失败(轮离线/姿态未就绪/无响应) */
uint8_t chassis_detect_follow_sign(void)
{
if (chassis_motor_offline() || !att_ready)
return 0;
chassis_output_zero();
HAL_Delay(50); // 静默期
const float yaw_before = att_deg_yaw;
chassis_control(0, 0, CHASSIS_DETECT_W, 1.0f);
for (uint16_t i = 0; i < CHASSIS_DETECT_TIME_MS; i++)
HAL_Delay(1);
chassis_output_zero();
const float dyaw = theta_format(att_deg_yaw - yaw_before);
HAL_Delay(80); // 回稳
if (dyaw > CHASSIS_DETECT_YAW_MIN)
{
dbg_follow_sign = 1.0f; // w+ -> 底盘 CCW
return 1;
}
if (dyaw < -CHASSIS_DETECT_YAW_MIN)
{
dbg_follow_sign = -1.0f; // w+ -> 底盘 CW
return 1;
}
return 0; // 车没动(架空无力矩/被挡),不给结论
}
/* 跟随符号综合:
* 云台自检给出"tick 增向 = 云台 CCW(+1)/CW(-1)"(ctoc_encoder_ccw)
* 底盘自检给出"w 正向 = 底盘 CCW(+1)/CW(-1)"
* 推导:底盘 CCW 转 δ(世界系)且云台被其 yaw 环锁定世界向时,
* 云台相对底盘 CW 偏 δ。若 tick 增向=云台 CCW,则相对 tick 误差 e
* (增=CCW 为正)随底盘 CCW 转动而减小 -> 需要 w 使底盘 CCW:
* w = chassis_ccw_sign * K * err_ccw,其中 err_ccw = enc_ccw * err_tick */
float chassis_follow_sign(void)
{
if (dbg_follow_sign == 0 || ctoc_encoder_ccw == 0)
return CHASSIS_FOLLOW_SIGN; // 自检未完成,回退参数
return dbg_follow_sign * (float)ctoc_encoder_ccw;
}
/* 跟随一步:反馈为 Yaw GM6020 编码器相对角(零漂移),输出 w */
float chassis_follow_w(void)
{
/* 尚未收到 0x205 反馈时不锁定基准(避免首帧假偏差),输出 0 */
if (motor_yaw_follow.fdb.last_online_tick == 0)
{
follow_latched = 0;
return 0;
}
if (follow_reset_request || !follow_latched)
{
follow_reset_request = 0;
follow_target_tick = (float)motor_yaw_follow.fdb.total_tick;
follow_latched = 1;
PID_PositionClean(&follow_pid);
}
/* 相对角误差(°):tick 增向按云台自检结果折算为 CCW 正 */
float err_deg = ((float)motor_yaw_follow.fdb.total_tick - follow_target_tick) * 360.0f / 8192.0f;
if (ctoc_encoder_ccw < 0)
err_deg = -err_deg;
if (err_deg > 180.0f)
err_deg -= 360.0f;
else if (err_deg < -180.0f)
err_deg += 360.0f;
if (err_deg > -CHASSIS_FOLLOW_DEADBAND && err_deg < CHASSIS_FOLLOW_DEADBAND)
err_deg = 0;
follow_pid.Need_Value = 0;
PID_PositionCalc(&follow_pid, chassis_follow_sign() * -err_deg);
dbg_follow_err_deg = err_deg;
dbg_follow_out = follow_pid.OUT;
return follow_pid.OUT;
}
void chassis_output_zero(void)
{
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
}
底盘/modules/chassis/chassis.h
+49
View File
@@ -0,0 +1,49 @@
#ifndef CHASSIS_H
#define CHASSIS_H
#include <stdint.h>
#include "dji_motor.h"
/* 底盘模块:麦轮逆解 + 四轮转速 PID + 跟随云台(双 INS 世界系,2026-09 重写)
* 电机接线(与 legacy 实际发送一致):
* 0x201 右前 0x202 左前 0x203 左后 0x204 右后 */
extern dji_motor_t motor_wheel_rf; // 0x201 右前
extern dji_motor_t motor_wheel_lf; // 0x202 左前
extern dji_motor_t motor_wheel_lr; // 0x203 左后
extern dji_motor_t motor_wheel_rr; // 0x204 右后
extern dji_motor_t motor_yaw_follow; // Yaw GM6020,CAN2 0x205(跟随反馈)
extern void chassis_motor_init(void);
/* 判定任一轮电机掉线 */
extern uint8_t chassis_motor_offline(void);
/* 底盘运动控制(2ms 调用)
* vx:前进(+)/后退(-) vy:左(+)/右(-) w:逆时针(+)旋转
* power_scale:功率限制系数(0~1),直接缩放输出电流 */
extern void chassis_control(float vx, float vy, float w, float power_scale);
/* 底盘自检:小 w 脉冲+底盘 INS 测"w 正向=底盘 CCW/CW",与云台下发的
* 编码器方向综合出跟随符号(阻塞约 200ms,整车会轻转)。
* 返回 1-成功 0-失败 */
extern uint8_t chassis_detect_follow_sign(void);
/* 综合后的跟随符号(+1/-1;自检未完成时为参数回退值) */
extern float chassis_follow_sign(void);
/* 跟随一步(2ms,FOLLOW 模式调用;反馈为编码器相对角,零漂移) */
extern float chassis_follow_w(void);
/* 请求下个周期重锁基准并清积分 */
extern void chassis_follow_reset(void);
/* 底盘输出清零(停车) */
extern void chassis_output_zero(void);
/* 跟随环诊断导出(debug_task 打印) */
extern volatile float dbg_follow_err_deg;
extern volatile float dbg_follow_out;
extern volatile float dbg_follow_sign;
#endif