test scs

examples/hopejr/scstest.py

@@ -0,0 +1,31 @@
+import time
+import serial
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Import the motor bus (adjust the import path as needed)
+from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus
+
+def main():
+
+    bus = FeetechMotorsBus(
+        port="/dev/ttyACM0",
+        motors={
+            "leader": [1, "scs0009"],
+            "follower": [2, "scs0009"]
+        },
+        protocol_version=1,
+        group_sync_read=False
+    )
+    bus.connect()
+    print(bus.read("Present_Position", "leader"))
+    bus.write("Torque_Enable", 0, ["leader"])
+    bus.write("Torque_Enable", 1, ["follower"])
+    for i in range(10000000):
+        time.sleep(0.01)
+        pos = bus.read("Present_Position", "leader")
+        if pos[0] > 1 and pos[0] < 1022:
+            bus.write("Goal_Position", pos, ["follower"])
+
+if __name__ == "__main__":
+    main()