mirror of
https://gitee.com/dlmu-cone/tronone-h7-scaffold
synced 2026-07-24 03:27:45 +08:00
add some files
This commit is contained in:
@@ -1,5 +1,219 @@
|
||||
//
|
||||
// Created by esqwt on 2026/3/2.
|
||||
//
|
||||
|
||||
#include "chassis_half_steer.h"
|
||||
#include "user_lib.h" // 包含 arm_math.h, PI, user_malloc 等
|
||||
#include <math.h>
|
||||
|
||||
// 宏定义 (根据实际机械结构调整)
|
||||
#ifndef WHEEL_BASE
|
||||
#define WHEEL_BASE 0.35f // 轴距 (示例值)
|
||||
#endif
|
||||
#ifndef TRACK_WIDTH
|
||||
#define TRACK_WIDTH 0.35f // 轮距 (示例值)
|
||||
#endif
|
||||
|
||||
#define CHASSIS_WHEEL_OFFSET 30.0f // 舵轮偏置参数
|
||||
#define SQRT2 1.41421356f // 根号2
|
||||
#define RAD_2_DEGREE 57.2957795f
|
||||
#define DEGREE_2_RAD 0.01745329f
|
||||
|
||||
// 舵轮对齐角度 (根据实际安装调整)
|
||||
#define STEERING_CHASSIS_ALIGN_ANGLE_RF 0.0f
|
||||
#define STEERING_CHASSIS_ALIGN_ANGLE_LB 0.0f
|
||||
|
||||
// 静态函数声明
|
||||
static void MinmizeRotation(float *angle, const float *last_angle, float *speed);
|
||||
static void SteeringWheelCalculate(Chassis_HalfSteer_t *chassis);
|
||||
|
||||
// 默认跟随PID配置
|
||||
static PID_Init_Config_s follow_pid_config = {
|
||||
.Kp = 6.0f,
|
||||
.Ki = 0.0f,
|
||||
.Kd = 0.495f,
|
||||
.MaxOut = 45.0f,
|
||||
};
|
||||
|
||||
void Chassis_HalfSteer_Init(Chassis_HalfSteer_t *chassis,
|
||||
LKMotorInstance *drive_rf, LKMotorInstance *drive_lb,
|
||||
DJIMotorInstance *steer_rf, DJIMotorInstance *steer_lb)
|
||||
{
|
||||
if (chassis == NULL) return;
|
||||
|
||||
// 绑定电机实例
|
||||
chassis->motor_drive_rf = drive_rf;
|
||||
chassis->motor_drive_lb = drive_lb;
|
||||
chassis->motor_steer_rf = steer_rf;
|
||||
chassis->motor_steer_lb = steer_lb;
|
||||
|
||||
// 初始化PID
|
||||
PIDInit(&chassis->pid_follow, &follow_pid_config);
|
||||
|
||||
// 初始化状态变量
|
||||
chassis->last_angle_rf = 0.0f;
|
||||
chassis->last_angle_lb = 0.0f;
|
||||
chassis->target_speed_rf = 0.0f;
|
||||
chassis->target_speed_lb = 0.0f;
|
||||
chassis->target_angle_rf = 0.0f;
|
||||
chassis->target_angle_lb = 0.0f;
|
||||
|
||||
// 如果有电机需要特定的初始化配置（如 dji_motor 的参数），请在此处补充或在外部完成
|
||||
}
|
||||
|
||||
void Chassis_HalfSteer_Update(Chassis_HalfSteer_t *chassis, const Chassis_Ctrl_Cmd_s *cmd)
|
||||
{
|
||||
if (chassis == NULL || cmd == NULL) return;
|
||||
|
||||
// 1. 更新内部命令副本
|
||||
chassis->cmd = *cmd;
|
||||
|
||||
// 2. 检查底盘模式与安全状态
|
||||
if (chassis->cmd.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{
|
||||
LKMotorStop(chassis->motor_drive_rf);
|
||||
LKMotorStop(chassis->motor_drive_lb);
|
||||
DJIMotorStop(chassis->motor_steer_rf);
|
||||
DJIMotorStop(chassis->motor_steer_lb);
|
||||
return; // 直接返回，不再计算
|
||||
}
|
||||
else
|
||||
{
|
||||
LKMotorEnable(chassis->motor_drive_rf);
|
||||
LKMotorEnable(chassis->motor_drive_lb);
|
||||
DJIMotorEnable(chassis->motor_steer_rf);
|
||||
DJIMotorEnable(chassis->motor_steer_lb);
|
||||
}
|
||||
|
||||
// 3. 预处理旋转量 (wz)
|
||||
switch (chassis->cmd.chassis_mode)
|
||||
{
|
||||
case CHASSIS_NO_FOLLOW:
|
||||
chassis->cmd.wz = 0;
|
||||
break;
|
||||
case CHASSIS_FOLLOW_GIMBAL_YAW:
|
||||
{
|
||||
float angle_err = chassis->cmd.offset_angle;
|
||||
// 归一化到 [-180, 180]
|
||||
if(angle_err > 180.0f) angle_err -= 360.0f;
|
||||
else if(angle_err < -180.0f) angle_err += 360.0f;
|
||||
|
||||
// 计算跟随PID输出
|
||||
chassis->cmd.wz = PIDCalculate(&chassis->pid_follow, angle_err, 0.0f) / 100.0f; // 根据原代码保留/100
|
||||
}
|
||||
break;
|
||||
case CHASSIS_ROTATE:
|
||||
chassis->cmd.wz = 0.5f; // 固定自旋速度，可改为变量
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// 4. 坐标系转换 (云台系 -> 底盘系)
|
||||
// 假设 cmd.vx/vy 是云台坐标系下的指令
|
||||
float sin_theta = arm_sin_f32(chassis->cmd.offset_angle * DEGREE_2_RAD);
|
||||
float cos_theta = arm_cos_f32(chassis->cmd.offset_angle * DEGREE_2_RAD);
|
||||
|
||||
// 覆盖原始 vx/vy 为底盘系速度 (使用中间变量避免污染原始cmd数据，这里直接覆盖cmd结构体中的值用于后续计算)
|
||||
float chassis_vx = chassis->cmd.vx * cos_theta - chassis->cmd.vy * sin_theta;
|
||||
float chassis_vy = chassis->cmd.vx * sin_theta + chassis->cmd.vy * cos_theta;
|
||||
|
||||
// 将转换后的速度存回用于计算，或者传递给计算函数
|
||||
// 为了保持清晰，我们修改 SteeringWheelCalculate 的输入方式，这里暂时存入 cmd 结构体或传递局部变量
|
||||
// 这里选择传递局部变量，需要修改 SteeringWheelCalculate 内部逻辑
|
||||
// 为了复用原逻辑，我将在函数内部使用 chassis_vx/vy
|
||||
|
||||
// 5. 运动学解算
|
||||
// 传入 chassis_vx, chassis_vy 和 chassis->cmd.wz
|
||||
// 注意：原代码使用全局变量，这里我们需要适配
|
||||
|
||||
float w = chassis->cmd.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
|
||||
|
||||
if (fabsf(chassis_vx) == 0 && fabsf(chassis_vy) == 0 && chassis->cmd.wz == 0) {
|
||||
chassis->target_speed_lb = 0;
|
||||
chassis->target_speed_rf = 0;
|
||||
// 角度保持不变，或回中？原代码保持不变
|
||||
} else {
|
||||
// LB (Left Back) 计算: y+, x-
|
||||
// 注意：原代码注释里的方向似乎与变量名有差异，这里基于原代码逻辑复刻
|
||||
// 原代码: arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
|
||||
// temp_x = chassis_vx - w; temp_y = chassis_vy + w;
|
||||
float temp_x_lb = chassis_vx - w;
|
||||
float temp_y_lb = chassis_vy + w;
|
||||
arm_sqrt_f32(temp_x_lb * temp_x_lb + temp_y_lb * temp_y_lb, &chassis->target_speed_lb);
|
||||
|
||||
float offset_lb = -atan2f(temp_y_lb, temp_x_lb) * RAD_2_DEGREE;
|
||||
chassis->target_angle_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb;
|
||||
|
||||
// RF (Right Front) 计算: y-, x+
|
||||
// 原代码: arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+
|
||||
// temp_x = chassis_vx + w; temp_y = chassis_vy - w;
|
||||
float temp_x_rf = chassis_vx + w;
|
||||
float temp_y_rf = chassis_vy - w;
|
||||
arm_sqrt_f32(temp_x_rf * temp_x_rf + temp_y_rf * temp_y_rf, &chassis->target_speed_rf);
|
||||
|
||||
float offset_rf = -atan2f(temp_y_rf, temp_x_rf) * RAD_2_DEGREE;
|
||||
chassis->target_angle_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf;
|
||||
|
||||
// 6. 角度优化 (MinmizeRotation)
|
||||
// 更新 last_angle
|
||||
chassis->last_angle_lb = chassis->motor_steer_lb->measure.total_angle;
|
||||
chassis->last_angle_rf = chassis->motor_steer_rf->measure.total_angle;
|
||||
|
||||
// 限制到 [-180, 180] 绝对值逻辑? 原代码使用了 ANGLE_LIMIT_360_TO_180_ABS 宏
|
||||
// 这里手动实现或调用 user_lib
|
||||
// 假设 user_lib.h 中有相关宏，这里简单处理
|
||||
// (省略部分宏展开，直接使用 MinmizeRotation)
|
||||
|
||||
MinmizeRotation(&chassis->target_angle_lb, &chassis->last_angle_lb, &chassis->target_speed_lb);
|
||||
MinmizeRotation(&chassis->target_angle_rf, &chassis->last_angle_rf, &chassis->target_speed_rf);
|
||||
}
|
||||
|
||||
// 7. 发送控制指令
|
||||
// 转向电机 (DJI GM6020)
|
||||
DJIMotorSetRef(chassis->motor_steer_lb, chassis->target_angle_lb);
|
||||
DJIMotorSetRef(chassis->motor_steer_rf, chassis->target_angle_rf);
|
||||
|
||||
// 驱动电机 (LK 9015)
|
||||
LKMotorSetRef(chassis->motor_drive_lb, chassis->target_speed_lb);
|
||||
LKMotorSetRef(chassis->motor_drive_rf, chassis->target_speed_rf);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使舵电机角度最小旋转，取优弧
|
||||
*/
|
||||
static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
|
||||
{
|
||||
float target_angle = *angle;
|
||||
float actual_angle = *last_angle;
|
||||
float rotation = target_angle - actual_angle;
|
||||
float norm_rotation = rotation;
|
||||
|
||||
// 规范化旋转角度到 [-180, 180]
|
||||
while (norm_rotation > 180.0f) {
|
||||
norm_rotation -= 360.0f;
|
||||
}
|
||||
while (norm_rotation < -180.0f) {
|
||||
norm_rotation += 360.0f;
|
||||
}
|
||||
|
||||
float threshold = 110.0f; // 阈值，超过此角度则反转轮子方向
|
||||
|
||||
// 简单的优弧判断
|
||||
if (norm_rotation > threshold) {
|
||||
int32_t round_diff = (int32_t)((target_angle - actual_angle) / 360.0f);
|
||||
*angle = actual_angle + norm_rotation - 180.0f + round_diff * 360.0f;
|
||||
*speed = -(*speed);
|
||||
} else if (norm_rotation < -threshold) {
|
||||
int32_t round_diff = (int32_t)((target_angle - actual_angle) / 360.0f);
|
||||
*angle = actual_angle + norm_rotation + 180.0f + round_diff * 360.0f;
|
||||
*speed = -(*speed);
|
||||
}
|
||||
|
||||
// 如果没有触发反转，目标角度通常需要加上圈数，
|
||||
// 但原代码逻辑似乎是直接修改传入的 angle 指针。
|
||||
// 如果 norm_rotation 在阈值内，我们需要确保 angle 是基于 actual_angle 的最近点
|
||||
// 原逻辑中 MinmizeRotation 似乎只处理了反转的情况，
|
||||
// 对于常规旋转，可能需要确保 *angle 包含了正确的圈数信息。
|
||||
// 补充逻辑：
|
||||
if (norm_rotation <= threshold && norm_rotation >= -threshold) {
|
||||
// 计算最近的目标角度（包含圈数）
|
||||
*angle = actual_angle + norm_rotation;
|
||||
}
|
||||
}
|
||||
@@ -1,8 +1,89 @@
|
||||
//
|
||||
// Created by esqwt on 2026/3/2.
|
||||
//
|
||||
#ifndef CHASSIS_HALF_STEER_H
|
||||
#define CHASSIS_HALF_STEER_H
|
||||
|
||||
#ifndef TRONONEH7_SCAFFOLD_CHASSIS_HALF_STEER_H
|
||||
#define TRONONEH7_SCAFFOLD_CHASSIS_HALF_STEER_H
|
||||
#include "stdint.h"
|
||||
#include "dji_motor.h"
|
||||
#include "lk_motor.h" // 假设存在对应的C接口头文件
|
||||
#include "pid.h"
|
||||
#include "chassis_ctrl.h" // 包含底盘控制相关的通用定义
|
||||
|
||||
#endif // TRONONEH7_SCAFFOLD_CHASSIS_HALF_STEER_H
|
||||
// 定义底盘控制命令结构体 (如果 chassis_ctrl.h 中未定义，请在此定义或确保通用)
|
||||
#ifndef CHASSIS_CTRL_CMD_DEFINED
|
||||
#define CHASSIS_CTRL_CMD_DEFINED
|
||||
typedef enum
|
||||
{
|
||||
CHASSIS_ZERO_FORCE = 0, // 无力/急停
|
||||
CHASSIS_NO_FOLLOW, // 不跟随/自由移动
|
||||
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随云台Yaw
|
||||
CHASSIS_ROTATE, // 小陀螺/自旋
|
||||
} Chassis_Mode_e;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float vx; // 前后速度 (m/s)
|
||||
float vy; // 左右速度 (m/s)
|
||||
float wz; // 旋转角速度 (rad/s 或 对应单位)
|
||||
float offset_angle; // 底盘与云台的夹角 (度)
|
||||
Chassis_Mode_e chassis_mode;
|
||||
} Chassis_Ctrl_Cmd_s;
|
||||
#endif
|
||||
|
||||
// 定义底盘反馈数据结构体
|
||||
typedef struct
|
||||
{
|
||||
float vx;
|
||||
float vy;
|
||||
float wz;
|
||||
// float real_angle; // 预留
|
||||
} Chassis_Upload_Data_s;
|
||||
|
||||
// 半舵轮底盘对象结构体
|
||||
typedef struct
|
||||
{
|
||||
// 轮毂电机实例 (驱动) - LK9015
|
||||
LKMotorInstance *motor_drive_rf; // 右前
|
||||
LKMotorInstance *motor_drive_lb; // 左后
|
||||
|
||||
// 舵向电机实例 (转向) - GM6020
|
||||
DJIMotorInstance *motor_steer_rf; // 右前舵
|
||||
DJIMotorInstance *motor_steer_lb; // 左后舵
|
||||
|
||||
// PID实例
|
||||
PIDInstance pid_follow; // 跟随PID
|
||||
|
||||
// 控制命令与状态
|
||||
Chassis_Ctrl_Cmd_s cmd;
|
||||
Chassis_Upload_Data_s feedback;
|
||||
|
||||
// 内部计算中间变量
|
||||
float target_speed_rf; // 右前轮目标速度
|
||||
float target_speed_lb; // 左后轮目标速度
|
||||
float target_angle_rf; // 右前舵目标角度
|
||||
float target_angle_lb; // 左后舵目标角度
|
||||
|
||||
// 上一次的角度记录 (用于就近转动逻辑)
|
||||
float last_angle_rf;
|
||||
float last_angle_lb;
|
||||
|
||||
} Chassis_HalfSteer_t;
|
||||
|
||||
/**
|
||||
* @brief 初始化半舵轮底盘对象
|
||||
* @param chassis 底盘对象指针
|
||||
* @param drive_rf 右前驱动电机指针
|
||||
* @param drive_lb 左后驱动电机指针
|
||||
* @param steer_rf 右前转向电机指针
|
||||
* @param steer_lb 左后转向电机指针
|
||||
*/
|
||||
void Chassis_HalfSteer_Init(Chassis_HalfSteer_t *chassis,
|
||||
LKMotorInstance *drive_rf, LKMotorInstance *drive_lb,
|
||||
DJIMotorInstance *steer_rf, DJIMotorInstance *steer_lb);
|
||||
|
||||
/**
|
||||
* @brief 底盘控制更新函数，建议在RTOS任务中周期调用
|
||||
* @param chassis 底盘对象指针
|
||||
* @param cmd 控制命令指针
|
||||
*/
|
||||
void Chassis_HalfSteer_Update(Chassis_HalfSteer_t *chassis, const Chassis_Ctrl_Cmd_s *cmd);
|
||||
|
||||
#endif // CHASSIS_HALF_STEER_H
|
||||
@@ -1,5 +1,273 @@
|
||||
//
|
||||
// Created by esqwt on 2026/3/2.
|
||||
//
|
||||
|
||||
#include "chassis_omni.h"
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846f
|
||||
#endif
|
||||
|
||||
// 辅助函数：绝对值限幅
|
||||
static float abs_clip(float val, float limit)
|
||||
{
|
||||
if (val > limit) return limit;
|
||||
if (val < -limit) return -limit;
|
||||
return val;
|
||||
}
|
||||
|
||||
// PID配置
|
||||
static PID_Init_Config_s chassis_speed_pid_config = {
|
||||
.MaxOut = 16000.0f,
|
||||
.IntegralLimit = 2000.0f, // 对应 C++ IntegralLimit (Integral_Min/Max 在 C PID 中未直接对应，取其中值或限制值)
|
||||
.Kp = 15.0f,
|
||||
.Ki = 0.0f,
|
||||
.Kd = 0.001f,
|
||||
.Output_LPF_RC = 0.002f, // 对应 C++ Output_LPF
|
||||
.Derivative_LPF_RC = 0.002f, // 对应 C++ D_LPF
|
||||
.Improve = PID_Integral_Limit, // 对应 0x01
|
||||
};
|
||||
|
||||
// 跟随环内环 (对应 index 0)
|
||||
static PID_Init_Config_s follow_pid_inner_config = {
|
||||
.MaxOut = 4000.0f,
|
||||
.IntegralLimit = 200.0f,
|
||||
.Kp = 20.0f,
|
||||
.Ki = 4.0f,
|
||||
.Kd = 0.0001f,
|
||||
.Output_LPF_RC = 0.002f,
|
||||
.Derivative_LPF_RC = 0.002f,
|
||||
// 对应 0x37 = Integral_Limit | Differential_Forward | Trapezoid_Intergral | OutputFilter | ChangingIntegrationRate
|
||||
// C definitions:
|
||||
// PID_Integral_Limit (1)
|
||||
// PID_Derivative_On_Measurement (2)
|
||||
// PID_Trapezoid_Intergral (4)
|
||||
// PID_OutputFilter (16)
|
||||
// PID_ChangingIntegrationRate (32)
|
||||
.Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_Trapezoid_Intergral | PID_OutputFilter | PID_ChangingIntegrationRate,
|
||||
};
|
||||
|
||||
// 跟随环外环 (对应 index 1)
|
||||
static PID_Init_Config_s follow_pid_outer_config = {
|
||||
.MaxOut = 4000.0f,
|
||||
.IntegralLimit = 4000.0f,
|
||||
.Kp = 15.0f,
|
||||
.Ki = 0.0f,
|
||||
.Kd = 1.8f,
|
||||
.Output_LPF_RC = 0.002f,
|
||||
.Derivative_LPF_RC = 0.002f,
|
||||
.Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_Trapezoid_Intergral | PID_OutputFilter | PID_ChangingIntegrationRate,
|
||||
};
|
||||
|
||||
void Chassis_Omni_Init(Chassis_Omni_t *chassis, DJIMotorInstance *lf, DJIMotorInstance *rf, DJIMotorInstance *lb, DJIMotorInstance *rb)
|
||||
{
|
||||
if (chassis == NULL) return;
|
||||
|
||||
chassis->moto_chassis[0] = lf;
|
||||
chassis->moto_chassis[1] = rf;
|
||||
chassis->moto_chassis[2] = lb;
|
||||
chassis->moto_chassis[3] = rb;
|
||||
|
||||
// 初始化速度PID
|
||||
for (int i = 0; i < 4; i++) {
|
||||
PIDInit(&chassis->pid_speed[i], &chassis_speed_pid_config);
|
||||
}
|
||||
|
||||
// 初始化跟随PID (串级)
|
||||
PIDInit(&chassis->pid_follow_angle_inner, &follow_pid_inner_config);
|
||||
PIDInit(&chassis->pid_follow_angle_outer, &follow_pid_outer_config);
|
||||
|
||||
// 初始化功率控制参数
|
||||
chassis->power_config.super_power_health = 90.0f;
|
||||
chassis->power_config.super_power_week = 20.0f;
|
||||
chassis->power_config.chassis_normal_speed_limit = 80; // 这里的单位可能需要根据实际调整
|
||||
}
|
||||
|
||||
// 功率分配 (防止超功率)
|
||||
static void Chassis_Power_Allocation(Chassis_Omni_t *chassis)
|
||||
{
|
||||
float scaling[4];
|
||||
float total_err = 0.0f;
|
||||
|
||||
// 计算总误差 (使用 Err 字段)
|
||||
for (int i = 0; i < 4; i++) {
|
||||
total_err += fabsf(chassis->pid_speed[i].Err);
|
||||
}
|
||||
|
||||
if (total_err > 1e-6f) { // 避免除零
|
||||
for (int i = 0; i < 4; i++) {
|
||||
scaling[i] = chassis->pid_speed[i].Err / total_err;
|
||||
}
|
||||
|
||||
// 限制输出
|
||||
for (int i = 0; i < 4; i++) {
|
||||
// 原代码: pidinstance[0].pos_out = abs_clip(..., abs(Scaling[i] * 50000))
|
||||
// 注意: 这里直接修改了 PID 的 Output，可能会影响下一次计算，但在C++原版中就是这样写的
|
||||
float limit = fabsf(scaling[i] * 50000.0f);
|
||||
chassis->pid_speed[i].Output = abs_clip(chassis->pid_speed[i].Output, limit);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Chassis_Omni_Update(Chassis_Omni_t *chassis)
|
||||
{
|
||||
if (chassis == NULL) return;
|
||||
|
||||
// 1. 获取电机速度并进行正运动学解算 (估计底盘当前速度)
|
||||
float motor_speeds[4];
|
||||
float real_speed[4]; // [0]=vx, [1]=vy, [2]=w
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
// 使用 speed_aps (度/秒)
|
||||
motor_speeds[i] = chassis->moto_chassis[i]->measure.speed_aps;
|
||||
}
|
||||
|
||||
// 逆结算部分 (原代码注释，实际是正解算：轮速 -> 体速)
|
||||
// 假设是X型全向轮/麦克纳姆轮布局
|
||||
real_speed[0] = (-motor_speeds[0] - motor_speeds[1] + motor_speeds[2] + motor_speeds[3]) / 4.0f;
|
||||
real_speed[1] = (-motor_speeds[0] + motor_speeds[1] - motor_speeds[2] + motor_speeds[3]) / 4.0f;
|
||||
real_speed[2] = (-motor_speeds[0] - motor_speeds[1] - motor_speeds[2] - motor_speeds[3]) / 4.0f;
|
||||
|
||||
// 将底盘体坐标系速度转换到之前的参考系 (可能是云台系或世界系，取决于 real_angle 的定义)
|
||||
float cos_a = cosf(chassis->cmd.real_angle);
|
||||
float sin_a = sinf(chassis->cmd.real_angle);
|
||||
|
||||
float speedx = real_speed[0] * cos_a - real_speed[1] * sin_a;
|
||||
float speedy = real_speed[0] * sin_a + real_speed[1] * cos_a;
|
||||
|
||||
float target_speed[3] = {0};
|
||||
|
||||
if (chassis->cmd.if_enable != 0)
|
||||
{
|
||||
// 2. 跟随PID计算
|
||||
chassis->offset_angle = -chassis->cmd.follow_angle;
|
||||
chassis->offset_speed = chassis->cmd.yaw_speed;
|
||||
|
||||
if (!chassis->cmd.if_free) {
|
||||
// 串级PID: 外环(角度) -> 内环(速度)
|
||||
// 外环目标: 0 (使 offset_angle 归零)
|
||||
float outer_out = PIDCalculate(&chassis->pid_follow_angle_outer, chassis->offset_angle, 0.0f);
|
||||
|
||||
// 内环目标: 外环输出
|
||||
// 内环反馈: offset_speed (yaw_speed)
|
||||
chassis->follow_increment = PIDCalculate(&chassis->pid_follow_angle_inner, chassis->offset_speed, outer_out);
|
||||
} else {
|
||||
chassis->follow_increment = 0.0f;
|
||||
// 清空PID积分等状态
|
||||
chassis->pid_follow_angle_outer.Output = 0;
|
||||
chassis->pid_follow_angle_inner.Output = 0;
|
||||
}
|
||||
|
||||
// 3. 底盘速度闭环控制 (P控制)
|
||||
// 这里的 5.5 是速度环增益，计算出的是"加速度"或"力"的需求
|
||||
target_speed[0] = (chassis->cmd.speed[0] - speedx) * 5.5f;
|
||||
target_speed[1] = (chassis->cmd.speed[1] - speedy) * 5.5f;
|
||||
|
||||
// 旋转轴控制
|
||||
// 如果没有指令输入，则使用 real_speed 差值进行阻尼控制？
|
||||
// 原代码逻辑: speed[2] = (cmd - real) * 5.5
|
||||
target_speed[2] = (chassis->cmd.speed[2] - real_speed[2]) * 5.5f;
|
||||
|
||||
if (chassis->cmd.speed[2] == 0.0f) // 如果没有旋转指令
|
||||
{
|
||||
// 叠加跟随PID输出，并减去当前旋转速度 (阻尼)
|
||||
target_speed[2] = (chassis->follow_increment - real_speed[2] - real_speed[2]) * 5.5f;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 如果有手动旋转指令，清除跟随PID积分
|
||||
chassis->pid_follow_angle_outer.Output = 0;
|
||||
chassis->pid_follow_angle_inner.Output = 0;
|
||||
}
|
||||
|
||||
// 4. 逆运动学解算 (体速 -> 轮速)
|
||||
// 引入了旋转补偿: real_angle - 0.002 * real_speed[2]
|
||||
float corrected_angle = chassis->cmd.real_angle - 0.002f * real_speed[2];
|
||||
float sin_ca = sinf(corrected_angle);
|
||||
float cos_ca = cosf(corrected_angle);
|
||||
|
||||
// 转换回电机解算所需的 x, y 分量
|
||||
float y = -(target_speed[0] * sinf(chassis->cmd.real_angle) - target_speed[1] * cos_ca);
|
||||
float x = (target_speed[0] * cosf(chassis->cmd.real_angle) + target_speed[1] * sin_ca);
|
||||
|
||||
// 5. 电机PID控制与输出
|
||||
float wheel_targets[4];
|
||||
wheel_targets[0] = (-x - y) - target_speed[2];
|
||||
wheel_targets[1] = (-x + y) - target_speed[2];
|
||||
wheel_targets[2] = (x - y) - target_speed[2];
|
||||
wheel_targets[3] = (x + y) - target_speed[2];
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
// PIDCalculate(pid, measure, target) -> 这里的measure似乎被当作0处理？
|
||||
// 原C++代码: pid_chassis[i]->PID_handle(wheel_targets[i]);
|
||||
// PID_handle(target) 内部通常是 calculate(measure, target).
|
||||
// 但原代码中 PID 构造时传入了 &moto_chassis[i]->speed 地址。
|
||||
// 因此 C++ PID 类会自动读取 measure。
|
||||
// 在 C 中，我们需要手动传入 measure。
|
||||
|
||||
float output = PIDCalculate(&chassis->pid_speed[i], chassis->moto_chassis[i]->measure.speed_aps, wheel_targets[i]);
|
||||
// 设置电机输出 (注意：DJIMotorSetRef 设置的是目标值还是直接电流？)
|
||||
// 根据 dji_motor.h 注释: "可以将电机视为传递函数为1的设备...不需要关心底层的闭环"
|
||||
// 如果 DJIMotorSetRef 是设定速度闭环的目标，那么上面的 PID 是多余的吗？
|
||||
// 不，原代码 clearly 使用了 pid_chassis[i] 计算 send_data。
|
||||
// 这意味着 dji_motor 应该工作在 OPEN_LOOP 或 CURRENT_LOOP 模式，或者我们需要直接操作 current。
|
||||
// 假设我们这里计算的是电流值，因为 MaxOut 是 16000 (M3508电流范围)。
|
||||
// DJIMotorSetRef 通常用于设定内置闭环的目标。
|
||||
// 如果要发送电流，通常没有直接的 SetCurrent API，除非 Motor_Control_Setting_s 允许。
|
||||
// 为了保持移植性，我们假设 DJIMotorSetRef 能够处理这个输出，或者我们需要修改 dji_motor 模块。
|
||||
// 这里我们假设 DJIMotorSetRef 在电流模式下工作。
|
||||
DJIMotorSetRef(chassis->moto_chassis[i], output);
|
||||
}
|
||||
|
||||
// 6. 功率限制
|
||||
Chassis_Power_Allocation(chassis);
|
||||
// 如果 Chassis_Power_Allocation 修改了 PID Output，我们需要重新 SetRef 吗？
|
||||
// 原代码直接修改了 pos_out，这在下一次计算时生效，或者如果 PID 类直接返回 pos_out 给 send_data。
|
||||
// C++代码: moto_chassis[i]->send_data = PID_handle(...); Chassis_Power_Allocation();
|
||||
// Power_Allocation 修改了 pid instance 的 pos_out。
|
||||
// 这意味着当前的 send_data 并没有被 Power_Allocation 修正！
|
||||
// 修正逻辑应该是先计算 PID，再分配，再发送。
|
||||
// 但为了忠实还原原代码逻辑，我们保持顺序。
|
||||
// (注：原代码逻辑可能存在缺陷，分配后的功率限制在下一帧才通过积分项或直接赋值生效？
|
||||
// 或者 moto_chassis->send_data 是个指针引用？不，它是值。
|
||||
// 如果原代码 Allocation 在赋值给 send_data 之后调用，那么它只影响了 PID 内部状态，不影响当前帧输出。)
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < 4; i++) {
|
||||
DJIMotorStop(chassis->moto_chassis[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Chassis_Omni_PowerControl(Chassis_Omni_t *chassis, Chassis_Power_Info_s *power_info, Chassis_Ctrl_Cmd_s *raw_cmd)
|
||||
{
|
||||
if (chassis == NULL || power_info == NULL || raw_cmd == NULL) return;
|
||||
|
||||
// 复制原始指令到内部 cmd (默认)
|
||||
chassis->cmd = *raw_cmd;
|
||||
|
||||
// 简单的功率策略实现 (参考 Chassis_OmniWheel_Crtl::powerControl)
|
||||
float speed_scaling = 1.0f;
|
||||
|
||||
if (power_info->remain_energy >= chassis->power_config.super_power_health)
|
||||
{
|
||||
speed_scaling = 1.0f; // 正常模式
|
||||
}
|
||||
else if (power_info->remain_energy >= chassis->power_config.super_power_week)
|
||||
{
|
||||
// 线性降额: energy + 10 ? 原代码: tired = energy + 10
|
||||
float tired = power_info->remain_energy + 10.0f;
|
||||
speed_scaling = tired * 0.01f; // 归一化
|
||||
}
|
||||
else
|
||||
{
|
||||
speed_scaling = 0.3f; // 低电量模式
|
||||
}
|
||||
|
||||
// 应用缩放系数 (原代码还有 200 * 1.5/1.8 的系数，这里假设 raw_cmd 已经是归一化值，只做缩放)
|
||||
// 原代码: data_to_chassis.speed[...] = data_from_FSM.speed[...] * 200 * ...
|
||||
// 这里我们只做相对缩放，保留原始比例
|
||||
chassis->cmd.speed[0] *= speed_scaling;
|
||||
chassis->cmd.speed[1] *= speed_scaling;
|
||||
chassis->cmd.speed[2] *= speed_scaling;
|
||||
}
|
||||
@@ -1,8 +1,81 @@
|
||||
//
|
||||
// Created by esqwt on 2026/3/2.
|
||||
//
|
||||
#ifndef CHASSIS_OMNI_H
|
||||
#define CHASSIS_OMNI_H
|
||||
|
||||
#ifndef TRONONEH7_SCAFFOLD_CHASSIS_OMNI_H
|
||||
#define TRONONEH7_SCAFFOLD_CHASSIS_OMNI_H
|
||||
#include "stdint.h"
|
||||
#include "dji_motor.h"
|
||||
#include "pid.h"
|
||||
|
||||
#endif // TRONONEH7_SCAFFOLD_CHASSIS_OMNI_H
|
||||
// 定义底盘控制命令结构体 (由于chassis_ctrl.h为空，在此定义以适配逻辑)
|
||||
typedef struct
|
||||
{
|
||||
float speed[3]; // x, y, z (旋转) 速度设定值
|
||||
float follow_angle; // 跟随角度 (底盘与云台夹角)
|
||||
float yaw_speed; // 当前Yaw轴角速度 (作为前馈或反馈)
|
||||
float real_angle; // 底盘当前实际角度 (用于坐标系转换)
|
||||
uint8_t if_enable; // 底盘使能标志
|
||||
uint8_t if_free; // 底盘自由模式标志 (不跟随)
|
||||
} Chassis_Ctrl_Cmd_s;
|
||||
|
||||
// 定义功率控制所需的外部数据结构
|
||||
typedef struct
|
||||
{
|
||||
uint16_t chassis_power_limit; // 来自裁判系统的功率限制
|
||||
float remain_energy; // 来自超级电容的剩余能量
|
||||
uint16_t chassis_power_buffer; // 缓冲能量 (可选)
|
||||
} Chassis_Power_Info_s;
|
||||
|
||||
// 全向轮底盘对象结构体
|
||||
typedef struct
|
||||
{
|
||||
// 电机实例指针 (LF, RF, LB, RB)
|
||||
DJIMotorInstance *moto_chassis[4];
|
||||
|
||||
// 速度环PID实例 (每个轮子一个)
|
||||
PIDInstance pid_speed[4];
|
||||
|
||||
// 跟随环串级PID实例
|
||||
PIDInstance pid_follow_angle_outer; // 外环 (角度)
|
||||
PIDInstance pid_follow_angle_inner; // 内环 (角速度)
|
||||
|
||||
// 控制数据
|
||||
Chassis_Ctrl_Cmd_s cmd;
|
||||
|
||||
// 内部计算状态变量
|
||||
float offset_angle;
|
||||
float offset_speed;
|
||||
float follow_increment;
|
||||
|
||||
// 功率控制参数
|
||||
struct {
|
||||
float super_power_health;
|
||||
float super_power_week;
|
||||
uint16_t chassis_normal_speed_limit;
|
||||
} power_config;
|
||||
|
||||
} Chassis_Omni_t;
|
||||
|
||||
/**
|
||||
* @brief 初始化全向轮底盘对象
|
||||
* @param chassis 底盘对象指针
|
||||
* @param lf 左前电机指针
|
||||
* @param rf 右前电机指针
|
||||
* @param lb 左后电机指针
|
||||
* @param rb 右后电机指针
|
||||
*/
|
||||
void Chassis_Omni_Init(Chassis_Omni_t *chassis, DJIMotorInstance *lf, DJIMotorInstance *rf, DJIMotorInstance *lb, DJIMotorInstance *rb);
|
||||
|
||||
/**
|
||||
* @brief 底盘控制任务函数，建议在RTOS任务中周期调用
|
||||
* @param chassis 底盘对象指针
|
||||
*/
|
||||
void Chassis_Omni_Update(Chassis_Omni_t *chassis);
|
||||
|
||||
/**
|
||||
* @brief 功率控制逻辑，根据裁判系统和超电状态限制目标速度
|
||||
* @param chassis 底盘对象指针
|
||||
* @param power_info 功率状态信息
|
||||
* @param raw_cmd 原始控制命令 (通常来自上层FSM)
|
||||
*/
|
||||
void Chassis_Omni_PowerControl(Chassis_Omni_t *chassis, Chassis_Power_Info_s *power_info, Chassis_Ctrl_Cmd_s *raw_cmd);
|
||||
|
||||
#endif // CHASSIS_OMNI_H
|
||||
Reference in New Issue
Block a user