feat: add integration test script; fix pump controller watchdog; add real config positions

.vscode/launch.json

@@ -31,6 +31,13 @@
             "request": "launch",
             "module": "playgrounds.movement_playground",
             "console": "integratedTerminal"
+        },
+        {
+            "name": "Integration_Test",
+            "type": "debugpy",
+            "request": "launch",
+            "module": "playgrounds.integration_test",
+            "console": "integratedTerminal"
         }
     ]
 }

playgrounds/gpio_playground.py

@@ -1,6 +1,14 @@
 import asyncio
 import pigpio
 
+"""
+A simple GPIO pin toggler using pigpio and asyncio.
+Toggles GPIO pins based on keyboard input (Enter key).
+"""
+
+pins = [27]
+# pins = [17, 27, 22]
+
 async def toggle_pin(pin: int, shared_state):
     """Toggle a GPIO pin based on shared state."""
     pi = pigpio.pi()
@@ -25,8 +33,6 @@ async def handle_input(shared_state):
 async def main():
     """Run input handling and pin toggling in parallel."""
     shared_state = {'state': 0}
-    pins = [27]
-    # pins = [17, 27, 22]
     pin_tasks = [toggle_pin(pin, shared_state) for pin in pins]
     
     await asyncio.gather(handle_input(shared_state), *pin_tasks)

playgrounds/integration_test.py

@@ -0,0 +1,86 @@
+import asyncio
+import logging
+from robot_control.src.robot.controller import RobotController, Cell, CellStatus
+from robot_control.src.utils.config import ConfigParser
+from robot_control.src.vision.datamatrix import DataMatrixReader
+from robot_control.src.api.i2c_handler import I2C, MockI2C
+from robot_control.src.vendor.mcp3428 import MCP3428
+from robot_control.src.robot.pump_controller import PumpController
+from robot_control.src.api.gpio import PiGPIO, MockGPIO
+
+logging.basicConfig(
+    level=logging.INFO,
+    format='%(asctime)s - %(module)s - %(levelname)s - %(message)s',
+)
+
+logger = logging.getLogger(__name__)
+
+"""
+This is a test script for the loader system.
+It initializes the robot controller, vision system, and I2C handler,
+"""
+
+class LoaderSystem:
+    def __init__(self):
+        self.config = ConfigParser().config
+        gpio_config = self.config.gpio
+        if gpio_config.debug:
+            self.gpio = MockGPIO()
+        else:
+            self.gpio = PiGPIO([gpio_config.pump_pin, gpio_config.valve_pin], [])
+
+        self.controller = RobotController(self.config, self.gpio)
+        # self.vision = DataMatrixReader(self.config)
+        
+        logger.info("Initializing LoaderSystem")
+        # 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
+        i2c_device_class = MCP3428 if not self.config.i2c.debug else MockI2C
+        self.i2c = I2C(i2c_device_class)
+        self.i2c.initialize()
+        logger.info(f"I2C initialized with {i2c_device_class.__name__}")
+
+        self.pump_controller = PumpController(self.config, self.gpio)
+
+    async def run(self):
+        await self.controller.connect()
+
+        await asyncio.gather(
+            self._loader_loop(),
+            self._poll_i2c_channels()
+        )
+
+    async def _poll_i2c_channels(self):
+        while True:
+            try:
+                readings = await self.i2c.read_channels([1, 3, 4])
+                for channel, value in readings.items():
+                    logger.debug(f"Channel {channel} reading: {value}")
+                    # if channel == 3:  # Pressure reading
+                    #     self.pump_controller.handle_tank_reading(value)
+                    if channel == 4:
+                        state = self.pump_controller.check_endeffector_state(value)
+                        self.controller.set_suction_state(state)
+            except Exception as e:
+                logger.error(f"Error polling I2C channels: {str(e)}")
+            await asyncio.sleep(1)  # Poll every second
+
+    async def _loader_loop(self):
+        await self.controller.pick_cell_from_slot(self.test_slot)
+        await self.controller.dropoff_cell(self.test_dropoff)
+
+        await self.controller.pick_cell_from_feeder()
+        cell = Cell(id="test_cell", status=CellStatus.WAITING)
+        await self.controller.insert_cell_to_slot(cell, self.test_slot)
+
+    def __del__(self):
+        self.pump_controller.cleanup()  # Ensure PumpController cleans up gpio
+
+if __name__ == "__main__":
+    loader_system = LoaderSystem()
+    asyncio.run(loader_system.run())

playgrounds/movement_playground.py

@@ -7,6 +7,13 @@ from robot_control.src.utils.config import RobotConfig, GRBLConfig, MovementConf
 
 from robot_control.src.api.gpio import PiGPIO
 
+"""
+This is a test script for the robot movement system.
+It initializes the GRBL handler and the robot movement system,
+and performs a series of simple A->B movements to test the system.
+Can also use GPIO pins to control pump and valve.
+"""
+
 # grbl_config = GRBLConfig(port="debug", baudrate=115200)
 grbl_config = GRBLConfig(port="/dev/ttyACM0", baudrate=115200)
 test_config = MovementConfig(
@@ -28,14 +35,26 @@ gpio = PiGPIO([pump_pin, valve_pin], [])
 
 y_offset = 130
 
+# Real positions
+# positions = [
+#     (453, 1028 - y_offset, 0),
+#     (453, 1028 - y_offset, 107),
+#     (453, 1028 - y_offset, 0),
+#     (170, 760 - y_offset, 0),
+#     (170, 760 - y_offset, 40),
+#     (170, 760 - y_offset, 0),
+#     (453, 600 - y_offset, 0), # neutral pos
+# ]
+
+# Test positions
 positions = [
-    (453, 1028 - y_offset, 0),
-    (453, 1028 - y_offset, 107),
-    (453, 1028 - y_offset, 0),
-    (170, 760 - y_offset, 0),
-    (170, 760 - y_offset, 40),
-    (170, 760 - y_offset, 0),
-    (453, 600 - y_offset, 0), # neutral pos
+    (100, 100, 0),
+    (100, 100, 107),
+    (100, 100, 0),
+    (150, 150, 0),
+    (150, 150, 40),
+    (150, 150, 0),
+    (100, 150, 0), # neutral pos
 ]
 
 async def wait_for_enter():
@@ -56,7 +75,7 @@ async def perform_movement_sequence():
         print(f"Moving to position {pos}...")
         if idx == 1:
             gpio.set_pin(pump_pin, 1)
-            await asyncio.sleep(5)
+            await asyncio.sleep(4)
             gpio.set_pin(pump_pin, 0)
         if idx == 2:
             gpio.set_pin(valve_pin, 1)
@@ -68,7 +87,6 @@ async def perform_movement_sequence():
 
 async def test_movement():
     await grbl_handler.connect()
-    await movement.perform_homing()
     
     print("Press Enter to start movement sequence (or Ctrl+C to exit)...")
     while True:

playgrounds/mqtt-test.py

@@ -2,6 +2,10 @@ import paho.mqtt.client as mqtt
 import time
 import random  # Replace with actual voltage measurement code
 
+"""
+This script connects to local MQTT broker and publishes random voltage measurements
+"""
+
 BROKER = "localhost"
 PORT = 1883  # Use 8883 for TLS
 TOPIC = "cells_inserted/device1"  # Change for each device

robot_control/config/config.yaml

@@ -1,27 +1,41 @@
 measurement_devices:
   - id: device_1
-    position: [50.0, 50.0, 0.0]
+    position: [378, 898, 107]
     slots:
-      - position: [10.0, 20.0, 0.0]
+      - position: [10, 0, 0]
         occupied: False
         slot_id: 0
-      - position: [20.0, 20.0, 0.0]
+      - position: [31, 0, 0]
         occupied: False
         slot_id: 1
-      - position: [30.0, 20.0, 0.0]
+      - position: [52, 0, 0]
         occupied: False
         slot_id: 2
+      - position: [75, 0, 0]
+        occupied: False
+        slot_id: 3
+      - position: [98, 0, 0]
+        occupied: False
+        slot_id: 4
+      - position: [120, 0, 0]
+        occupied: False
+        slot_id: 5
+      - position: [142, 0, 0]
+        occupied: False
+        slot_id: 6
+      - position: [165, 0, 0]
+        occupied: False
+        slot_id: 7
 
 feeder:
-  position: [0.0, 0.0, 0.0]
-  approach_position: [0.0, 0.0, 20.0]
+  position: [30, 621.5, 47.5]
 
 dropoff_grades:
   - id: accepted
-    position: [0.0, 10.0, 0.0]
+    position: [170, 630, 40]
     capacity_threshold: 0.8
   - id: rejected
-    position: [0.0, 20.0, 0.0]
+    position: [170, 630, 40]
     capacity_threshold: 0
 
 mqtt:
@@ -32,8 +46,8 @@ mqtt:
   keepalive: 60
 
 grbl:
-  # port: /dev/ttyACM0 # or debug
-  port: debug
+  port: /dev/ttyACM0 # or debug
+  # port: debug
   baudrate: 115200
 
 vision:
@@ -52,17 +66,17 @@ vacuum:
   pump_watchdog_timeout_s: 30
 
 gpio:
-  debug: true
+  debug: false
   pump_pin: 17
   valve_pin: 27
 
 i2c:
-  debug: true
+  debug: false
 
 movement:
   speed: 400.0
   acceleration: 20.0
-  safe_height: 25.0
+  safe_height: 0.0
 
 logging:
   level: INFO

robot_control/src/robot/controller.py

@@ -36,7 +36,6 @@ class RobotController:
             self.config.grbl.baudrate
         )
         self.movement = RobotMovement(self.config.movement, self.grbl_handler)
-        self.movement.perform_homing()
 
         self.gpio = gpio_handler
         self.valve_pin = self.config.gpio.valve_pin
@@ -65,6 +64,9 @@ class RobotController:
                 callback=lambda measurement_result: self.work_queue.append(measurement_result)
             )
 
+    async def perform_homing(self):
+        await self.movement.perform_homing()
+
     def activate_endeffector(self) -> bool:
         """Activate the pump end effector via GPIO pin."""
         try:
@@ -147,7 +149,7 @@ class RobotController:
         if not waiting_slot:
             logger.error(f"Measurement ({measurement_result}) could not be processed.")
             return
-        await self.collect_cell_from_slot(waiting_slot)
+        await self.pick_cell_from_slot(waiting_slot)
 
 
         if not cell: # Cell not found, create new
@@ -165,11 +167,14 @@ class RobotController:
             return False
             
         feeder_pos = self.feeder.position
-        safe_pos = self.feeder.approach_position
+        feeder_pos_close = (feeder_pos[0], feeder_pos[1], feeder_pos[2]-2)
+        safe_pos = (feeder_pos[0], feeder_pos[1], self.config.movement.safe_height)
+        feeder_pos_side = (feeder_pos[0]-25, feeder_pos[1], feeder_pos[2])
+        safe_pos_side = (feeder_pos_side[0], safe_pos[1], safe_pos[2])
         logger.info(f"Moving to feeder position (safe) {safe_pos}...")
         await self.movement.move_to_position(*safe_pos)
-        logger.info(f"Moving to feeder position {feeder_pos}...")
-        await self.movement.move_to_position(*feeder_pos)
+        logger.info(f"Moving to feeder position (close) {feeder_pos_close}...")
+        await self.movement.move_to_position(*feeder_pos_close)
 
         # Grip cell
         try:
@@ -177,9 +182,14 @@ class RobotController:
             if not self.suction_state:
                 logger.error("Suction state is off, cannot pick cell")
                 return False
+
+            logger.info(f"Moving to feeder position {feeder_pos}...")
+            await self.movement.move_to_position(*feeder_pos)
             self.gripper_occupied = True
-            logger.info(f"Moving to feeder position (safe) {safe_pos}...")
-            await self.movement.move_to_position(*safe_pos)
+            logger.info(f"Moving to feeder position (side) {feeder_pos_side}...")
+            await self.movement.move_to_position(*feeder_pos_side)
+            logger.info(f"Moving to feeder position (side-safe) {safe_pos_side}...")
+            await self.movement.move_to_position(*safe_pos_side)
             logger.info("Cell picked from feeder")
             return True
         
@@ -237,7 +247,7 @@ class RobotController:
         logger.info(f"Cell {cell.id} inserted to {slot}")
         return True
 
-    async def collect_cell_from_slot(self, slot: SlotConfig):
+    async def pick_cell_from_slot(self, slot: SlotConfig):
         if self.gripper_occupied:
             logger.error("Gripper already occupied")
             return None

robot_control/src/robot/movement.py

@@ -1,7 +1,4 @@
-from typing import Optional
-import time
 import asyncio
-import pigpio
 from robot_control.src.api.grbl_handler import GRBLHandler
 import logging
 from robot_control.src.utils.config import MovementConfig

robot_control/src/robot/pump_controller.py

@@ -3,19 +3,26 @@ from robot_control.src.utils.config import RobotConfig
 from robot_control.src.api.gpio import GPIOInterface
 import logging
 
+logging.basicConfig(
+    level=logging.DEBUG,
+    format='%(asctime)s - %(module)s - %(levelname)s - %(message)s',
+)
+
 logger = logging.getLogger(__name__)
 
 class PumpController:
     def __init__(self, config:RobotConfig, gpio:GPIOInterface):
         self.logger = logger
-        self.pressure_threshold_on = config.vacuum.min_pressure_bar
-        self.pressure_threshold_off = config.vacuum.max_pressure_bar 
+        self.pressure_threshold_on = config.vacuum.max_pressure_bar
+        self.pressure_threshold_off = config.vacuum.min_pressure_bar
+        self.max_pump_time_s = config.vacuum.max_pump_time_s
         self.pump_watchdog_timeout = config.vacuum.pump_watchdog_timeout_s
         self.gripping_threshold = config.vacuum.gripping_threshold_bar
 
         gpio_config = config.gpio
         self.pump_pin = gpio_config.pump_pin
-        self.pump_last_activated = None
+        self.last_activated_time = None
+        self.watchdog_trigger_time = None
         self.pump_active = False
         self.gpio = gpio
 
@@ -25,13 +32,22 @@ class PumpController:
     def handle_tank_reading(self, pressure_bar):
         current_time = time.time()
 
+        if self.watchdog_trigger_time and \
+            (current_time - self.watchdog_trigger_time < self.pump_watchdog_timeout):
+            logger.debug("Pump keeps being deactivated due to watchdog timeout")
+            self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
+            self.pump_active = False
+            self.last_activated_time = None
+            return
+
         # Check if pump needs to be deactivated due to watchdog timeout
-        if self.pump_active and self.pump_last_activated and \
-           (current_time - self.pump_last_activated > self.pump_watchdog_timeout):
+        if self.pump_active and self.last_activated_time and \
+            (current_time - self.last_activated_time > self.max_pump_time_s):
             logger.warning("Pump deactivated due to watchdog timeout")
             self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
             self.pump_active = False
-            self.pump_last_activated = None
+            self.watchdog_trigger_time = current_time
+            self.last_activated_time = None
             return
 
         # Activate pump if pressure is above the threshold (lower value is more vacuum)
@@ -39,14 +55,16 @@ class PumpController:
             logger.info("Activating pump due to pressure threshold")
             self.gpio.set_pin(self.pump_pin, 1)  # Turn on pump
             self.pump_active = True
-            self.pump_last_activated = current_time
+            self.watchdog_trigger_time = None
+            self.last_activated_time = current_time
 
         # Deactivate pump if pressure is below the threshold
         elif self.pump_active and pressure_bar < self.pressure_threshold_off:
             logger.info("Deactivating pump due to pressure threshold")
             self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
             self.pump_active = False
-            self.pump_last_activated = None
+            self.watchdog_trigger_time = None
+            self.last_activated_time = None
 
     def cleanup(self):
         self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump

robot_control/src/utils/config.py

@@ -28,7 +28,6 @@ class DeviceConfig(BaseModel):
 
 class FeederConfig(BaseModel):
     position: Tuple[float, float, float]
-    approach_position: Tuple[float, float, float]
 
 class DropoffGradeConfig(BaseModel):
     id: str

robot_control/src/vendor/mcp3428.py

@@ -32,7 +32,7 @@ from ..api.i2c_handler import I2CDevice
 class MCP3428(I2CDevice):
     default_address = MCP3428_DEFAULT_ADDRESS
 
-    def initialize(self, bus: smbus.SMBus, address: int):
+    def initialize(self, address: int):
         self.bus = smbus.SMBus(1)
         self.address = address
 

tests/test_robot.py

@@ -64,7 +64,7 @@ class TestRobotController:
         assert not slot.occupied
 
     @pytest.mark.asyncio
-    async def test_insert_and_collect_cell(self, robot_controller: RobotController):
+    async def test_insert_and_pick_cell(self, robot_controller: RobotController):
         # Test cell insertion
         cell = Cell(id="test_cell", status=CellStatus.WAITING)
         slot = robot_controller.get_next_free_slot()
@@ -75,7 +75,7 @@ class TestRobotController:
         assert slot.cell_id == cell.id
 
         # Test cell collection
-        collected_cell = await robot_controller.collect_cell_from_slot(slot)
+        collected_cell = await robot_controller.pick_cell_from_slot(slot)
         assert not slot.occupied
         assert slot.cell_id is None
         assert collected_cell == cell.id