Initial commit from MyParking project

data_process/电机响应处理/README.md

@@ -0,0 +1,28 @@
+# 舵轮转向响应处理
+
+此工具读取 M 层 `StartWheelSpeedDiagnostic` / `StopWheelSpeedDiagnostic` 生成的 `*_snapshot.csv`，用于分析正常、蟹行、自转模式切换时四个舵轮的响应。
+
+首次使用时安装依赖：
+
+```powershell
+pip install -r requirements.txt
+```
+
+在本目录运行：
+
+```powershell
+python .\plot_steering_response.py
+```
+
+默认选择 `MyParking\logs\wheel-speed` 中最新的快照 CSV，并在同级 `plots` 文件夹生成：
+
+- `*_steering_angles.png`：四轮目标角、实际角和 ±120°机械限位；
+- `*_steering_error_pid.png`：四轮转角误差与 PID 输出；
+- `*_motor_command_feedback.png`：八个电机的最终命令速度与 CAN 反馈速度；
+- `*_mode_speed_limit.png`：正常/蟹行/自转模式变化和 `SendThresSpeed`。
+
+也可以指定一个文件或局部时间范围：
+
+```powershell
+python .\plot_steering_response.py --input "D:\\xxx_snapshot.csv" --from-seconds 2 --to-seconds 15
+```

data_process/电机响应处理/plot_steering_response.py

@@ -0,0 +1,238 @@
+"""可视化停车机器人四舵轮的模式切换与转向响应。
+
+默认读取 MyParking/logs/wheel-speed 中最新的 *_snapshot.csv，输出四张 PNG：
+1. 四个舵轮的目标角、实际角和模式切换时刻；
+2. 四个舵轮的转角误差；
+3. 四个转向 PID 输出；
+4. 八个电机的最终命令速度与 CAN 反馈速度。
+
+示例：
+    python plot_steering_response.py
+    python plot_steering_response.py --input "D:\\logs\\xxx_snapshot.csv"
+"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+
+
+SCRIPT_DIRECTORY = Path(__file__).resolve().parent
+PROJECT_DIRECTORY = SCRIPT_DIRECTORY.parent.parent
+DEFAULT_LOG_DIRECTORY = PROJECT_DIRECTORY / "logs" / "wheel-speed"
+
+WHEELS = (
+    ("LeftFront", "左前", "tab:blue"),
+    ("LeftRear", "左后", "tab:orange"),
+    ("RightFront", "右前", "tab:green"),
+    ("RightRear", "右后", "tab:red"),
+)
+
+MOTORS = (
+    ("LFL", "左前左"), ("LFR", "左前右"),
+    ("LRL", "左后左"), ("LRR", "左后右"),
+    ("RFL", "右前左"), ("RFR", "右前右"),
+    ("RRL", "右后左"), ("RRR", "右后右"),
+)
+
+MODE_NAMES = {0: "正常", 1: "蟹行", 2: "自转"}
+
+
+def parse_arguments() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="绘制四舵轮模式切换响应图")
+    parser.add_argument("--input", type=Path, help="指定 *_snapshot.csv；缺省时取最新文件")
+    parser.add_argument("--output", type=Path, help="图片输出目录；缺省时写入本次日志同级 plots")
+    parser.add_argument("--from-seconds", type=float, default=0.0, help="从第几秒开始显示")
+    parser.add_argument("--to-seconds", type=float, help="显示到第几秒结束")
+    return parser.parse_args()
+
+
+def find_snapshot(path: Path | None) -> Path:
+    if path is not None:
+        if not path.is_file():
+            raise FileNotFoundError(f"找不到快照文件：{path}")
+        return path
+
+    candidates = sorted(
+        DEFAULT_LOG_DIRECTORY.glob("*_snapshot.csv"),
+        key=lambda item: item.stat().st_mtime,
+        reverse=True,
+    )
+    if not candidates:
+        raise FileNotFoundError(
+            f"{DEFAULT_LOG_DIRECTORY} 中没有 *_snapshot.csv。\n"
+            "请先在 M 层点击 StartWheelSpeedDiagnostic，完成模式切换后点击 StopWheelSpeedDiagnostic。"
+        )
+    return candidates[0]
+
+
+def load_snapshot(path: Path) -> pd.DataFrame:
+    frame = pd.read_csv(path, comment="#")
+    required = {"ElapsedMs", "ManualControlMode", "SendThresSpeed"}
+    missing = required.difference(frame.columns)
+    if missing:
+        raise ValueError(f"CSV 缺少字段：{', '.join(sorted(missing))}。请部署最新 MedullaAdapter.dll 后重新记录。")
+
+    frame = frame.apply(pd.to_numeric, errors="coerce")
+    frame = frame.dropna(subset=["ElapsedMs"]).sort_values("ElapsedMs")
+    if frame.empty:
+        raise ValueError("CSV 中没有有效数据行。")
+    frame["ElapsedSeconds"] = (frame["ElapsedMs"] - frame["ElapsedMs"].iloc[0]) / 1000.0
+    return frame
+
+
+def crop(frame: pd.DataFrame, start: float, end: float | None) -> pd.DataFrame:
+    result = frame[frame["ElapsedSeconds"] >= start]
+    if end is not None:
+        result = result[result["ElapsedSeconds"] <= end]
+    if result.empty:
+        raise ValueError("所选时间范围内没有数据。")
+    return result
+
+
+def require_columns(frame: pd.DataFrame, names: list[str]) -> None:
+    missing = [name for name in names if name not in frame.columns]
+    if missing:
+        raise ValueError("CSV 缺少字段：" + ", ".join(missing))
+
+
+def add_mode_markers(axis: plt.Axes, frame: pd.DataFrame) -> None:
+    modes = frame["ManualControlMode"].round().astype("Int64")
+    changes = modes.ne(modes.shift())
+    for _, row in frame.loc[changes].iterrows():
+        mode = int(row["ManualControlMode"])
+        axis.axvline(row["ElapsedSeconds"], color="0.55", linestyle="--", linewidth=0.8, alpha=0.75)
+        axis.text(
+            row["ElapsedSeconds"], 0.99, MODE_NAMES.get(mode, f"模式{mode}"),
+            transform=axis.get_xaxis_transform(), rotation=90,
+            va="top", ha="right", fontsize=8, color="0.35",
+        )
+
+
+def save_steering_angle_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None:
+    required = []
+    for key, _, _ in WHEELS:
+        required.extend([f"TargetTh{key}", f"ActualTh{key}"])
+    require_columns(frame, required)
+
+    figure, axes = plt.subplots(2, 2, figsize=(14, 8), sharex=True)
+    for axis, (key, label, color) in zip(axes.flat, WHEELS):
+        time = frame["ElapsedSeconds"]
+        axis.plot(time, frame[f"TargetTh{key}"], label="目标角", color=color, linewidth=1.8)
+        axis.plot(time, frame[f"ActualTh{key}"], label="实际角", color="0.15", linewidth=1.1)
+        axis.axhline(120, color="tab:red", linestyle=":", linewidth=0.8, label="机械限位 ±120°")
+        axis.axhline(-120, color="tab:red", linestyle=":", linewidth=0.8)
+        add_mode_markers(axis, frame)
+        axis.set_title(f"{label}舵轮")
+        axis.set_ylabel("转角 (deg)")
+        axis.grid(alpha=0.25)
+        axis.legend(loc="best", fontsize=8)
+    for axis in axes[1]:
+        axis.set_xlabel("时间 (s)")
+    figure.suptitle("四舵轮目标转角与实际转角")
+    figure.tight_layout()
+    figure.savefig(output / f"{prefix}_steering_angles.png", dpi=180)
+    plt.close(figure)
+
+
+def save_error_and_pid_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None:
+    error_columns = [f"ErrorTh{key}" for key, _, _ in WHEELS]
+    pid_columns = [f"PidOut{key}" for key, _, _ in WHEELS]
+    require_columns(frame, error_columns + pid_columns)
+
+    figure, axes = plt.subplots(2, 1, figsize=(14, 9), sharex=True)
+    time = frame["ElapsedSeconds"]
+    for key, label, color in WHEELS:
+        axes[0].plot(time, frame[f"ErrorTh{key}"], label=label, color=color, linewidth=1.2)
+        axes[1].plot(time, frame[f"PidOut{key}"], label=label, color=color, linewidth=1.2)
+    axes[0].axhline(2, color="0.4", linestyle=":", linewidth=0.9, label="到位阈值 ±2°")
+    axes[0].axhline(-2, color="0.4", linestyle=":", linewidth=0.9)
+    for axis in axes:
+        add_mode_markers(axis, frame)
+        axis.grid(alpha=0.25)
+        axis.legend(loc="best", ncol=3, fontsize=9)
+    axes[0].set_ylabel("目标角 - 实际角 (deg)")
+    axes[1].set_ylabel("转向 PID 输出 (m/s)")
+    axes[1].set_xlabel("时间 (s)")
+    figure.suptitle("转角误差与转向 PID 输出")
+    figure.tight_layout()
+    figure.savefig(output / f"{prefix}_steering_error_pid.png", dpi=180)
+    plt.close(figure)
+
+
+def save_motor_speed_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None:
+    command_columns = [f"Pid{name}" for name, _ in MOTORS]
+    feedback_columns = [f"Actual{name}" for name, _ in MOTORS]
+    require_columns(frame, command_columns + feedback_columns)
+
+    figure, axes = plt.subplots(4, 2, figsize=(15, 12), sharex=True)
+    time = frame["ElapsedSeconds"]
+    for axis, (name, label) in zip(axes.flat, MOTORS):
+        axis.plot(time, frame[f"Pid{name}"], label="最终命令", color="tab:blue", linewidth=1.2)
+        axis.plot(time, frame[f"Actual{name}"], label="CAN反馈", color="tab:orange", linewidth=1.0)
+        add_mode_markers(axis, frame)
+        axis.set_title(f"{label}电机 ({name})")
+        axis.set_ylabel("速度 (m/s)")
+        axis.grid(alpha=0.25)
+        axis.legend(loc="best", fontsize=8)
+    for axis in axes[-1]:
+        axis.set_xlabel("时间 (s)")
+    figure.suptitle("八个电机最终速度命令与 CAN 实际速度反馈")
+    figure.tight_layout()
+    figure.savefig(output / f"{prefix}_motor_command_feedback.png", dpi=180)
+    plt.close(figure)
+
+
+def save_summary_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None:
+    require_columns(frame, ["SendThresSpeed"])
+    figure, axes = plt.subplots(2, 1, figsize=(14, 7), sharex=True)
+    time = frame["ElapsedSeconds"]
+    axes[0].step(time, frame["ManualControlMode"], where="post", color="tab:purple", linewidth=1.5)
+    axes[0].set_yticks([0, 1, 2], ["正常", "蟹行", "自转"])
+    axes[0].set_ylabel("控制模式")
+    axes[0].grid(alpha=0.25)
+    axes[1].plot(time, frame["SendThresSpeed"], color="tab:brown", linewidth=1.4, label="SendThresSpeed")
+    axes[1].set_ylabel("速度限幅 (m/s)")
+    axes[1].set_xlabel("时间 (s)")
+    axes[1].grid(alpha=0.25)
+    axes[1].legend(loc="best")
+    figure.suptitle("模式切换与整车下发速度限幅")
+    figure.tight_layout()
+    figure.savefig(output / f"{prefix}_mode_speed_limit.png", dpi=180)
+    plt.close(figure)
+
+
+def print_parameter_summary(frame: pd.DataFrame) -> None:
+    parameter_names = [
+        "DiffSteerKp", "DiffSteerKi", "DiffSteerKd", "DiffSteerMaxI",
+        "DiffSteerDeadZone", "DiffSteerThresh", "DiffSteerSpeedAcc",
+    ]
+    if not set(parameter_names).issubset(frame.columns):
+        return
+    print("本次记录的转向 PID 参数：")
+    print("  " + ", ".join(f"{name}={frame[name].iloc[0]:.6g}" for name in parameter_names))
+
+
+def main() -> None:
+    arguments = parse_arguments()
+    snapshot_path = find_snapshot(arguments.input)
+    frame = crop(load_snapshot(snapshot_path), arguments.from_seconds, arguments.to_seconds)
+    output_directory = arguments.output or snapshot_path.parent / "plots"
+    output_directory.mkdir(parents=True, exist_ok=True)
+    prefix = snapshot_path.name.removesuffix("_snapshot.csv")
+
+    save_steering_angle_plot(frame, output_directory, prefix)
+    save_error_and_pid_plot(frame, output_directory, prefix)
+    save_motor_speed_plot(frame, output_directory, prefix)
+    save_summary_plot(frame, output_directory, prefix)
+    print_parameter_summary(frame)
+    print(f"已读取：{snapshot_path}")
+    print(f"已生成四张图：{output_directory}")
+
+
+if __name__ == "__main__":
+    main()

data_process/电机响应处理/requirements.txt

@@ -0,0 +1,3 @@
+matplotlib>=3.7
+numpy>=1.24
+pandas>=2.0