feat: enhance interactive movement script with servo control; fix servo controlling; adapt usb device in config

playgrounds/interactive_movement.py

@@ -12,7 +12,12 @@ Interactive movement script for the robot.
 - Type 'exit' to quit.
 """
 
-grbl_config = GRBLConfig(port="/dev/ttyUSB0", baudrate=115200)
+# GPIO pin assignments
+pump_pin = 17
+valve_pin = 27
+servo_pin = 13
+
+grbl_config = GRBLConfig(port="/dev/ttyACM0", baudrate=115200)
 test_config = MovementConfig(
     speed_mmmin=15000,
     safe_height=10
@@ -24,19 +29,17 @@ grbl_handler = GRBLHandler(
 )
 movement = RobotMovement(test_config, grbl_handler)
 
-pump_pin = 17
-valve_pin = 27
-gpio = PiGPIO([pump_pin, valve_pin], [])
+gpio = PiGPIO([pump_pin, valve_pin, servo_pin], [])
 
 pump_state = 0
 valve_state = 0
 
 async def main():
     await grbl_handler.connect()
-    print("Robot ready. Enter X Y Z [speed_mmmin], 'p' to toggle pump, 'v' to toggle valve (or 'exit' to quit):")
+    print("Robot ready. Enter X Y Z [speed_mmmin], 'p' to toggle pump, 'v' to toggle valve, 's <angle> <speed>' for smooth servo (or 'exit' to quit):")
     global pump_state, valve_state
     while True:
-        user_input = input("Target position (X Y Z [speed] | p | v): ").strip().lower()
+        user_input = input("Target position (X Y Z [speed] | p | v | s <angle> <speed>): ").strip().lower()
         if user_input in ("exit", "quit"):
             print("Exiting...")
             break
@@ -50,6 +53,17 @@ async def main():
             gpio.set_pin(valve_pin, valve_state)
             print(f"Valve {'ON' if valve_state else 'OFF'}")
             continue
+        elif user_input.startswith("s "):
+            try:
+                _, angle_str, duration_str = user_input.split()
+                angle = float(angle_str)
+                duration = float(duration_str)
+                print(f"Moving servo on pin {servo_pin} to {angle}° for {duration}ms...")
+                gpio.set_servo_angle_smooth(servo_pin, angle, duration)
+                print("Servo move complete.")
+            except Exception:
+                print("Invalid servo command. Use: s <angle> <time_in_ms>")
+            continue
         try:
             parts = user_input.split()
             if len(parts) not in (3, 4):
@@ -60,11 +74,11 @@ async def main():
             else:
                 speed = test_config.speed_mmmin
         except Exception:
-            print("Invalid input. Please enter three or four numbers separated by spaces, 'p', 'v', or 'exit'.")
+            print("Invalid input. Please enter three or four numbers separated by spaces, 'p', 'v', 's <angle> <speed>', or 'exit'.")
             continue
         print(f"Moving to ({x}, {y}, {z}) at {speed} mm/min...")
         await movement.move_to_position(x, y, z, speed_mmmin=speed)
-        print("Move complete. Enter next position, 'p', 'v', or 'exit'.")
+        print("Move complete. Enter next position, 'p', 'v', 's <angle> <speed>', or 'exit'.")
 
 if __name__ == "__main__":
     try:

playgrounds/servo.py → playgrounds/servo_direct.py

@@ -3,7 +3,7 @@ import pigpio
 def main():
 
     pi = pigpio.pi()
-    servo_pin = 14
+    servo_pin = 13
 
     print("Servo Playground")
     print("Type an angle (0-180) to set the servo, or 'q' to quit.")

robot_control/config/config.yaml

@@ -91,7 +91,7 @@ mqtt:
   keepalive: 60
 
 grbl:
-  port: /dev/ttyUSB0 # or debug
+  port: /dev/ttyACM0 # or debug
   # port: debug
   baudrate: 115200
 

robot_control/src/api/gpio.py

@@ -97,6 +97,7 @@ class PiGPIO(GPIOInterface):
 
         if pin not in self._servo_angles:
             self.set_servo_angle(pin, target_angle_deg)
+            self._servo_angles[pin] = target_angle_deg
             return
 
         current_angle = self._servo_angles[pin]