supercap&PowerCtrl

application/chassis/chassis.c

@@ -26,6 +26,17 @@
 #include "referee_UI.h"
 #include "arm_math.h"
 
+// =================【功率模型参数】=================
+// ⚠️警告：以下参数原本是群星战队为 DJI M3508 电机拟合的。
+// 你们的驱动电机是 LK9015，它的扭矩常数、内阻、空载损耗和 M3508 不同！
+// 建议后期针对 LK9015 重新拟合这四个参数以达到完美的控制效果。
+#define TOQUE_COEFF 1.99688994e-6f
+#define K1 1.23e-07f
+#define K2 1.453e-07f
+#define CONSTANT_POWER 4.081f
+#define WARRING_ENERGY_BUFFER 50.0f
+// ==================================================
+
 /* 根据robot_def.h中的macro自动计算的参数 */
 #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
@@ -179,7 +190,7 @@ void ChassisInit()
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
 
     // （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整
-    SuperCapSetPowerOffset(cap, 5);
+    // SuperCapSetPowerOffset(cap, 5);
 
     // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
@@ -337,19 +348,102 @@ static void SteeringWheelCalculate()
  * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
  *
  */
+/**
+ * @brief 根据裁判系统和电容剩余容量对输出进行限制，并应用 WJTLU 功率模型
+ */
 static void LimitChassisOutput()
 {
-    // 功率限制待添加
-    // referee_data->PowerHeatData.chassis_power;
-    // referee_data->PowerHeatData.chassis_power_buffer;
+    float chassis_max_power = 0;
+    float chassis_set_power = 90.0f; // 设定的期望功率，可根据需求修改
 
-    // 完成功率限制后进行电机参考输入设定
+    // 获取裁判系统数据
+    uint16_t referee_power_limit = referee_data->GameRobotState.chassis_power_limit;
+    float energy_buffer = referee_data->PowerHeatData.chassis_power_buffer;
 
-    // LKMotorSetRef( motor_lf , vt_lf );
-    // LKMotorSetRef( motor_rb , vt_rb );
+    // 1. ============ 超级电容与裁判系统功率统筹 ============
+    if (SuperCapIsOnline(cap)) {
+        SuperCap_Msg_s cap_msg = SuperCapGet(cap);
+        if (cap_msg.ready == READY) {
+            // 超电正常可用，动态按比例缩放允许功率
+            if (cap_msg.chassis_power > chassis_set_power) {
+                chassis_max_power = chassis_set_power * (chassis_set_power / (float)cap_msg.chassis_power);
+            } else {
+                chassis_max_power = chassis_set_power;
+            }
+        } else {
+            // 超电保护中，使用裁判系统限制
+            if (cap_msg.chassis_power > referee_power_limit) {
+                chassis_max_power = referee_power_limit * ((float)referee_power_limit / (float)cap_msg.chassis_power);
+            } else {
+                chassis_max_power = referee_power_limit;
+            }
+        }
 
-    LKMotorSetRef( motor_rf , vt_rf );
-    LKMotorSetRef( motor_lb , vt_lb );
+        // 缓冲能量补偿逻辑
+        uint8_t power_offset = 0;
+        if (energy_buffer < WARRING_ENERGY_BUFFER) {
+            power_offset = (uint8_t)((WARRING_ENERGY_BUFFER - energy_buffer) * 0.2f);
+        }
+        SuperCapSetPowerOffset(cap, power_offset);
+        chassis_max_power -= power_offset;
+    } else {
+        // 超电离线时逻辑
+        chassis_max_power = (energy_buffer < WARRING_ENERGY_BUFFER) ?
+                            (referee_power_limit * energy_buffer * 0.02f) : referee_power_limit;
+    }
+
+    // 2. ============ 基于 LK9015 的 WJTLU 二次功率模型限流 ============
+    float initial_total_power = 0;
+    float initial_motor_power[2];
+    LKMotorInstance* motors[2] = {motor_rf, motor_lb}; // 注意舵轮底盘只有两个驱动轮
+    float current_pid_out[2];
+
+    for (int i = 0; i < 2; i++) {
+        // 修正 1：LK 电机驱动中，speed_PID 是实例的直接成员
+        current_pid_out[i] = motors[i]->speed_PID.Output;
+
+        // 修正 2：LK 的速度单位是 rad/s，WJTLU模型要求 RPM，进行单位转换 (rad/s * 30/pi ≈ RPM)
+        float rpm = motors[i]->measure.speed_rads * 9.549296f;
+
+        initial_motor_power[i] = current_pid_out[i] * TOQUE_COEFF * rpm +
+                                 K1 * current_pid_out[i] * current_pid_out[i] +
+                                 K2 * rpm * rpm + CONSTANT_POWER;
+
+        if (initial_motor_power[i] > 0) initial_total_power += initial_motor_power[i];
+    }
+
+    // 3. 动态修改电机 PID 的 MaxOut 属性
+    if (initial_total_power > chassis_max_power && initial_total_power > 0) {
+        float scale = chassis_max_power / initial_total_power;
+        for (int i = 0; i < 2; i++) {
+            float scaled_p = initial_motor_power[i] * scale;
+            if (scaled_p <= CONSTANT_POWER) {
+                motors[i]->speed_PID.MaxOut = 0; // 该轮分配不到功率
+                continue;
+            }
+
+            // 同步修正单位
+            float rpm = motors[i]->measure.speed_rads * 9.549296f;
+            float b = TOQUE_COEFF * rpm;
+            float c = K2 * rpm * rpm - scaled_p + CONSTANT_POWER;
+            float delta = b * b - 4 * K1 * c;
+
+            if (delta >= 0) {
+                // 解方程获得当前允许的最大电流
+                float out_limit = (-b + sqrtf(delta)) / (2.0f * K1);
+                // 修正 3：动态限制 LK 电机 speed_PID 的最大输出（LK9015 默认最大输出为 12000）
+                motors[i]->speed_PID.MaxOut = fminf(out_limit, 12000.0f);
+            }
+        }
+    } else {
+        // 功率充裕时，恢复电机的默认最大输出
+        motor_rf->speed_PID.MaxOut = 12000.0f;
+        motor_lb->speed_PID.MaxOut = 12000.0f;
+    }
+
+    // 4. 下发目标速度（限流操作已通过改变 MaxOut 动态生效）
+    LKMotorSetRef(motor_rf, vt_rf);
+    LKMotorSetRef(motor_lb, vt_lb);
 }
 
 /**
@@ -411,7 +505,8 @@ void ChassisTask() {
             chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/70.0f;
             break;
         case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-            chassis_cmd_recv.wz = -0.35;//实验版
+            // chassis_cmd_recv.wz = 0.75;
+            chassis_cmd_recv.wz = -0.75;//实验版
             break;
         default:
             break;
@@ -424,8 +519,8 @@ void ChassisTask() {
     sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
     // chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
     // chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
-    chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;//实验版
-     chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
+    chassis_vy = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;//实验版
+     chassis_vx = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
 
     // 根据控制模式进行正运动学解算,计算底盘输出
     SteeringWheelCalculate();
@@ -444,6 +539,16 @@ void ChassisTask() {
     // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
     // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
 
+    // ================== 发送超级电容控制指令 ==================
+    // 每帧向超电控制板发送最新状态
+    SuperCap_TxMsg_s cap_tx = {
+        .Enable = 1,
+        .Reserved1 = 0, // 可根据需求动态关断充电
+        .Powerlimit = referee_data->GameRobotState.chassis_power_limit,
+        .Reserved2 = 0 // PLUS版本忽略此位
+    };
+    SuperCapSend(cap, &cap_tx);
+
     // 推送反馈消息
 #ifdef ONE_BOARD
     PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);

application/cmd/robot_cmd.c

@@ -200,7 +200,7 @@ static void RemoteControlSet()
     // 云台参数,确定云台控制数据
     if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
     {
-        // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制/
+        // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
         // ...
     }
     // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
@@ -216,9 +216,9 @@ static void RemoteControlSet()
         gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
 
     // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
-    chassis_cmd_send.vx = (0.25f * (float)rc_data[TEMP].rc.rocker_r1); // 竖直方向
-    chassis_cmd_send.vy = (0.25f * (float)rc_data[TEMP].rc.rocker_r_); // 水平方向
-    //此处如果是右手系，那么x应该是前进方向即竖直方向
+    chassis_cmd_send.vx = (0.25f * (float)rc_data[TEMP].rc.rocker_r_); // _水平方向
+    chassis_cmd_send.vy = (0.25f * (float)rc_data[TEMP].rc.rocker_r1); // 1数值方向 .
+
     // // 发射参数
     // if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
     //     ;                                            // 弹舱舵机控制,待添加servo_motor模块,开启

modules/super_cap/super_cap.c

@@ -5,6 +5,7 @@
 #include "super_cap.h"
 #include "memory.h"
 #include "stdlib.h"
+#include "bsp_dwt.h" // 【新增】引入获取时间的头文件，或者是 "main.h" 用 HAL_GetTick()
 
 static SuperCapInstance *super_cap_instance = NULL;
 
@@ -15,6 +16,9 @@ static void SuperCapRxCallback(CANInstance *_instance)
     rxbuff = _instance->rx_buff;
     Msg = &super_cap_instance->cap_msg;
 
+    // 【新增】更新最后一次收到消息的时间戳
+    super_cap_instance->last_update_tick = DWT_GetTimeline_ms(); // 如果没有DWT，用 HAL_GetTick() 替换
+
     // 按照新协议解析接收数据
     Msg->ready = (SuperCapReadyEnum)rxbuff[0];
     Msg->state = (SuperCapStateEnum)rxbuff[1];
@@ -24,6 +28,18 @@ static void SuperCapRxCallback(CANInstance *_instance)
     Msg->bat_power = rxbuff[5];
 }
 
+// 【新增】在线判断函数
+uint8_t SuperCapIsOnline(SuperCapInstance *instance)
+{
+    if (instance == NULL) return 0;
+
+    // 如果当前时间减去上次接收时间超过 1000ms (1秒)，则认为离线
+    if (DWT_GetTimeline_ms() - instance->last_update_tick > 2000) {
+        return 0; // 离线
+    }
+    return 1; // 在线
+}
+
 SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
 {
     super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance));
@@ -40,9 +56,9 @@ void SuperCapSend(SuperCapInstance *instance, SuperCap_TxMsg_s *tx_data)
 
     // 按照新协议装载发送数据
     txbuff[0] = tx_data->Enable;
-    txbuff[1] = tx_data->Charge;
+    txbuff[1] = tx_data->Reserved1;
     txbuff[2] = tx_data->Powerlimit - instance->power_offset; // 计算功率补偿
-    txbuff[3] = tx_data->ChargePower;
+    txbuff[3] = tx_data->Reserved2;
 
     CANTransmit(instance->can_ins, 1);
 }

modules/super_cap/super_cap.h

@@ -44,9 +44,9 @@ typedef struct
 /* 需要发送给超级电容的控制信息结构体 */
 typedef struct {
     uint8_t Enable;      // 超级电容使能。1使能，0失能
-    uint8_t Charge;      // 超级电容充电。1充电，0放电
+    uint8_t Reserved1;      // Reserved 超级电容充电。1充电，0放电
     uint8_t Powerlimit;  // 裁判系统功率限制
-    uint8_t ChargePower; // 超级电容充电功率
+    uint8_t Reserved2; // Reserved 超级电容充电功率
 } SuperCap_TxMsg_s;
 #pragma pack()
 
@@ -56,6 +56,7 @@ typedef struct
     CANInstance *can_ins;    // CAN实例
     SuperCap_Msg_s cap_msg;  // 超级电容接收信息
     uint8_t power_offset;    // 功率补偿值
+    uint32_t last_update_tick; // 【新增】上次收到数据的时间戳
 } SuperCapInstance;
 
 /* 超级电容初始化配置 */
@@ -69,6 +70,9 @@ typedef struct
  */
 SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
 
+// 【新增】函数声明
+uint8_t SuperCapIsOnline(SuperCapInstance *instance);
+
 /**
  * @brief 发送超级电容控制信息
  */