From 819a87f7b4a3cae496aaa67767cf5ee85da7dba5 Mon Sep 17 00:00:00 2001
From: apirrone <awanegenebayo@gmail.com>
Date: Mon, 16 Jun 2025 18:59:00 +0200
Subject: [PATCH] test

---
 scripts/new_record_data.py | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/scripts/new_record_data.py b/scripts/new_record_data.py
index fdb3c6b..548839d 100644
--- a/scripts/new_record_data.py
+++ b/scripts/new_record_data.py
@@ -14,6 +14,7 @@ parser.add_argument(
 args = parser.parse_args()
 io = FeetechSTS3215IO("/dev/ttyACM0")
 
+
 def convert_load(raw_load):
     sign = -1
     if raw_load > 1023:
@@ -22,19 +23,19 @@ def convert_load(raw_load):
     return sign * raw_load * 0.001
 
 
-id = 1
-kp = 16
+id = 1 if args.new_firmware else 11
+kp = 32
 kd = 0
 acceleration = 0
 maximum_acceleration = 0
 maximum_velocity = 0
+goal_speed = 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_goal_speed({id: goal_speed})
 io.set_P_coefficient({id: kp})
 io.set_D_coefficient({id: kd})
 time.sleep(1)
@@ -44,6 +45,7 @@ goal_position = 90
 io.set_goal_position({id: 0})
 time.sleep(3)
 
+exit()
 
 times = []
 positions = []
@@ -52,18 +54,18 @@ speeds = []
 loads = []
 currents = []
 
-io.set_goal_position({1: goal_position})
+io.set_goal_position({id: 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]
+    io.set_goal_position({id: goal_position})
+    present_position = np.deg2rad(io.get_present_position([id])[0])
+    present_speed = np.deg2rad(io.get_present_speed([id])[0])
+    present_load = convert_load(io.get_present_load([id])[0])
+    present_current = io.get_present_current([id])[0]
 
     times.append(t)
     positions.append(present_position)