fix: rotate

packet.md

@@ -106,6 +106,12 @@
 
 > ⚠️ 注意： X Y R 的取值范围为 -100 到 100，为有符号 8 位整数
 
+| 数据 | 正数 | 负数 |
+| --- | ---- | ---- |
+| X   | 左 | 右 |
+| Y   | 前 | 后 |
+| R   | 逆时针 | 顺时针 |
+
 包体 `00 07 24 X Y R FF`
 
 控制示例 `00 07 24 01 01 01 FF`

src/motor.cpp

@@ -1,6 +1,5 @@
 #include "motor.h"
 
-
 // 静态成员初始化
 MotorStatus MotorController::motorA = {{0, 0, 0}, false, false, 0};
 MotorStatus MotorController::motorB = {{0, 0, 0}, false, false, 0};
@@ -147,14 +146,14 @@ void MotorController::motorXYRControl(int8_t x, int8_t y, int8_t r)
   int16_t mappedR = map(r, -100, 100, -255, 255);
 
   // 麦克纳姆轮运动学方程
-  // 左前轮 = Y + X + R
+  // 左前轮(A) = Y - X - R
-  // 右前轮 = Y - X - R
+  // 右前轮(D) = Y + X - R
-  // 右后轮 = Y + X - R
+  // 右后轮(C) = Y - X + R
-  // 左后轮 = Y - X + R
+  // 左后轮(B) = Y + X + R
-  int16_t speedA = mappedY + mappedX + mappedR; // 左前轮
+  int16_t speedA = mappedY - mappedX - mappedR; // 左前轮
-  int16_t speedB = mappedY - mappedX - mappedR; // 右前轮
+  int16_t speedB = mappedY + mappedX + mappedR; // 左后轮
-  int16_t speedC = mappedY + mappedX - mappedR; // 右后轮
+  int16_t speedC = mappedY - mappedX + mappedR; // 右后轮
-  int16_t speedD = mappedY - mappedX + mappedR; // 左后轮
+  int16_t speedD = mappedY + mappedX - mappedR; // 右前轮
 
   // 找出最大速度的绝对值
   int16_t maxSpeed = max(max(abs(speedA), abs(speedB)),
@@ -182,13 +181,29 @@ MotorStatus MotorController::getMotorStatus(char motor)
   switch (motor)
   {
   case 'A':
-    return motorA;
+    return {
+        digitalRead(motorA.pin.in1),
+        digitalRead(motorA.pin.in2),
+        motorA.pwm,
+    };
   case 'B':
-    return motorB;
+    return {
+        digitalRead(motorB.pin.in1),
+        digitalRead(motorB.pin.in2),
+        motorB.pwm,
+    };
   case 'C':
-    return motorC;
+    return {
+        digitalRead(motorC.pin.in1),
+        digitalRead(motorC.pin.in2),
+        motorC.pwm,
+    };
   case 'D':
-    return motorD;
+    return {
+        digitalRead(motorD.pin.in1),
+        digitalRead(motorD.pin.in2),
+        motorD.pwm,
+    };
   default:
     return {false, false, 0};
   }