feat: add interactive movement script for real-time robot control; update launch configuration

.vscode/launch.json

@@ -32,6 +32,13 @@
             "module": "playgrounds.movement_playground",
             "console": "integratedTerminal"
         },
+        {
+            "name": "Interactive_Movement",
+            "type": "debugpy",
+            "request": "launch",
+            "module": "playgrounds.interactive_movement",
+            "console": "integratedTerminal"
+        },
         {
             "name": "Integration_Test",
             "type": "debugpy",

playgrounds/integration_test.py

@@ -50,21 +50,25 @@ class LoaderSystem:
         i2c_device_class = MCP3428 if not self.config.i2c.debug else MockI2C
         self.i2c = I2C(i2c_device_class)
 
+        self.feeder_queue: asyncio.Queue[int] = asyncio.Queue(self.config.feeder.max_capacity)
+        self.defeeder_queue: asyncio.Queue[int]  = asyncio.Queue(self.config.defeeder.max_capacity)
+
         # Initialize robot controller with all required arguments
         self.controller = RobotController(
             self.config,
             self.gpio,
             self.i2c,
             self.vision,
-            self.pump_controller
+            self.pump_controller,
+            self.feeder_queue,
+            self.defeeder_queue
         )
         
         self.logger = logging.getLogger(__name__)
         self.logger.info("Initializing LoaderSystem")
-        # self.vision.initialize()
+        self.vision.initialize()
 
         # Test stuff
-        self.test_dropoff = self.config.dropoff_grades[0]
         self.test_slot = self.config.measurement_devices[0].slots[3]
 
         # Use mock I2C device if debug is enabled
@@ -104,7 +108,7 @@ class LoaderSystem:
             await wait_for_enter()
             self.logger.info("Starting movement sequence...")
             await self.controller.pick_cell_from_slot(self.test_slot)
-            await self.controller.dropoff_cell(self.test_dropoff)
+            await self.controller.dropoff_cell()
 
             await self.controller.pick_cell_from_feeder()
             cell = Cell(id=1, status=CellStatus.WAITING)

playgrounds/interactive_movement.py

@@ -0,0 +1,47 @@
+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
+
+"""
+Interactive movement script for the robot.
+- Homes only once at startup.
+- Lets you enter X, Y, Z positions in a loop.
+- No restart or re-homing between moves.
+- Type 'exit' to quit.
+"""
+
+grbl_config = GRBLConfig(port="/dev/ttyUSB0", baudrate=115200)
+test_config = MovementConfig(
+    speed=1000,
+    safe_height=10
+)
+
+grbl_handler = GRBLHandler(
+    grbl_config.port,
+    grbl_config.baudrate
+)
+movement = RobotMovement(test_config, grbl_handler)
+
+async def main():
+    await grbl_handler.connect()
+    print("Robot ready. Enter X Y Z coordinates (or 'exit' to quit):")
+    while True:
+        user_input = input("Target position (X Y Z): ").strip()
+        if user_input.lower() in ("exit", "quit"):  # Allow exit
+            print("Exiting...")
+            break
+        try:
+            x, y, z = map(float, user_input.split())
+        except Exception:
+            print("Invalid input. Please enter three numbers separated by spaces, 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'.")
+
+if __name__ == "__main__":
+    try:
+        asyncio.run(main())
+    except KeyboardInterrupt:
+        print("\nExiting...")