feat: edited prepared_feeder_cell and got process mag-->mag_dist-->feeder-->cell measurement--> pick and place robot--> defeeder running

robot_control/config/config.yaml

@@ -28,7 +28,7 @@ measurement_devices:
         slot_id: 7
 
 feeder:
-  robot_pos: [13, 714, 45]
+  robot_pos: [23, 720, 47]
   mag_pos: 
     pos_mm: 46
     rot_deg: 153
@@ -36,7 +36,7 @@ feeder:
   max_num_cells: 10
 
 defeeder:
-  robot_pos: [1, 526, 72]
+  robot_pos: [12, 526, 72]
   mag_pos: 
     pos_mm: 0
     rot_deg: -90
@@ -44,7 +44,7 @@ defeeder:
 
 feeder_magazines:
   - mag_pos:
-      pos_mm: 298
+      pos_mm: 297
       rot_deg: 90
     max_num_cells: 70
   - mag_pos:
@@ -91,8 +91,8 @@ mqtt:
   keepalive: 60
 
 grbl:
-  #port: /dev/ttyACM0 # or debug
-  port: debug
+  port: /dev/ttyUSB0 # or debug
+  #port: debug
   baudrate: 115200
 
 vision:
@@ -145,6 +145,7 @@ i2c:
 movement:
   speed_mmmin: 15000
   safe_height: 0.0
+  safe_length: 200.0
 
 logging:
   level: INFO

robot_control/src/robot/controller.py

@@ -8,6 +8,7 @@ from robot_control.src.api.mqtt_handler import MQTTHandler, MeasurementResult
 from robot_control.src.api.grbl_handler import GRBLHandler
 from robot_control.src.api.gpio import GPIOInterface
 import asyncio
+import time
 
 logger = logging.getLogger(__name__)
 
@@ -230,12 +231,14 @@ class RobotController:
             except Exception as e:
                 logger.warning(f"Failed to get from feeder_queue: {e}")
         feeder_pos = self.feeder.robot_pos
-        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])
+        feeder_pos_close = (feeder_pos[0], feeder_pos[1], self.config.movement.safe_height)
+        safe_pos = (self.config.movement.safe_length, feeder_pos[1], self.config.movement.safe_height)
+        feeder_pos_side = (feeder_pos[0]-23, 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}...")
+        time.sleep(0.25)
         await self.movement.move_to_position(*safe_pos)
+        time.sleep(0.25)
         logger.info(f"Moving to feeder position (close) {feeder_pos_close}...")
         await self.movement.move_to_position(*feeder_pos_close)
 
@@ -250,9 +253,14 @@ class RobotController:
             await self.movement.move_to_position(*feeder_pos)
             self.gripper_occupied = True
             logger.info(f"Moving to feeder position (side) {feeder_pos_side}...")
+            time.sleep(0.25)
             await self.movement.move_to_position(*feeder_pos_side)
             logger.info(f"Moving to feeder position (side-safe) {safe_pos_side}...")
+            time.sleep(0.25)
             await self.movement.move_to_position(*safe_pos_side)
+            logger.info(f"Moving to feeder position (safe) {safe_pos}...")
+            time.sleep(0.25)
+            await self.movement.move_to_position(*safe_pos)
             logger.info("Cell picked from feeder")
             return True
         
@@ -416,9 +424,12 @@ class RobotController:
                 logger.warning(f"Failed to put to defeeder_queue: {e}")
 
         pos = self.config.defeeder.robot_pos
-        safe_pos = (pos[0], pos[1], self.config.movement.safe_height)
-        logger.debug(f"Moving to dropoff (safe) {safe_pos}...")
+        safe_pos = (self.config.movement.safe_length, pos[1], self.config.movement.safe_height)
+        safe_dropoff_pos = (pos[0], pos[1], self.config.movement.safe_height)
+        logger.debug(f"Moving to position (safe) {safe_pos}...")
         await self.movement.move_to_position(*safe_pos)
+        logger.debug(f"Moving to safe dropoff position {safe_dropoff_pos}...")
+        await self.movement.move_to_position(*safe_dropoff_pos)
         logger.debug(f"Moving to dropoff position {pos}...")
         await self.movement.move_to_position(*pos)
         # Release cell
@@ -427,6 +438,8 @@ class RobotController:
         self.gripper_occupied = False
 
         # Move back to safe height
+        logger.debug(f"Moving to dropoff position (safe) {safe_dropoff_pos}...")
+        await self.movement.move_to_position(*safe_dropoff_pos)
         logger.debug(f"Moving to dropoff position (safe) {safe_pos}...")
         await self.movement.move_to_position(*safe_pos)
         logger.info(f"Cell dropped off at defeeder {defeeder_mag.name}")
@@ -445,14 +458,15 @@ class RobotController:
         logger.info(f"Cell {cell_id_str} voltage({voltage}) is good")
         return True
 
-    async def cprepare_feeder_cell(self) -> bool:
+    async def prepare_feeder_cell(self) -> bool:
         """
         Handles the process of preparing a cell in the feeder. Loops until a suitable cell is found or no cell is detected.
         Returns True if a cell is prepared, otherwise False.
         """
         io_conf = self.config.gpio
         while not self.next_cell_id:
-            self.next_cell_id = self.vision.read_datamatrix()
+            #self.next_cell_id = self.vision.read_datamatrix() commented out for test purposes Y
+            self.next_cell_id = 1
             if not self.next_cell_id:
                 logger.debug("No cell detected")
                 return False  # No cell detected
@@ -468,9 +482,9 @@ class RobotController:
                 return True # Desired case!
             # Discard cell directly from feeder
             logger.info(f"Cell {self.next_cell_id} voltage({cell_v}) is bad, discarding cell")
-            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_end, 1000)
-            await asyncio.sleep(0.25)  # Wait for cell to be ejected
-            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_start, 250)
+            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_end, 200)
+            await asyncio.sleep(1)  # Wait for cell to be ejected
+            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_start, 1000)
             try:
                 self.next_cell_id = ""
                 await self.pick_cell_from_feeder()

robot_control/src/utils/config.py

@@ -147,6 +147,7 @@ class I2CConfig(BaseModel):
 class MovementConfig(BaseModel):
     speed_mmmin: int = Field(default=400, ge=0)
     safe_height: float = Field(default=25.0, ge=0.0)
+    safe_length: float = Field(default=100.0, ge=0.0)
 
 class LoggingConfig(BaseModel):
     level: str = Field(default="INFO")