ref: remove converstion in PumpController; add GPIO handling in movement playground; update GRBL configuration; fix logging in pump controller

main.py

@@ -34,7 +34,7 @@ class LoaderSystem:
         self.i2c.initialize()
         self.logger.info(f"I2C initialized with {i2c_device_class.__name__}")
 
-        self.pump_controller = PumpController(self.config, self.gpio, self.logger)
+        self.pump_controller = PumpController(self.config, self.gpio)
 
     async def run(self):
         await self.controller.connect()

playgrounds/gpio_playground.py

@@ -1,30 +1,39 @@
 import asyncio
 import pigpio
 
-async def toggle_pin(pin: int):
-    """Toggle a GPIO pin indefinitely."""
+async def toggle_pin(pin: int, shared_state):
+    """Toggle a GPIO pin based on shared state."""
     pi = pigpio.pi()
     if not pi.connected:
         raise RuntimeError(f"Failed to connect to pigpio daemon for pin {pin}")
     
-    state = 0
     try:
         while True:
-            state = 1 - state  # Toggle state between 0 and 1
-            pi.write(pin, state)
-            await asyncio.sleep(1)  # Wait 1 second between toggles
+            pi.write(pin, shared_state['state'])
+            await asyncio.sleep(0.1)  # Small delay to prevent CPU overuse
     finally:
-        pi.write(pin, 0)  # Ensure the pin is turned off
+        pi.write(pin, 0)
         pi.stop()
 
+async def handle_input(shared_state):
+    """Handle keyboard input to toggle pins."""
+    while True:
+        await asyncio.get_event_loop().run_in_executor(None, input, "Press Enter to toggle pins...")
+        shared_state['state'] = 1 - shared_state['state']  # Toggle state
+        print(f"Toggled pins to: {'ON' if shared_state['state'] else 'OFF'}")
+
 async def main():
-    """Run multiple toggle_pin tasks in parallel."""
-    pins = [17, 27, 22]  # Replace with the GPIO pins you want to toggle
-    tasks = [toggle_pin(pin) for pin in pins]
-    await asyncio.gather(*tasks)
+    """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)
 
 if __name__ == "__main__":
     try:
         asyncio.run(main())
     except KeyboardInterrupt:
         print("Exiting...")
+

playgrounds/movement_playground.py

@@ -1,12 +1,14 @@
 import asyncio
+import time
 from dataclasses import dataclass
 from robot_control.src.robot.movement import RobotMovement
 from robot_control.src.api.grbl_handler import GRBLHandler
 from robot_control.src.utils.config import RobotConfig, GRBLConfig, MovementConfig, GPIOConfig, MQTTConfig
 
+from robot_control.src.api.gpio import PiGPIO
 
 # grbl_config = GRBLConfig(port="debug", baudrate=115200)
-grbl_config = GRBLConfig(port="/dev/ttyACM1", baudrate=115200)
+grbl_config = GRBLConfig(port="/dev/ttyACM0", baudrate=115200)
 test_config = MovementConfig(
         speed=1000,
         safe_height=10,
@@ -19,6 +21,11 @@ grbl_handler = GRBLHandler(
 )
 movement = RobotMovement(test_config, grbl_handler)
 
+pump_pin = 17
+valve_pin = 27
+
+gpio = PiGPIO([pump_pin, valve_pin], [])
+
 y_offset = 130
 
 positions = [
@@ -45,8 +52,18 @@ async def wait_for_enter():
     await event.wait()
 
 async def perform_movement_sequence():
-    for pos in positions:
+    for idx, pos in enumerate(positions):
         print(f"Moving to position {pos}...")
+        if idx == 1:
+            gpio.set_pin(pump_pin, 1)
+            await asyncio.sleep(5)
+            gpio.set_pin(pump_pin, 0)
+        if idx == 2:
+            gpio.set_pin(valve_pin, 1)
+            await asyncio.sleep(0.5)
+        if idx == 5:
+            gpio.set_pin(valve_pin, 0)
+            await asyncio.sleep(0.5)
         await movement.move_to_position(*pos)
 
 async def test_movement():

robot_control/config/config.yaml

@@ -12,19 +12,6 @@ measurement_devices:
         occupied: False
         slot_id: 2
 
-  - id: device_2
-    position: [100.0, 50.0, 0.0]
-    slots:
-      - position: [10.0, 20.0, 0.0]
-        occupied: False
-        slot_id: 0
-      - position: [20.0, 20.0, 0.0]
-        occupied: False
-        slot_id: 1
-      - position: [30.0, 20.0, 0.0]
-        occupied: False
-        slot_id: 2
-
 feeder:
   position: [0.0, 0.0, 0.0]
   approach_position: [0.0, 0.0, 20.0]
@@ -58,10 +45,10 @@ vision:
   bbox: [100, 100,  200, 200]  # (x, y, width, height)
 
 vacuum:
-  min_pressure_bar: -0.85
-  max_pressure_bar: -0.35
-  max_pump_time_s: 30
-  gripping_threshold_bar: -0.35
+  min_pressure_bar: 0.14
+  max_pressure_bar: 0.7
+  max_pump_time_s: 20
+  gripping_threshold_bar: 0.7
   pump_watchdog_timeout_s: 30
 
 gpio:

robot_control/src/api/gpio.py

@@ -31,6 +31,9 @@ class PiGPIO(GPIOInterface):
 
     def get_pin(self, pin: int) -> int:
         return self.pi.read(pin)
+    
+    def __del__(self):
+        self.pi.stop()
 
 class MockGPIO(GPIOInterface):
     def set_pin(self, pin: int, value: int) -> None:

robot_control/src/api/grbl_handler.py

@@ -118,6 +118,12 @@ class GRBLHandler:
             if response and "Idle" in response:
                 logger.debug("Movement complete.\nContinuing...")
                 break
+
+            if response and "ALARM" in response:
+                if ":1" in response:
+                    raise RuntimeError("Hard Limit was triggered!")
+                else:
+                    raise RuntimeError(f"Grbl threw ALARM: {response}")
                     
             now = datetime.datetime.now()
             if (now - start).total_seconds() > timeout_s:

robot_control/src/robot/pump_controller.py

@@ -1,10 +1,12 @@
 import time
-import pigpio
 from robot_control.src.utils.config import RobotConfig
 from robot_control.src.api.gpio import GPIOInterface
+import logging
+
+logger = logging.getLogger(__name__)
 
 class PumpController:
-    def __init__(self, config:RobotConfig, gpio:GPIOInterface, logger):
+    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 
@@ -17,22 +19,16 @@ class PumpController:
         self.pump_active = False
         self.gpio = gpio
 
-    def _convert_to_bar(self, value: float) -> float:
-        """Convert sensor value (0-1.6) to bar (-1 to 1)"""
-        return (value - 0.8) * 2.5
-
-    def check_endeffector_state(self, value) -> bool:
-        pressure_bar = self._convert_to_bar(value)
+    def check_endeffector_state(self, pressure_bar) -> bool:
         return pressure_bar < self.gripping_threshold        
 
-    def handle_tank_reading(self, value):
-        pressure_bar = self._convert_to_bar(value)
+    def handle_tank_reading(self, pressure_bar):
         current_time = time.time()
 
         # 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):
-            self.logger.warning("Pump deactivated due to watchdog timeout")
+            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
@@ -40,19 +36,18 @@ class PumpController:
 
         # Activate pump if pressure is above the threshold (lower value is more vacuum)
         if not self.pump_active and pressure_bar > self.pressure_threshold_on:
-            self.logger.info("Activating pump due to pressure threshold")
+            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
 
         # Deactivate pump if pressure is below the threshold
         elif self.pump_active and pressure_bar < self.pressure_threshold_off:
-            self.logger.info("Deactivating pump due to pressure threshold")
+            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
 
     def cleanup(self):
-        self.pi.write(self.pump_pin, 0)  # Ensure pump is off
-        self.pi.stop()  # Disconnect from pigpio daemon
-        self.logger.info("Pigpio cleanup completed")
+        self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
+        logger.info("Pigpio cleanup completed")