fix: xyr control & many profiles support

.gitignore

@@ -4,4 +4,5 @@
 .vscode/launch.json
 .vscode/ipch
 src/consts.h
-.DS_Store
+.DS_Store
+src/consts-*.h

src/main.cpp

@@ -212,6 +212,48 @@ void motorControl(char motor, int speed)
   pwmControl(motor, speed);
 }
 
+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);
+  
+  // 麦克纳姆轮运动学方程
+  // 左前轮 = 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;  // 左后轮
+  
+  // 找出最大速度的绝对值
+  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);
+}
+
+
 float getDistance()
 {
   digitalWrite(HC_SR04_TRIG, HIGH);
@@ -691,41 +733,4 @@ 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);
-}
+}