From 6b6da795f94a4ccec14762ead732c80865cce2ff Mon Sep 17 00:00:00 2001
From: apirrone <awanegenebayo@gmail.com>
Date: Tue, 13 May 2025 14:33:42 +0200
Subject: [PATCH] record data

---
 scripts/new_record_data.py    | 102 ++++++++++++++++++++++++++++++++++
 scripts/plot_recorded_data.py |   3 +-
 2 files changed, 104 insertions(+), 1 deletion(-)
 create mode 100644 scripts/new_record_data.py

diff --git a/scripts/new_record_data.py b/scripts/new_record_data.py
new file mode 100644
index 0000000..fdb3c6b
--- /dev/null
+++ b/scripts/new_record_data.py
@@ -0,0 +1,102 @@
+from pypot.feetech import FeetechSTS3215IO
+import pickle
+import numpy as np
+import time
+import argparse
+
+parser = argparse.ArgumentParser(description="Record data from Feetech motor")
+parser.add_argument(
+    "--new_firmware",
+    action="store_true",
+    help="Use new firmware for the motor",
+    default=False,
+)
+args = parser.parse_args()
+io = FeetechSTS3215IO("/dev/ttyACM0")
+
+def convert_load(raw_load):
+    sign = -1
+    if raw_load > 1023:
+        raw_load -= 1024
+        sign = 1
+    return sign * raw_load * 0.001
+
+
+id = 1
+kp = 16
+kd = 0
+acceleration = 0
+maximum_acceleration = 0
+maximum_velocity = 0
+
+io.set_mode({id: 0})
+io.set_lock({id: 1})
+time.sleep(1)
+io.set_maximum_acceleration({id: maximum_acceleration})
+io.set_acceleration({id: acceleration})
+io.set_maximum_velocity({id: maximum_velocity})
+io.set_P_coefficient({id: kp})
+io.set_D_coefficient({id: kd})
+time.sleep(1)
+
+goal_position = 90
+
+io.set_goal_position({id: 0})
+time.sleep(3)
+
+
+times = []
+positions = []
+goal_positions = []
+speeds = []
+loads = []
+currents = []
+
+io.set_goal_position({1: goal_position})
+
+s = time.time()
+set = False
+while True:
+    t = time.time() - s
+    # goal_position = np.rad2deg(np.sin(t**2))
+    io.set_goal_position({1: goal_position})
+    present_position = np.deg2rad(io.get_present_position([1])[0])
+    present_speed = np.deg2rad(io.get_present_speed([1])[0])
+    present_load = convert_load(io.get_present_load([1])[0])
+    present_current = io.get_present_current([1])[0]
+
+    times.append(t)
+    positions.append(present_position)
+    goal_positions.append(np.deg2rad(goal_position))
+    speeds.append(present_speed)
+    loads.append(present_load)
+    currents.append(present_current)
+
+    if t > 3:
+        break
+
+    time.sleep(0.01)
+
+fw_version = "new" if args.new_firmware else "old"
+data = {
+    "maximum_acceleration": maximum_acceleration,
+    "acceleration": acceleration,
+    "maximum_velocity": maximum_velocity,
+    "kp": kp,
+    "kd": kd,
+    "times": times,
+    "positions": positions,
+    "goal_positions": goal_positions,
+    "speeds": speeds,
+    "loads": loads,
+    "currents": currents,
+    "fw_version": fw_version,
+}
+
+pickle.dump(
+    data,
+    open(
+        f"data_KP_{kp}_{fw_version}.pkl",
+        "wb",
+    ),
+)
diff --git a/scripts/plot_recorded_data.py b/scripts/plot_recorded_data.py
index a3444a2..82f7d0d 100644
--- a/scripts/plot_recorded_data.py
+++ b/scripts/plot_recorded_data.py
@@ -28,7 +28,8 @@ plt.subplot(4, 1, 4)
 plt.plot(data["times"], data["currents"], label="current (mA)")
 plt.legend()
 
-plt.suptitle(f"Acceleration: {data['acceleration']}, KP: {data['kp']}, KD: {data['kd']}")
+# plt.suptitle(f"Acceleration: {data['acceleration']}, KP: {data['kp']}, KD: {data['kd']}")
+plt.suptitle(f"KP: {data['kp']}, firmware: {data['fw_version']}")
 plt.tight_layout()
 
 plt.show()