feat: saved current session

playgrounds/integration_test.py

@@ -117,12 +117,12 @@ class LoaderSystem:
             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()
+            # 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

playgrounds/interactive_movement.py

@@ -17,7 +17,7 @@ pump_pin = 17
 valve_pin = 27
 servo_pin = 13
 
-grbl_config = GRBLConfig(port="/dev/ttyACM0", baudrate=115200)
+grbl_config = GRBLConfig(port="/dev/ttyUSB0", baudrate=115200)
 test_config = MovementConfig(
     speed_mmmin=15000,
     safe_height=10

playgrounds/mag_dist_playground.py

@@ -22,11 +22,14 @@ def main():
     print("Commands:")
     print("  h - home")
     print("  m <pos_mm> <rot_deg> [speed_mmmin] [servo_speed_ms] - move to position")
+    print("  s <servo_id> <angle> [speed_ms] - move servo (1: pin 6, 2: pin 13)")
     print("  q - quit")
     print("Examples:")
     print("  m -50 60        - move to -50mm, 60° with default speeds")
     print("  m -50 60 1000   - move to -50mm, 60° at 1000 mm/min")
     print("  m -50 60 1000 500 - move to -50mm, 60° at 1000 mm/min, servo 500ms")
+    print("  s 1 90 1000     - move servo 1 to 90° in 1000ms")
+    print("  s 2 45          - move servo 2 to 45° with default speed")
 
     while True:
         cmd = input("Enter command: ").strip()
@@ -44,27 +47,43 @@ def main():
                 if len(parts) < 3:
                     print("Usage: m <pos_mm> <rot_deg> [speed_mmmin] [servo_speed_ms]")
                     continue
-                    
                 pos_mm = float(parts[1])
                 rot_deg = float(parts[2])
                 speed_mmmin = float(parts[3]) if len(parts) > 3 else None
                 servo_speed_ms = float(parts[4]) if len(parts) > 4 else 1000
-                
                 print(f"Moving to {pos_mm} mm, {rot_deg}°" + 
                       (f" at {speed_mmmin} mm/min" if speed_mmmin else " (default speed)") +
                       f" (servo: {servo_speed_ms}ms)")
-                
                 if speed_mmmin:
-                    # Convert speed to step delay
                     step_delay = mag_dist._speed_to_step_delay(speed_mmmin)
                     mag_dist.move_mag_distributor_at_pos(pos_mm, rot_deg, servo_speed_ms, step_delay)
                 else:
                     mag_dist.move_mag_distributor_at_pos(pos_mm, rot_deg, servo_speed_ms)
-                    
                 print(f"✓ Moved to {pos_mm} mm, {rot_deg}°")
             except Exception as e:
                 print(f"Invalid move command: {e}")
                 print("Usage: m <pos_mm> <rot_deg> [speed_mmmin] [servo_speed_ms]")
+        elif cmd.startswith("s "):
+            try:
+                parts = cmd.split()
+                if len(parts) < 3:
+                    print("Usage: s <servo_id> <angle> [speed_ms]")
+                    continue
+                servo_id = int(parts[1])
+                angle = float(parts[2])
+                speed_ms = float(parts[3]) if len(parts) > 3 else 1000
+                if servo_id == 1:
+                    servo_pin = 6
+                elif servo_id == 2:
+                    servo_pin = 13
+                else:
+                    print(f"Unknown servo_id: {servo_id}. Use 1 or 2.")
+                    continue
+                gpio.set_servo_angle_smooth(servo_pin, angle, speed_ms)
+                print(f"✓ Moved servo {servo_id} (pin {servo_pin}) to {angle}° in {speed_ms}ms")
+            except Exception as e:
+                print(f"Invalid servo command: {e}")
+                print("Usage: s <servo_id> <angle> [speed_ms]")
         else:
             print("Unknown command.")
 

playgrounds/mag_to_feeder_moving_sequence.py

@@ -71,10 +71,10 @@ def execute_sequence():
     commands = [
         ("S", 1, 60, 1000),     # S 1 40 1000
         ("H",),                 # Homing sequence
-        ("M", -222, 0, 10),    # M -223 0 200 (increased speed from 100 to 200)
+        ("M", 298, 0, 10),    # M -223 0 200 (increased speed from 100 to 200)
         ("S", 1, 5, 250),       # S 1 0 250
         ("S", 1, 60, 250),      # S 1 40 250
-        ("M", -46, 0, 10),     # M -46 0 150 (increased speed from 100 to 150)
+        ("M", 46, 0, 10),     # M -46 0 150 (increased speed from 100 to 150)
         ("S", 2, 15, 500),      # S 2 35 250
         ("S", 1, 153, 200),     # S 1 153 200
         ("S", 2, 88, 500),     # S 2 90 1000

robot_control/config/config.yaml

@@ -2,7 +2,7 @@ measurement_devices:
   - id: device_1
     position: [1, 802, 70]
     slots:
-      - position: [0, 0, 0]
+      - position: [5, 802, 65]
         occupied: False
         slot_id: 0
       - position: [21, 0, 0]
@@ -36,7 +36,7 @@ feeder:
   max_num_cells: 10
 
 defeeder:
-  robot_pos: [40, 530, 68]
+  robot_pos: [1, 526, 72]
   mag_pos: 
     pos_mm: 0
     rot_deg: -90
@@ -44,7 +44,7 @@ defeeder:
 
 feeder_magazines:
   - mag_pos:
-      pos_mm: 222
+      pos_mm: 298
       rot_deg: 90
     max_num_cells: 70
   - mag_pos:
@@ -116,9 +116,10 @@ gpio:
   valve_pin: 27
 
   measure_probe_oping: 22  
-  measure_servo_pin: 11
-  measure_servo_angle_start: 0 
-  measure_servo_angle_end: 45
+  measure_servo_pin: 13
+  measure_servo_angle_start: 15 
+  measure_servo_angle_probe: 88
+  measure_servo_angle_end: 130
 
   mag_dist_pos_dir_pin: 11
   mag_dist_pos_step_pin: 9
@@ -133,13 +134,13 @@ gpio:
   mag_dist_rot_servo_pin: 6
   mag_dist_servo_angle_magazin: 5 
   mag_dist_servo_angle_neutral: 60
-  mag_dist_servo_angle_feeder: 153
+  mag_dist_servo_angle_feeder: 155
   mag_dist_servo_angle_defeeder: 60 
 
   mag_dist_sensor_pin: -1
 
 i2c:
-  debug: true
+  debug: false
 
 movement:
   speed_mmmin: 15000

robot_control/src/api/gpio.py

@@ -144,7 +144,8 @@ class PiGPIO(GPIOInterface):
             direction: True for forward, False for reverse
         """
         #print(f"[CHUNKED] Moving {steps} steps, direction: {direction}, delay: {step_delay_us}us")
-        
+        #self.set_pin(en_pin, 0)
+
         # Set direction once at the beginning
         self.set_pin(dir_pin, 1 if direction else 0)
         
@@ -156,7 +157,7 @@ class PiGPIO(GPIOInterface):
             chunk_size = min(remaining_steps, self.MAX_STEPS_PER_CHUNK)
             chunk_count += 1
             
-            print(f"  Chunk {chunk_count}: {chunk_size} steps (remaining: {remaining_steps})")
+            #print(f"  Chunk {chunk_count}: {chunk_size} steps (remaining: {remaining_steps})")
             
             # Create pulse waveform for this chunk
             pulses = []
@@ -186,7 +187,7 @@ class PiGPIO(GPIOInterface):
 
         # Clear direction pin
         self.set_pin(dir_pin, 0)
-        print(f"[CHUNKED] Movement complete: {steps} steps in {chunk_count} chunks")
+        #print(f"[CHUNKED] Movement complete: {steps} steps in {chunk_count} chunks")
 
     def move_to_position_mm(self, dir_pin: int, step_pin: int, target_position_mm: float, 
                            current_position_mm: float, steps_per_mm: float, 

robot_control/src/robot/controller.py

@@ -445,7 +445,7 @@ class RobotController:
         logger.info(f"Cell {cell_id_str} voltage({voltage}) is good")
         return True
 
-    async def prepare_feeder_cell(self) -> bool:
+    async def cprepare_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.

robot_control/src/robot/mag_distributor.py

@@ -104,7 +104,7 @@ class MagDistributor:
         for _ in range(max_steps):
             if not self.gpio.get_pin(endstop_pin):
                 break
-            self.gpio.do_step(dir_pin, step_pin, 1, step_delay, direction=True)
+            self.gpio.do_step(dir_pin, step_pin, 1, step_delay, True)
             await asyncio.sleep(0.001)
         else:
             raise RuntimeError(f"{axis} axis homing failed")
@@ -180,11 +180,15 @@ class MagDistributor:
             if self.mag_dist_sensor_pin and self.gpio.get_pin(self.mag_dist_sensor_pin):
                 pos = self.config.feeder.mag_pos
                 # feeder, feeder
-                self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_feeder, 250)
+                self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_feeder, 200)
                 time.sleep(0.5)
                 # feeder, neutral
                 self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
                 time.sleep(1)
+                self.gpio.set_servo_angle_smooth(self.config.gpio.measure_servo_pin, self.config.gpio.measure_servo_angle_start, 500)
+                time.sleep(2)
+                self.gpio.set_servo_angle_smooth(self.config.gpio.measure_servo_pin, self.config.gpio.measure_servo_angle_probe, 500)
+
                 self.logger.info(f"Cell successfully picked from magazine {mag_id} and deposited to feeder.")
                 self.current_magazine = mag_id  # update current
                 return

robot_control/src/utils/config.py

@@ -120,6 +120,7 @@ class GPIOConfig(BaseModel):
     measure_probe_oping: int = Field(ge=-1, le=27)
     measure_servo_pin: int = Field(ge=-1, le=27)
     measure_servo_angle_start: float = Field(ge=0, le=180)
+    measure_servo_angle_probe: float = Field(ge=0, le=180)
     measure_servo_angle_end: float = Field(ge=0, le=180)
 
     # Mag dist