import pandas as pd
import matplotlib.pyplot as plt

# Load the CSV exported by the PID controller
df = pd.read_csv("pid_logs.csv")

# --- Plot target vs current ---
for coord in ["x", "y", "z"]:
    plt.figure()
    plt.plot(df["time"], df["target_" + coord], label="target_" + coord)
    plt.plot(df["time"], df["current_" + coord], label="current_" + coord)
    plt.xlabel("Time (s)")
    plt.ylabel("Angular velocity")
    plt.title(f"Target vs Current Angular Velocity ({coord})")
    plt.legend()
    plt.grid(True)
    plt.show()

plt.figure()
plt.plot(df["time"], df["target_x"], label="target_x")
plt.plot(df["time"], df["current_x"], label="current_x")
plt.plot(df["time"], df["target_y"], label="target_y")
plt.plot(df["time"], df["current_y"], label="current_y")
plt.plot(df["time"], df["target_z"], label="target_z")
plt.plot(df["time"], df["current_z"], label="current_z")
plt.xlabel("Time (s)")
plt.ylabel("Angular velocity")
plt.title("Target vs Current Angular Velocity")
plt.legend()
plt.grid(True)
plt.show()
# --- Plot error over time ---
plt.figure()
plt.plot(df["time"], df["error_x"], label="error_x")
plt.plot(df["time"], df["error_y"], label="error_y")
plt.plot(df["time"], df["error_z"], label="error_z")
plt.xlabel("Time (s)")
plt.ylabel("Error")
plt.title("PID Error Over Time")
plt.legend()
plt.grid(True)
plt.show()

