对齐控制方法

README.md

@@ -147,23 +147,14 @@ curl -X POST http://{IP}:8765/cmd \
   -d '{"speed": 1.0, "steer": 0.3}'
 ```
 
-**Ackermann 模式**：
+匹配串口协议（11 字节帧），对接 `cmd_vel/Twist`：
 
 | 字段 | 类型 | 说明 |
 |------|------|------|
-| `speed` | float | 目标速度 m/s，正=前进，负=后退 |
-| `steer` | float | 目标转向角 rad，正=左转，负=右转 |
-
-**差速模式**（兼容 `cmd_vel/Twist`）：
-
-| 字段 | 类型 | 说明 |
-|------|------|------|
-| `vx` | float | 线速度 m/s |
-| `vy` | float | 横向速度 m/s（忽略） |
-| `wz` | float | 角速度 rad/s，自动转为转向角 |
+| `vx` | float | 线速度 m/s（对应 `X_speed`） |
+| `wz` | float | 角速度 rad/s（对应 `Z_speed`，固件内用 bicycle model 转阿克曼转向角） |
 
 ```bash
-# 差速模式
 curl -X POST http://{IP}:8765/cmd \
   -H "Content-Type: application/json" \
   -d '{"vx": 1.0, "wz": 0.5}'
@@ -186,10 +177,10 @@ with open("frame.jpg", "wb") as f:
 state = requests.get(f"http://{IP}:8765/state").json()
 print(f"位置: ({state['x']}, {state['y']}), 速度: {state['speed_actual']} m/s")
 
-# 发送控制指令
-requests.post(f"http://{IP}:8765/cmd", json={"speed": 1.0, "steer": 0.2})
+# 发送控制指令（串口协议格式：vx + wz）
+requests.post(f"http://{IP}:8765/cmd", json={"vx": 1.0, "wz": 0.5})
 time.sleep(2)
-requests.post(f"http://{IP}:8765/cmd", json={"speed": 0.0, "steer": 0.0})
+requests.post(f"http://{IP}:8765/cmd", json={"vx": 0.0, "wz": 0.0})
 ```
 
 **ROS2 桥接示例**（仿真机 HTTP → ROS2 局域网）：

main.py

@@ -211,20 +211,17 @@ class HTTPHandler(BaseHTTPRequestHandler):
             length = int(self.headers.get("Content-Length", 0))
             body = json.loads(self.rfile.read(length))
             with shared_lock:
-                # Ackermann mode: {"speed": v, "steer": angle}
-                if "speed" in body or "steer" in body:
-                    shared["cmd_speed"] = float(body.get("speed", 0))
-                    shared["cmd_steer"] = float(body.get("steer", 0))
-                # Differential mode: {"vx":, "vy":, "wz":} → mapped to speed/steer
-                elif "vx" in body:
-                    vx = float(body.get("vx", 0))
-                    wz = float(body.get("wz", 0))
-                    shared["cmd_speed"] = vx
-                    # Convert angular.z to steer via bike model (wheelbase ≈ 0.17m)
-                    if abs(vx) > 0.01:
-                        shared["cmd_steer"] = math.atan(wz * L / vx)
-                    else:
-                        shared["cmd_steer"] = 0.0
+                vx = float(body.get("vx", 0))
+                wz = float(body.get("wz", 0))
+                shared["cmd_speed"] = vx
+                # Bicycle model Vz→steer (matches firmware Vz_to_Akm_Angle)
+                # Axle_spacing=0.158, Wheel_spacing=0.162
+                if abs(vx) > 0.01 and abs(wz) > 1e-6:
+                    R_turn = vx / wz
+                    steer = math.atan(0.158 / (R_turn + 0.05 * 0.162))
+                    shared["cmd_steer"] = steer if wz >= 0 else -steer
+                else:
+                    shared["cmd_steer"] = 0.0
             self.send_response(200)
             self.end_headers()
         else: