diff --git a/User_Code/application/chassis_app/half_steer/chassis_half_steer.c b/User_Code/application/chassis_app/half_steer/chassis_half_steer.c index 4311ebe..6ad72c2 100644 --- a/User_Code/application/chassis_app/half_steer/chassis_half_steer.c +++ b/User_Code/application/chassis_app/half_steer/chassis_half_steer.c @@ -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 + +// 宏定义 (根据实际机械结构调整) +#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; + } +} \ No newline at end of file diff --git a/User_Code/application/chassis_app/half_steer/chassis_half_steer.h b/User_Code/application/chassis_app/half_steer/chassis_half_steer.h index c29e616..d8d4a88 100644 --- a/User_Code/application/chassis_app/half_steer/chassis_half_steer.h +++ b/User_Code/application/chassis_app/half_steer/chassis_half_steer.h @@ -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 \ No newline at end of file diff --git a/User_Code/application/chassis_app/omni/chassis_omni.c b/User_Code/application/chassis_app/omni/chassis_omni.c index 2a8ad1c..cb3d5e3 100644 --- a/User_Code/application/chassis_app/omni/chassis_omni.c +++ b/User_Code/application/chassis_app/omni/chassis_omni.c @@ -1,5 +1,273 @@ -// -// Created by esqwt on 2026/3/2. -// - #include "chassis_omni.h" +#include +#include +#include + +#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; +} \ No newline at end of file diff --git a/User_Code/application/chassis_app/omni/chassis_omni.h b/User_Code/application/chassis_app/omni/chassis_omni.h index 260fe00..2e812d3 100644 --- a/User_Code/application/chassis_app/omni/chassis_omni.h +++ b/User_Code/application/chassis_app/omni/chassis_omni.h @@ -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 \ No newline at end of file diff --git a/User_Code/user_task/half_steering/halfsteering_def.h b/User_Code/user_task/half_steering/halfsteering_def.h new file mode 100644 index 0000000..ca9cc79 --- /dev/null +++ b/User_Code/user_task/half_steering/halfsteering_def.h @@ -0,0 +1,8 @@ +// +// Created by esqwt on 2026/3/3. +// + +#ifndef TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H +#define TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H + +#endif // TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H diff --git a/User_Code/user_task/half_steering/halfsteering_tasks.c b/User_Code/user_task/half_steering/halfsteering_tasks.c new file mode 100644 index 0000000..a205819 --- /dev/null +++ b/User_Code/user_task/half_steering/halfsteering_tasks.c @@ -0,0 +1,3 @@ +// +// Created by esqwt on 2026/3/3. +//