电流环控制，并实现简单的补偿控制，还差参数具体化。

2026-10-16 00:54:52 +08:00
parent 67eea77789
commit 370d4519b5
20 changed files with 477 additions and 163 deletions
@@ -85,8 +85,8 @@ static int8_t linkcheck_first_offline(void)
 {
    uint32_t now = HAL_GetTick();
 #if defined(ROBOT_TYPE_GIMBAL)
-    /* 设备序号:0 摩擦轮L(0x201) 1 摩擦轮R(0x202) 2 Pitch(0x206)
-     *           3 Yaw(0x205) 4 拨弹(0x207,安装时) 5 底盘回传(0x019) */
+    /* 设备序号:0 摩擦轮L(CAN1 0x201) 1 摩擦轮R(CAN1 0x202) 2 Pitch(CAN1 0x206)
+     *           3 Yaw(CAN2 0x205) 4 拨弹(CAN2 0x201,安装时) 5 底盘回传(0x019) */
    if (LINK_OFFLINE(motor_friction_l.fdb.last_online_tick))
        return 0;
    if (LINK_OFFLINE(motor_friction_r.fdb.last_online_tick))
@@ -97,8 +97,8 @@ void robot_cmd_task(void const * argument)

            if (!keyboard_online)
            {
-                /* 遥控器:左摇杆,输出与摇杆偏量成比例(legacy 为过死区即全速,
-                 * 体感过灵敏),限位门控与方向保持 legacy */
+                /* 遥控器:左摇杆向上(>1050)为抬头(pitch_delta减小,pos向2260减小),门控须 pos > UPPER_LIMIT(2260);
+                 *         左摇杆向下(<1000)为低头(pitch_delta增大,pos向3950增大),门控须 pos < LOWER_LIMIT(3950) */
                if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
                    pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
                                   ((float)dr16.rc_l_UD - 1024.0f) / 660.0f;
@@ -113,13 +113,13 @@ void robot_cmd_task(void const * argument)
            }
            else
            {
-                /* 键鼠:同改为比例输出 */
-                if (keyboard_data.mouse_du < -8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
+                /* 键鼠:鼠标前推(>8)为抬头(pitch_delta减小),门控须 pos > UPPER_LIMIT;鼠标后拉(<-8)为低头(pitch_delta增大),门控须 pos < LOWER_LIMIT */
+                if (keyboard_data.mouse_du > 8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
                    pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
-                                   (-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
-                else if (keyboard_data.mouse_du > 8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
-                    pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
                                   ((float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
+                else if (keyboard_data.mouse_du < -8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
+                    pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
+                                   (-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;

                if (keyboard_data.mouse_rl != 0)
                    yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE *
@@ -9,11 +9,11 @@
 #define ROBOT_PARAM_H

 /* ============================ 云台 ============================ */
-#define PITCH_GM6020_POSITION_LOWER_LIMIT   3990    // Pitch 轴编码器值下限(云台抬起方向)
-#define PITCH_GM6020_POSITION_UPPER_LIMIT   2220    // Pitch 轴编码器值上限(云台压低方向)
-#define PITCH_GM6020_ENCODER_ZERO           3245    // Pitch 轴编码器零位
-#define YAW_GM6020_ENCODER_ZERO             6804    // Yaw 轴编码器零位(机械正前方,0~8191)
-#define CHASSIS_SPIN_FEEDFORWARD_K          0.0725f  // 小陀螺解算时延前馈补偿系数(s)
+#define PITCH_GM6020_POSITION_UPPER_LIMIT   2260    // Pitch 轴抬起极限(最高位,编码器较小值;实测2203撞死限位,留余量设2260)
+#define PITCH_GM6020_POSITION_LOWER_LIMIT   3950    // Pitch 轴压低极限(最低位,编码器较大值;实测4234撞死限位,留余量设3950)
+#define PITCH_GM6020_ENCODER_ZERO           3200    // Pitch 轴编码器水平零位
+#define YAW_GM6020_ENCODER_ZERO             7730    // Yaw 轴编码器零位(机械正前方,0~8191)
+#define CHASSIS_SPIN_FEEDFORWARD_K          0.008f  // 小陀螺解算时延前馈补偿系数(s)

 #define GIMBAL_FRICTION_WHEEL_SPEED         5500.0f // 摩擦轮转速(RPM)
 #define GIMBAL_RAMMER_SPEED                 -5400.0f// 拨弹盘转速(RPM,负向发射)
@@ -31,7 +31,7 @@
 *    这是反复"疯转"的根源,已废弃;
 *  - 现在两环共用陀螺,整条链只有一个装配符号,由上电自检自动测定
 *    (GIMBAL_YAW_DETECT 见下),无需人工猜;
- *  - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电压。
+ *  - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电流。
 * 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */
 #define GIMBAL_YAW_ANGLE_KP                 20.0f   // ° -> °/s:10° 误差给 150°/s
 #define GIMBAL_YAW_ANGLE_KI                 0.0f
@@ -39,28 +39,35 @@
 #define GIMBAL_YAW_ANGLE_EK_DEADBAND        0.05f   // 误差死区(°),QEKF 噪声小可用窄死区
 #define GIMBAL_YAW_ANGLE_OUT_LIMIT          360.0f  // 目标角速度上限(°/s)

-#define GIMBAL_YAW_SPEED_KP                 85.0f   // °/s -> 电压:100°/s 误差给 6000(约 24% 满电压)
+#define GIMBAL_YAW_SPEED_KP                 85.0f   // °/s -> 电流:100°/s 误差给 6000(约 36% 满电流)
 #define GIMBAL_YAW_SPEED_KI                 0.0f    // 稳定后可回填 0.5~2 消速度稳态差
 #define GIMBAL_YAW_SPEED_KD                 0.0f    // 陀螺速率反馈噪声小,先不用 D
-#define GIMBAL_YAW_SPEED_OUT_LIMIT          25000.0f// GM6020 电压限幅
-#define GIMBAL_YAW_SPIN_FEEDFORWARD_K       24.0f   // 小陀螺底盘自旋解耦前馈系数(RPM->电压,通常15~35)
+#define GIMBAL_YAW_SPEED_OUT_LIMIT          16384.0f// GM6020 电流限幅(±16384 对应 ±3A)
+#define GIMBAL_YAW_SPIN_FEEDFORWARD_K       24.0f   // 小陀螺底盘自旋解耦前馈系数(RPM->电流)

-/* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电压脉冲,
+/* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电流脉冲,
 * 看陀螺 z 轴往哪边转——脉冲引起同向转动则符号 +1,反向则 -1。
 * 检测时云台会轻转几度,勿扶云台;失败(被按住/电机离线)时用 FALLBACK */
 #define GIMBAL_YAW_SIGN_AUTO                1       // 1=上电自动检测 0=直接用 FALLBACK
 #define GIMBAL_YAW_SIGN_FALLBACK            (1.0f)
-#define GIMBAL_YAW_DETECT_VOLTAGE           5000    // 检测脉冲电压(约 20% 满电压)
+#define GIMBAL_YAW_DETECT_CURRENT           3500    // 检测脉冲电流(约 21% 满电流 16384)
 #define GIMBAL_YAW_DETECT_RATE_MIN          20.0f   // 判定转动的陀螺速率阈值(°/s)

 /* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */
-#define GIMBAL_PITCH_ANGLE_EK_DEADBAND      0.3f    // 位置环误差死区(°)
-#define GIMBAL_PITCH_ANGLE_KP               15.0f
+#define GIMBAL_PITCH_ANGLE_EK_DEADBAND      0.05f   // 误差死区(°),收窄死区消除边缘阶跃
+#define GIMBAL_PITCH_ANGLE_KP               40.0f
 #define GIMBAL_PITCH_ANGLE_KI               0.0f
-#define GIMBAL_PITCH_ANGLE_KD               100.0f
-#define GIMBAL_PITCH_SPEED_KP               150.0f
-#define GIMBAL_PITCH_SPEED_KI               0.001f
-#define GIMBAL_PITCH_SPEED_KD               5.0f
+#define GIMBAL_PITCH_ANGLE_KD               0.0f    // 原100.0f是万级电流振颤主因!清零以消除剧烈振荡
+#define GIMBAL_PITCH_SPEED_KP               100.0f   // 速度/电流环Kp
+#define GIMBAL_PITCH_SPEED_KI               0.0f
+#define GIMBAL_PITCH_SPEED_KD               0.0f
+
+/* Pitch 重力拟合前馈参数: I_g = K_COS * cos(a) + K_SIN * sin(a) + K_CONST
+ * 其中 a = (raw_angle - ZERO_ECD) * 360 / 8192 (°) */
+#define GIMBAL_PITCH_GRAVITY_ZERO_ECD       3200.0f // 重力补偿拟合基准编码值
+#define GIMBAL_PITCH_GRAVITY_K_COS          2856.1f// cos 项系数
+#define GIMBAL_PITCH_GRAVITY_K_SIN          4340.3f// sin 项系数
+#define GIMBAL_PITCH_GRAVITY_K_CONST        2826.5f  // 常数偏置项

 #define GIMBAL_FRICTION_WHEEL_KP            16.0f
 #define GIMBAL_FRICTION_WHEEL_KI            0.0f
@@ -97,7 +104,7 @@
 #define CHASSIS_FOLLOW_KP                   0.4678f    // w 量纲/°:10° 偏差给 w=4
 #define CHASSIS_FOLLOW_KI                   0.0f    // 调试期 0,同旧教训(积分残留)
 #define CHASSIS_FOLLOW_KD                   24.452f
-#define CHASSIS_FOLLOW_OUT_LIMIT            4.5f    // 输出限幅(介于慢/快陀螺之间)
+#define CHASSIS_FOLLOW_OUT_LIMIT            5.5f    // 输出限幅(介于慢/快陀螺之间)
 #define CHASSIS_FOLLOW_DEADBAND             1.0f    // 误差死区(°)

 /* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向,
@@ -119,8 +126,8 @@
 #define CTOC_SLAVE_ID3   0x1A9  // 云台->底盘:云台世界系 yaw(float)+att_ready

 /* ============================ CAN ID ============================ */
-/* 云台板:CAN1 = Pitch GM6020(0x206)+摩擦轮 M3508(0x201/0x202)
- *        CAN2 = Yaw GM6020(0x205)+底盘回传(0x019)
+/* 云台板:CAN1 = Pitch GM6020(0x206,0x1FE)+摩擦轮 M3508(0x201/0x202,0x200)
+ *        CAN2 = Yaw GM6020(0x205,0x1FE)+拨弹 M2006(0x201,0x200)+底盘回传(0x019)
 * 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
 *        CAN2 = 云台数据(0x149/0x189/0x1A9) */

@@ -128,11 +135,10 @@
 #define DEBUG_SHOOT_WITHOUT_REFEREE  1  // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0
 #define GIMBAL_RAMMER_INSTALLED      0  // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定

-/* 1=调试模式:把 CAN 掉线警告音替换成循环播放的旋律(台架调试时避免连续滴滴声
- *   干扰)。红灯常亮与其它灯语完全不变,仍能看出是哪台设备掉线;比赛请改 0。
- * 旋律音符表见 modules/BUZZER/buzzer.c 的 buzzer_melody[] */
+/* 1=把 CAN 掉线警告音替换成循环播放的旋律《你》(RM动画片尾曲),报警时播放;
+ * 旋律音符表见 modules/dev/buzzer.c 的 buzzer_melody[] */
 #define DEBUG_BUZZER_MELODY          0
-#define BUZZER_MELODY_SPEED_DIV      1   // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍)   // 旋律降速分频(默认2:放慢一倍匹配原曲抒情节拍;设为3/4可进一步放慢)
+#define BUZZER_MELODY_SPEED_DIV      1   // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍)

 /* ============================ 掉线检测 ============================ */
 #define DEVICE_OFFLINE_TIME_MS 100   // CAN 设备/裁判系统掉线判定时间
@@ -6,9 +6,8 @@
 *       (无 legacy 的 +-1 圈限制,底盘小陀螺时 Yaw 可连续旋转),
 *       记录 last_online_tick 供掉线检测
 * 控制路径(任务上下文):
- *   dji_motor_send_current_low/high()  0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
- *   dji_motor_send_voltage_high()      0x2FF 电压帧(GM6020 ID5~7)
- *   注意 0x1FF 帧为 M3508/M2006 高位电流与 GM6020 低位电压共用,按槽位区分
+ *   dji_motor_send_current_low/high()        0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
+ *   dji_motor_send_gm6020_current_low/high() 0x1FE / 0x2FE GM6020 电流帧(+/-16384 = 3A)
 * ========================================================================== */
 #include "dji_motor.h"

@@ -20,7 +19,8 @@
 static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX];
 static uint8_t dji_motor_count = 0;

-static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
+static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
+static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);

 /* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */
 static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8])
@@ -71,16 +71,32 @@ void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_moto
    if (dji_motor_count < DJI_MOTOR_MAX)
    {
        dji_motor_list[dji_motor_count++] = motor;
-        can_rx_register(hcan, rx_id, dji_motor_rx_callback);
+        if (hcan == &hcan1)
+            can_rx_register(hcan, rx_id, dji_motor_can1_rx_callback);
+        else
+            can_rx_register(hcan, rx_id, dji_motor_can2_rx_callback);
    }
 }

-static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
+static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
 {
    (void)dlc;
    for (uint8_t i = 0; i < dji_motor_count; i++)
    {
-        if (dji_motor_list[i]->rx_id == std_id)
+        if (dji_motor_list[i]->hcan == &hcan1 && dji_motor_list[i]->rx_id == std_id)
+        {
+            dji_motor_parse(dji_motor_list[i], rx_data);
+            return;
+        }
+    }
+}
+
+static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
+{
+    (void)dlc;
+    for (uint8_t i = 0; i < dji_motor_count; i++)
+    {
+        if (dji_motor_list[i]->hcan == &hcan2 && dji_motor_list[i]->rx_id == std_id)
        {
            dji_motor_parse(dji_motor_list[i], rx_data);
            return;
@@ -119,8 +135,14 @@ void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t
    dji_motor_send_int16x4(hcan, 0x1FF, v);
 }

-void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7)
+void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4)
 {
-    const int16_t v[4] = {v5, v6, v7, 0};
-    dji_motor_send_int16x4(hcan, 0x2FF, v);
+    const int16_t v[4] = {i1, i2, i3, i4};
+    dji_motor_send_int16x4(hcan, 0x1FE, v);
+}
+
+void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8)
+{
+    const int16_t v[4] = {i5, i6, i7, i8};
+    dji_motor_send_int16x4(hcan, 0x2FE, v);
 }
@@ -5,13 +5,13 @@

 /* DJI 电机(M3508/M2006/GM6020)统一驱动
 * - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计)
- * - 控制:组装 0x200/0x1FF/0x2FF 控制帧,经 CAN_SendTxMessage 发送 */
+ * - 控制:组装 0x200/0x1FF/0x1FE/0x2FE 控制帧,经 CAN_SendTxMessage 发送 */

 typedef enum
 {
    DJI_MOTOR_3508,  // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A
    DJI_MOTOR_2006,  // 减速比 36:1, 转矩常数 0.18N·m/A
-    DJI_MOTOR_6020,  // 直驱, 电压控制
+    DJI_MOTOR_6020,  // 直驱, 电流控制(ID1~4 为 0x1FE 帧, ±16384 对应 ±3A)
 } dji_motor_type_e;

 typedef struct
@@ -20,7 +20,7 @@ typedef struct
    int64_t total_tick;        // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制)
    float rotor_angle_deg;     // 累计转子机械角(°)
    int16_t rotor_speed;       // 转子转速(RPM)
-    int16_t raw_torque;        // 转矩电流/转矩电压原始数据
+    int16_t raw_torque;        // 转矩电流原始数据
    float torque_current;      // 转矩电流(A)
    uint8_t temperature;       // 电机温度(℃)
    uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick)
@@ -39,11 +39,12 @@ extern void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, d

 /* ---- 控制帧发送 ----
 * M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A)
- * M3508/M2006 ID5~8:0x1FF 电流帧;GM6020 ID1~4:0x1FF 电压帧(±25000)
- * GM6020 ID5~7:0x2FF 电压帧 */
+ * M3508/M2006 ID5~8:0x1FF 电流帧(±16384)
+ * GM6020 电流控制:ID1~4 为 0x1FE 电流帧(±16384 对应 ±3A),ID5~8 为 0x2FE 电流帧(±16384) */
 extern void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
 extern void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
-extern void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7);
+extern void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
+extern void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);

 /* 通用 int16x4 发送(板间通讯等复用) */
 extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]);