From 704ce34845aea1c08355b0b32ca9d4fe4a97e8d6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E7=8E=96=E5=8F=81?= <colour_93@furry.top>
Date: Fri, 27 Dec 2024 23:20:56 +0800
Subject: [PATCH] fix: rotate

---
 packet.md     |  6 ++++++
 src/motor.cpp | 41 ++++++++++++++++++++++++++++-------------
 2 files changed, 34 insertions(+), 13 deletions(-)

diff --git a/packet.md b/packet.md
index 60280b2..d8be855 100644
--- a/packet.md
+++ b/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`
diff --git a/src/motor.cpp b/src/motor.cpp
index 08e2e53..2991e2f 100644
--- a/src/motor.cpp
+++ b/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
-  // 右前轮 = Y - X - R
-  // 右后轮 = Y + X - R
-  // 左后轮 = Y - X + R
-  int16_t speedA = mappedY + mappedX + mappedR; // 左前轮
-  int16_t speedB = mappedY - mappedX - mappedR; // 右前轮
-  int16_t speedC = mappedY + mappedX - mappedR; // 右后轮
-  int16_t speedD = mappedY - mappedX + mappedR; // 左后轮
+  // 左前轮(A) = Y - X - R
+  // 右前轮(D) = Y + X - R
+  // 右后轮(C) = Y - X + R
+  // 左后轮(B) = Y + X + R
+  int16_t speedA = mappedY - mappedX - mappedR; // 左前轮
+  int16_t speedB = mappedY + mappedX + mappedR; // 左后轮
+  int16_t speedC = 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};
   }