feat: enhance interactive movement script with pump and valve control; update speed configuration in movement settings

playgrounds/interactive_movement.py

@@ -2,6 +2,7 @@ import asyncio
 from robot_control.src.robot.movement import RobotMovement
 from robot_control.src.api.grbl_handler import GRBLHandler
 from robot_control.src.utils.config import GRBLConfig, MovementConfig
+from robot_control.src.api.gpio import PiGPIO
 
 """
 Interactive movement script for the robot.
@@ -13,7 +14,7 @@ Interactive movement script for the robot.
 
 grbl_config = GRBLConfig(port="/dev/ttyUSB0", baudrate=115200)
 test_config = MovementConfig(
-    speed=1000,
+    speed_mmmin=15000,
     safe_height=10
 )
 
@@ -23,22 +24,47 @@ grbl_handler = GRBLHandler(
 )
 movement = RobotMovement(test_config, grbl_handler)
 
+pump_pin = 17
+valve_pin = 27
+gpio = PiGPIO([pump_pin, valve_pin], [])
+
+pump_state = 0
+valve_state = 0
+
 async def main():
     await grbl_handler.connect()
-    print("Robot ready. Enter X Y Z coordinates (or 'exit' to quit):")
+    print("Robot ready. Enter X Y Z [speed_mmmin], 'p' to toggle pump, 'v' to toggle valve (or 'exit' to quit):")
+    global pump_state, valve_state
     while True:
-        user_input = input("Target position (X Y Z): ").strip()
-        if user_input.lower() in ("exit", "quit"):  # Allow exit
+        user_input = input("Target position (X Y Z [speed] | p | v): ").strip().lower()
+        if user_input in ("exit", "quit"):
             print("Exiting...")
             break
+        elif user_input == "p":
+            pump_state ^= 1
+            gpio.set_pin(pump_pin, pump_state)
+            print(f"Pump {'ON' if pump_state else 'OFF'}")
+            continue
+        elif user_input == "v":
+            valve_state ^= 1
+            gpio.set_pin(valve_pin, valve_state)
+            print(f"Valve {'ON' if valve_state else 'OFF'}")
+            continue
         try:
-            x, y, z = map(float, user_input.split())
+            parts = user_input.split()
+            if len(parts) not in (3, 4):
+                raise ValueError
+            x, y, z = map(float, parts[:3])
+            if len(parts) == 4:
+                speed = int(parts[3])
+            else:
+                speed = test_config.speed_mmmin
         except Exception:
-            print("Invalid input. Please enter three numbers separated by spaces, or 'exit'.")
+            print("Invalid input. Please enter three or four numbers separated by spaces, 'p', 'v', or 'exit'.")
             continue
-        print(f"Moving to ({x}, {y}, {z})...")
-        await movement.move_to_position(x, y, z)
-        print("Move complete. Enter next position or 'exit'.")
+        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'.")
 
 if __name__ == "__main__":
     try:

robot_control/config/config.yaml

@@ -1,6 +1,6 @@
 measurement_devices:
   - id: device_1
-    position: [375.5, 898, 107]
+    position: [25, 804, 68]
     slots:
       - position: [10, 0, 0]
         occupied: False
@@ -79,8 +79,8 @@ defeeder_magazines:
 
 mag_distributor:
     debug: True
-    max_speed_mmmin: 100
-    home_speed_mmmin: 100
+    max_speed_mmmin: 10000
+    home_speed_mmmin: 2000
     length_mm: 500
 
 mqtt:
@@ -134,7 +134,7 @@ i2c:
   debug: false
 
 movement:
-  speed_mmmin: 400
+  speed_mmmin: 15000
   safe_height: 0.0
 
 logging: