From 438685ecef030376ddc4f95a8eb8c2b44c497d67 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E7=8E=96=E5=8F=81?= <x@fur93.icu>
Date: Fri, 20 Dec 2024 00:11:04 +0800
Subject: [PATCH] feat: xyr

---
 .gitignore   |  1 +
 packet.md    |  8 ++++++++
 src/main.cpp | 51 ++++++++++++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index fa22808..258dcd5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,3 +4,4 @@
 .vscode/launch.json
 .vscode/ipch
 src/consts.h
+.DS_Store
\ No newline at end of file
diff --git a/packet.md b/packet.md
index 0475f43..0f99f66 100644
--- a/packet.md
+++ b/packet.md
@@ -102,6 +102,14 @@
 
 控制示例 `00 06 23 01 01 FF`
 
+### XYR 控制 `0x24`
+
+> ⚠️ 注意： X Y R 的取值范围为 -100 到 100，为有符号 8 位整数
+
+包体 `00 07 24 X Y R FF`
+
+控制示例 `00 07 24 01 01 01 FF`
+
 ## 状态回报
 
 ### 电机状态汇报 `0xE0`
diff --git a/src/main.cpp b/src/main.cpp
index 90bbd63..db2e13b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -25,6 +25,7 @@
 #define CMD_MOTOR_ROTATE_CONTROL 0x23
 #define CMD_DEMO_PID 0xf0
 #define CMD_DEMO_PATH 0xf1
+#define CMD_MOTOR_XYR_CONTROL 0x24  // XYR轴向控制指令
 
 #define CMD_STATUS_MOTOR 0xE0
 
@@ -583,6 +584,17 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
 
     break;
 
+  case CMD_MOTOR_XYR_CONTROL:
+    if (length >= 7) {  // 确保数据包长度正确
+      int8_t x = (int8_t)packet[3];  // X轴向速度
+      int8_t y = (int8_t)packet[4];  // Y轴向速度
+      int8_t r = (int8_t)packet[5];  // 旋转速度
+      
+      Serial.printf("CMD_MOTOR_XYR_CONTROL: X=%d, Y=%d, R=%d\n", x, y, r);
+      motorXYRControl(x, y, r);
+    }
+    break;
+
   default:
     break;
   }
@@ -612,7 +624,7 @@ void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characte
         expectedLength = incomingByte;
       }
 
-      // 到达预期长度时检查包尾
+      // 到预期长度时检查包尾
       if (packetIndex == expectedLength - 1)
       {
         isReceivingPacket = false;
@@ -680,3 +692,40 @@ void loop()
     sendMotorStatus();
   }
 }
+
+void motorXYRControl(int8_t x, int8_t y, int8_t r) {
+  // 将输入范围限制在 -100 到 100
+  x = constrain(x, -100, 100);
+  y = constrain(y, -100, 100);
+  r = constrain(r, -100, 100);
+  
+  // 将 -100~100 映射到 -255~255
+  int16_t mappedX = map(x, -100, 100, -255, 255);
+  int16_t mappedY = map(y, -100, 100, -255, 255);
+  int16_t mappedR = map(r, -100, 100, -255, 255);
+  
+  // 计算每个电机的速度
+  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)), 
+                        max(abs(speedC), abs(speedD)));
+  
+  // 如果最大速度超过255，等比例缩放所有速度
+  if (maxSpeed > 255) {
+    float scale = 255.0f / maxSpeed;
+    speedA *= scale;
+    speedB *= scale;
+    speedC *= scale;
+    speedD *= scale;
+  }
+  
+  // 控制各个电机
+  motorControl('A', speedA);
+  motorControl('B', speedB);
+  motorControl('C', speedC);
+  motorControl('D', speedD);
+}