feat: saved current session

playgrounds/gpio_playground.py

@@ -6,7 +6,7 @@ A simple GPIO pin toggler using pigpio and asyncio.
 Toggles GPIO pins based on keyboard input (Enter key).
 """
 
-pins = [22]
+pins = [17]
 # pins = [17, 27, 22]
 
 async def toggle_pin(pin: int, shared_state):

playgrounds/integration_test.py

@@ -110,40 +110,42 @@ class LoaderSystem:
             await wait_for_enter()
             # Feeding with MagDistributor
             ###########################
+            self.logger.info("Homing the Magazin Distributor Axis...")
+            await self.mag_distributor.home()
             self.logger.info("Picking up a cell from magazine and placing it into feeder using MagDistributor...")
             self.mag_distributor.mag_to_feeder()
             await self.feeder_queue.put(1)
             self.logger.info("Done.")
 
-            await wait_for_enter()
-            # Prepare Feeder Cell
-            ##############################
-            self.logger.info("Preparing feeder cell...")
-            await self.controller.prepare_feeder_cell()
-            self.logger.info("Done.")
-
-            await wait_for_enter()
-            # Feeder -> Slot
-            ##############################
-            self.logger.info("Picking up a cell from feeder and placing it into slot...")
-            await self.controller.pick_cell_from_feeder()
-            cell = Cell(id=1, status=CellStatus.WAITING)
-            await self.controller.insert_cell_to_slot(cell, self.test_drop_slot)
-
-            await wait_for_enter()
-            # Slot -> Defeeder
-            ##############################
-            self.logger.info("Picking up a cell from slot and placing it into defeeder...")
-            await self.controller.pick_cell_from_slot(self.test_pickup_slot)
-            await self.controller.dropoff_cell()
-            self.logger.info("Done.")
-
-            await wait_for_enter()
-            # Defeeding with MagDistributor
-            ###########################
-            self.logger.info("Defeeding a cell from feeder to magazine using MagDistributor...")
-            self.mag_distributor.defeeder_to_mag(self.config.defeeder_magazines[0])
-            self.logger.info("Done.")
+            # await wait_for_enter()
+            # # Prepare Feeder Cell
+            # ##############################
+            # self.logger.info("Preparing feeder cell...")
+            # await self.controller.prepare_feeder_cell()
+            # self.logger.info("Done.")
+
+            # await wait_for_enter()
+            # # Feeder -> Slot
+            # ##############################
+            # self.logger.info("Picking up a cell from feeder and placing it into slot...")
+            # await self.controller.pick_cell_from_feeder()
+            # cell = Cell(id=1, status=CellStatus.WAITING)
+            # await self.controller.insert_cell_to_slot(cell, self.test_drop_slot)
+
+            # await wait_for_enter()
+            # # Slot -> Defeeder
+            # ##############################
+            # self.logger.info("Picking up a cell from slot and placing it into defeeder...")
+            # await self.controller.pick_cell_from_slot(self.test_pickup_slot)
+            # await self.controller.dropoff_cell()
+            # self.logger.info("Done.")
+
+            # await wait_for_enter()
+            # # Defeeding with MagDistributor
+            # ###########################
+            # self.logger.info("Defeeding a cell from feeder to magazine using MagDistributor...")
+            # self.mag_distributor.defeeder_to_mag(self.config.defeeder_magazines[0])
+            # self.logger.info("Done.")
 
             self.logger.info("\nPress Enter to repeat sequence (or Ctrl+C to exit)...")
 

playgrounds/motor_playground.py

@@ -27,7 +27,7 @@ def move_motor(steps, direction=True, step_delay_us=10):
     pi.write(EN_PIN, 0)
 
     # Set direction
-    pi.write(DIR_PIN, 1 if direction else 0)
+    pi.write(DIR_PIN, 0 if direction else 1)
 
     # Create pulse waveform
     pulses = []

robot_control/config/config.yaml

@@ -30,8 +30,8 @@ measurement_devices:
 feeder:
   robot_pos: [13, 714, 45]
   mag_pos: 
-    pos_mm: 0
-    rot_deg: 0
+    pos_mm: 46
+    rot_deg: 153
   min_voltage: 2.0
   max_num_cells: 10
 
@@ -44,7 +44,7 @@ defeeder:
 
 feeder_magazines:
   - mag_pos:
-      pos_mm: 100
+      pos_mm: 222
       rot_deg: 90
     max_num_cells: 70
   - mag_pos:
@@ -59,7 +59,7 @@ feeder_magazines:
 # Order of magazines is implying the priority
 defeeder_magazines:
   - mag_pos:
-      pos_mm: 222
+      pos_mm: 100
       rot_deg: 5
     max_num_cells: 70
     health_range: [60, 100]
@@ -79,8 +79,8 @@ defeeder_magazines:
 
 mag_distributor:
     debug: False
-    max_speed_mmmin: 10000
-    home_speed_mmmin: 2000
+    max_speed_mmmin: 100
+    home_speed_mmmin: 10
     length_mm: 500
 
 mqtt:
@@ -91,8 +91,8 @@ mqtt:
   keepalive: 60
 
 grbl:
-  port: /dev/ttyACM0 # or debug
-  # port: debug
+  #port: /dev/ttyACM0 # or debug
+  port: debug
   baudrate: 115200
 
 vision:

robot_control/src/api/gpio.py

@@ -111,8 +111,16 @@ class PiGPIO(GPIOInterface):
         current_angle = self._servo_angles[pin]
 
         target_angle_deg = max(0, min(180, target_angle_deg))
+        
+        # Check for zero difference to prevent division by zero
+        angle_diff = abs(target_angle_deg - current_angle)
+        if angle_diff == 0:
+            self.set_servo_angle(pin, target_angle_deg)
+            self._servo_angles[pin] = target_angle_deg
+            return
+            
         step = 1 if target_angle_deg > current_angle else -1
-        delay = duration_ms / int(abs(target_angle_deg-current_angle)) / 1000
+        delay = duration_ms / angle_diff / 1000
         for angle in range(int(current_angle), int(target_angle_deg), step):
             self.set_servo_angle(pin, angle)
             time.sleep(delay)

robot_control/src/robot/mag_distributor.py

@@ -166,17 +166,20 @@ class MagDistributor:
         # Check if cell can be picked from magazines, if not add to empty magazines and continue
         for mag_id in mags_to_try:
             pos = magazines[mag_id].mag_pos
-            self.move_mag_distributor_at_pos(pos.pos_mm, 0)
-            self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
+            self.move_mag_distributor_at_pos(0, self.mag_dist_servo_angle_neutral, 1000)
+            self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
+            self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_magazin, 1000)
+            self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
             time.sleep(1)
             # Check cell pick sensor
             if self.mag_dist_sensor_pin and self.gpio.get_pin(self.mag_dist_sensor_pin):
-                self.move_mag_distributor_at_pos(pos.pos_mm, 0)
                 pos = self.config.feeder.mag_pos
-                self.move_mag_distributor_at_pos(pos.pos_mm, 0)
-                self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
+                self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_feeder, 1000)
+                self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
                 time.sleep(1)
-                self.move_mag_distributor_at_pos(pos.pos_mm, 0)
+                pos_feeder = self.config.feeder.mag_pos
+                self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
+                self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_feeder, 1000)
                 self.logger.info(f"Cell successfully picked from magazine {mag_id} and deposited to feeder.")
                 self.current_magazine = mag_id  # update current
                 return