底盘能进行前后左右运动

Core/Src/chassis_control.c

@@ -1,54 +1,127 @@
 #include "chassis_control.h"
 #include "CAN_receive.h"
+#include "remote_control.h"
+#include <math.h>
+#include <string.h>
 
-static pid_t pid_motor1;
-static pid_t pid_motor2;
-static pid_t pid_motor3;
-static pid_t pid_motor4;
+#define PI 3.1415926535f
 
-static int16_t target_speed_m1;
-static int16_t target_speed_m2;
-static int16_t target_speed_m3;
-static int16_t target_speed_m4;
+static pid_t pid_motor[4];        // 四个底盘电机的PID控制器
+static int16_t target_speed[4];  // 四个轮子的目标转速(rpm)
+static Chassis_Speed chassis_speed;          // 底盘目标速度(vx,vy,vw)
+static ChassisMode chassis_mode = CHASSIS_NORMAL; // 底盘当前模式
+
+static void omni_calc(Chassis_Speed *speed, int16_t *out_speed);
+static void absolute_cal(Chassis_Speed *absolute_speed, float angle);
 
 /**
  * @brief 初始化底盘电机的PID
- *        从左到右依次是：PID作用模块，KP,KI,KD,电机最大输出，电机最大限制输出
  */
 void chassis_control_init(void)
 {
-    pid_init(&pid_motor1, 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);//底盘电机1
-    pid_init(&pid_motor2, 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);//底盘电机2
-    pid_init(&pid_motor3, 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);//底盘电机3
-    pid_init(&pid_motor4, 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);//底盘电机4
+    for (int i = 0; i < 4; i++)
+    {
+        pid_init(&pid_motor[i], 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);
+    }
+    memset(target_speed, 0, sizeof(target_speed));
+    memset(&chassis_speed, 0, sizeof(chassis_speed));
 }
 
 /**
- * @brief 控制底盘电机的转速
+ * @brief 设置底盘目标速度（vx, vy, vw）
+ */
+void chassis_set_target_speed(const Chassis_Speed *speed)
+{
+    if (speed == NULL) return;
+    chassis_speed.vx = speed->vx;
+    chassis_speed.vy = speed->vy;
+    chassis_speed.vw = speed->vw;
+}
+
+/**
+ * @brief 设置底盘模式
+ */
+void chassis_set_mode(ChassisMode mode)
+{
+    chassis_mode = mode;
+}
+
+/**
+ * @brief 底盘控制主循环：全向轮运动学解算 + PID + CAN发送
  */
 void chassis_control_task(void)
 {
-    const motor_measure_t *m1 = get_chassis_motor_measure_point(0);
-    const motor_measure_t *m2 = get_chassis_motor_measure_point(1);
-    const motor_measure_t *m3 = get_chassis_motor_measure_point(2);
-    const motor_measure_t *m4 = get_chassis_motor_measure_point(3);
+    // 遥控器掉线检测：超过100ms无信号，停止CAN发送，电机随惯性停下
+    if (!is_rc_online())
+    {
+        for (int i = 0; i < 4; i++)
+        {
+            pid_motor[i].integral = 0;     // 清空PID积分，防止重新上线时积分饱和
+            pid_motor[i].last_error = 0;   // 清空上次误差
+        }
+        memset(target_speed, 0, sizeof(target_speed));  // 目标转速清零
+        return;  // 不发送CAN信号，电机随惯性停下
+    }
+    const motor_measure_t *m[4];
+    m[0] = get_chassis_motor_measure_point(0);
+    m[1] = get_chassis_motor_measure_point(1);
+    m[2] = get_chassis_motor_measure_point(2);
+    m[3] = get_chassis_motor_measure_point(3);
 
-    //pid运算
-    int16_t cur1 = (int16_t)pid_calc(&pid_motor1, target_speed_m1, m1->speed_rpm);
-    int16_t cur2 = (int16_t)pid_calc(&pid_motor2, target_speed_m2, m2->speed_rpm);
-    int16_t cur3 = (int16_t)pid_calc(&pid_motor3, target_speed_m3, m3->speed_rpm);
-    int16_t cur4 = (int16_t)pid_calc(&pid_motor4, target_speed_m4, m4->speed_rpm);
+    absolute_cal(&chassis_speed, 0);
 
-    CAN_cmd_chassis(cur1, cur2, cur3, cur4);
+    int16_t cur[4];
+    for (int i = 0; i < 4; i++)
+    {
+        cur[i] = (int16_t)pid_calc(&pid_motor[i], (float)target_speed[i], (float)m[i]->speed_rpm);
+    }
+
+    CAN_cmd_chassis(cur[0], cur[1], cur[2], cur[3]);
 }
 
 /**
- * @brief 设置电机转速的电流值，范围（0~16384）
+ * @brief 全向轮运动学解算：将 vx,vy,vw 转换为四个轮子的转速
+ *        轮子编号：0=左, 1=前, 2=右, 3=后
  */
-void chassis_set_speed(int16_t m1, int16_t m2, int16_t m3, int16_t m4)
+static void omni_calc(Chassis_Speed *speed, int16_t *out_speed)
 {
-    target_speed_m1 = m1;
-    target_speed_m2 = m2;
-    target_speed_m3 = m3;
-    target_speed_m4 = m4;
+    float wheel_rpm_ratio;  // 速度(m/s)到转速(rpm)的转换系数
+    float L;                // 底盘中心到轮子的距离 = LENGTH_A + LENGTH_B
+    int16_t wheel_rpm[4];
+
+    // 60 / 轮周长 * 减速比 = 转速转换系数 (m/s → rpm)
+    // 例如：vx=1m/s → 轮子60/0.152/19=7500rpm(电机端)
+    wheel_rpm_ratio = 60.0f / WHEEL_PERIMETER * CHASSIS_DECELE_RATIO;
+    L = LENGTH_A + LENGTH_B;
+
+    // 轮子编号：基于用户观察修正
+    // M1(0x201)=左前, M2(0x202)=左后, M3(0x203)=右前, M4(0x204)=右后
+    // 物理层vy符号：+, -, -, + 是X型麦轮的标准平移公式（右侧电机反向安装，代码层vy: +, -, +, -）
+    // 四个轮子对角配对：左前+右后同向，右前+左后同向，两组反向
+    wheel_rpm[0] = (int16_t)(( speed->vx + speed->vy + speed->vw * L) * wheel_rpm_ratio); // 左前轮
+    wheel_rpm[1] = (int16_t)(( speed->vx - speed->vy + speed->vw * L) * wheel_rpm_ratio); // 左后轮
+    wheel_rpm[2] = (int16_t)((-speed->vx + speed->vy + speed->vw * L) * wheel_rpm_ratio); // 右前轮
+    wheel_rpm[3] = (int16_t)((-speed->vx - speed->vy + speed->vw * L) * wheel_rpm_ratio); // 右后轮
+
+    memcpy(out_speed, wheel_rpm, 4 * sizeof(int16_t));
+}
+
+/**
+ * @brief 坐标系变换：将遥控器坐标系的速度转换到底盘坐标系
+ * @param absolute_speed 遥控器输入的目标速度
+ * @param angle 云台相对于底盘的角度（度）
+ */
+static void absolute_cal(Chassis_Speed *absolute_speed, float angle)
+{
+    float angle_hd;          // 角度转弧度
+    Chassis_Speed temp_speed; // 旋转后的临时速度
+
+    angle_hd = angle * PI / 180.0f;  // 角度转弧度
+
+    // 坐标系旋转矩阵：将遥控器坐标系速度旋转到当前底盘坐标系
+    temp_speed.vw = absolute_speed->vw;
+    temp_speed.vx = absolute_speed->vx * cosf(angle_hd) - absolute_speed->vy * sinf(angle_hd);
+    temp_speed.vy = absolute_speed->vx * sinf(angle_hd) + absolute_speed->vy * cosf(angle_hd);
+
+    omni_calc(&temp_speed, target_speed);  // 全向轮解算，结果存入target_speed
 }