236 lines
9.3 KiB
C
236 lines
9.3 KiB
C
/* =============================================================================
|
|
* 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 <math.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 编码器对齐设定机械零位(4050),输出 w */
|
|
float chassis_follow_w(void)
|
|
{
|
|
/* 尚未收到 0x205 反馈时输出 0 */
|
|
if (motor_yaw_follow.fdb.last_online_tick == 0)
|
|
return 0;
|
|
|
|
if (follow_reset_request)
|
|
{
|
|
follow_reset_request = 0;
|
|
PID_PositionClean(&follow_pid);
|
|
}
|
|
|
|
/* 计算当前机械编码值距离设定零位(4050)的最短差值(-4096 ~ +4096) */
|
|
int16_t tick_diff = (int16_t)motor_yaw_follow.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
|
|
if (tick_diff > 4096)
|
|
tick_diff -= 8192;
|
|
else if (tick_diff < -4096)
|
|
tick_diff += 8192;
|
|
|
|
/* 相对角误差(°):tick 增向按云台自检结果折算为 CCW 正 */
|
|
float err_deg = (float)tick_diff * (360.0f / 8192.0f);
|
|
if (ctoc_encoder_ccw < 0)
|
|
err_deg = -err_deg;
|
|
|
|
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);
|
|
}
|
|
|
|
#ifndef PI
|
|
#define PI 3.14159265358979323846f
|
|
#endif
|
|
|
|
void chassis_vector_to_body(float vx_g, float vy_g, float w, float * vx_c, float * vy_c)
|
|
{
|
|
/* 电机掉线兜底:若无云台编码器反馈,直接直通输出 */
|
|
if (motor_yaw_follow.fdb.last_online_tick == 0)
|
|
{
|
|
*vx_c = vx_g;
|
|
*vy_c = vy_g;
|
|
return;
|
|
}
|
|
|
|
/* 1. 计算当前机械编码值距离设定零位的最短差值(-4096 ~ +4096) */
|
|
int16_t tick_diff = (int16_t)motor_yaw_follow.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
|
|
if (tick_diff > 4096)
|
|
tick_diff -= 8192;
|
|
else if (tick_diff < -4096)
|
|
tick_diff += 8192;
|
|
|
|
/* 2. 相对机械角误差(rad):tick 增向按云台自检结果折算为 CCW 正 */
|
|
float alpha_rad = (float)tick_diff * (2.0f * PI / 8192.0f);
|
|
int8_t enc_sign = (ctoc_encoder_ccw != 0) ? ctoc_encoder_ccw : 1;
|
|
if (enc_sign < 0)
|
|
alpha_rad = -alpha_rad;
|
|
|
|
/* 3. 叠加角速度前馈补偿(-kw 消除通信及响应时延滞后) */
|
|
float alpha_comp = alpha_rad - (CHASSIS_SPIN_FEEDFORWARD_K * w);
|
|
|
|
/* 4. 二维旋转矩阵投影(修正正负号: 适配 vx<0前进, vy>0向左 的坐标系定义) */
|
|
float cos_a = cosf(alpha_comp);
|
|
float sin_a = sinf(alpha_comp);
|
|
|
|
*vx_c = vx_g * cos_a + vy_g * sin_a;
|
|
*vy_c = -vx_g * sin_a + vy_g * cos_a;
|
|
}
|
|
|