From 489da82052c2891c8099769a9586e913d1e3b5b8 Mon Sep 17 00:00:00 2001
From: apirrone <awanegenebayo@gmail.com>
Date: Tue, 24 Jun 2025 13:25:48 +0200
Subject: [PATCH] add configure_all_motors.py

---
 scripts/configure_all_motors.py | 40 ++++++++++++++++++++++++++++
 scripts/test2.py                | 47 ---------------------------------
 2 files changed, 40 insertions(+), 47 deletions(-)
 create mode 100644 scripts/configure_all_motors.py
 delete mode 100644 scripts/test2.py

diff --git a/scripts/configure_all_motors.py b/scripts/configure_all_motors.py
new file mode 100644
index 0000000..5d391cc
--- /dev/null
+++ b/scripts/configure_all_motors.py
@@ -0,0 +1,40 @@
+from pypot.feetech import FeetechSTS3215IO
+import time
+
+joints = {
+    "left_hip_yaw": 20,
+    "left_hip_roll": 21,
+    "left_hip_pitch": 22,
+    "left_knee": 23,
+    "left_ankle": 24,
+    "neck_pitch": 30,
+    "head_pitch": 31,
+    "head_yaw": 32,
+    "head_roll": 33,
+    "right_hip_yaw": 10,
+    "right_hip_roll": 11,
+    "right_hip_pitch": 12,
+    "right_knee": 13,
+    "right_ankle": 14,
+}
+
+joints_inv = {v: k for k, v in joints.items()}
+
+ids = list(joints.values())
+io = FeetechSTS3215IO("/dev/ttyACM0")
+for current_id in ids:
+    print("Configuring", joints_inv[current_id])
+    io.set_lock({current_id: 0})
+    # io.set_mode({current_id: 0})
+    io.set_maximum_acceleration({current_id: 0})
+    io.set_acceleration({current_id: 0})
+    # io.set_maximum_velocity({current_id: 0})
+    # io.set_goal_speed({current_id: 0})
+    io.set_P_coefficient({current_id: 32})
+    io.set_I_coefficient({current_id: 0})
+    io.set_D_coefficient({current_id: 0})
+
+    io.set_lock({current_id: 1})
+    time.sleep(1)
+    input("Press any key to set this dof to 0 position ... Or press Ctrl+C to cancel")
+    io.set_goal_position({current_id: 0})
diff --git a/scripts/test2.py b/scripts/test2.py
deleted file mode 100644
index b9f28f3..0000000
--- a/scripts/test2.py
+++ /dev/null
@@ -1,47 +0,0 @@
-from openai import OpenAI
-import os
-import base64
-from mini_bdx_runtime.camera import Cam
-
-cam = Cam()
-
-client = OpenAI()
-
-def encode_image(image_path: str):
-    # check if the image exists
-    if not os.path.exists(image_path):
-        raise FileNotFoundError(f"Image file not found: {image_path}")
-    with open(image_path, "rb") as image_file:
-        return base64.b64encode(image_file.read()).decode("utf-8")
-
-# response = client.responses.create(
-#     model="gpt-4o-mini",
-#     input=[{
-#         "role": "user",
-#         "content": [
-#             {"type": "input_text", "text": "what's in this image?"},
-#             {
-#                 "type": "input_image",
-#                 "image_url": "http://s-nguyen.net:4444/images/aze.jpg",
-#             },
-#         ],
-#     }],
-# )
-
-# image64 = encode_image("/home/antoine/aze.jpg")
-image64 = cam.get_encoded_image()
-response = client.responses.create(
-    model="gpt-4o-mini",
-    input=[{
-        "role": "user",
-        "content": [
-            {"type": "input_text", "text": "what's in this image?"},
-            {
-                "type": "input_image",
-                "image_url": "data:image/jpeg;base64,"+image64,
-            },
-        ],
-    }],
-)
-
-print(response.output_text)