feat: edited move_mag_to_pos, so that rotation will be done by servo instead of stepper

.vscode/launch.json

@@ -25,6 +25,13 @@
             "module": "playgrounds.i2c_playground",
             "console": "integratedTerminal"
         },
+        {
+            "name": "Camera Playground",
+            "type": "debugpy",
+            "request": "launch",
+            "module": "playgrounds.camera_playground",
+            "console": "integratedTerminal"
+        },
         {
             "name": "Mag Dist Playground",
             "type": "debugpy",

playgrounds/mag_dist_playground.py

@@ -10,7 +10,7 @@ def main():
     # You may need to adjust these to your environment
     config = ConfigParser().config
     gpio_config = config.gpio
-   # gpio_config.mag_dist_rot_pin = 14
+    gpio_config.mag_dist_rot_servo_pin = 6
     if gpio_config.debug:
         gpio = MockGPIO()
     else:
@@ -21,8 +21,12 @@ def main():
     print("MagDistributor Playground")
     print("Commands:")
     print("  h - home")
-    print("  m <pos_mm> <rot_deg> - move to position (mm, deg)")
+    print("  m <pos_mm> <rot_deg> [speed_mmmin] [servo_speed_ms] - move to position")
     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")
 
     while True:
         cmd = input("Enter command: ").strip()
@@ -36,13 +40,31 @@ def main():
             print("Homed.")
         elif cmd.startswith("m "):
             try:
-                _, pos_mm, rot_deg = cmd.split()
-                pos_mm = float(pos_mm)
-                rot_deg = float(rot_deg)
-                mag_dist.move_mag_distributor_at_pos(pos_mm, rot_deg)
-                print(f"Moved to {pos_mm} mm, {rot_deg} deg.")
+                parts = cmd.split()
+                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]")
         else:
             print("Unknown command.")
 

robot_control/config/config.yaml

@@ -59,8 +59,8 @@ feeder_magazines:
 # Order of magazines is implying the priority
 defeeder_magazines:
   - mag_pos:
-      pos_mm: 100
-      rot_deg: -90
+      pos_mm: 222
+      rot_deg: 5
     max_num_cells: 70
     health_range: [60, 100]
     name: accept
@@ -129,13 +129,17 @@ gpio:
   mag_dist_rot_step_pin: -1
   mag_dist_rot_en_pin: -1
   mag_dist_rot_limit_pin: -1
+
   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_defeeder: 60 
 
   mag_dist_sensor_pin: -1
 
 i2c:
-  debug: false
+  debug: true
 
 movement:
   speed_mmmin: 15000

robot_control/src/robot/mag_distributor.py

@@ -20,7 +20,11 @@ class MagDistributor:
         #self.rot_step_pin = gpio_conf.mag_dist_rot_step_pin
         #self.rot_en_pin = gpio_conf.mag_dist_rot_en_pin
         self.rot_servo_pin = gpio_conf.mag_dist_rot_servo_pin
-        #print(self.rot_servo_pin)
+        self.mag_dist_rot_servo_pin = gpio_conf.mag_dist_rot_servo_pin
+        self.mag_dist_servo_angle_magazin = gpio_conf.mag_dist_servo_angle_magazin
+        self.mag_dist_servo_angle_neutral = gpio_conf.mag_dist_servo_angle_neutral
+        self.mag_dist_servo_angle_feeder = gpio_conf.mag_dist_servo_angle_feeder
+        self.mag_dist_servo_angle_defeeder = gpio_conf.mag_dist_servo_angle_defeeder
 
         # Endstop pins
         self.linear_limit_pin = gpio_conf.mag_dist_pos_limit_pin
@@ -94,6 +98,8 @@ class MagDistributor:
         """
         if self.debug:
             return
+        # bring servo in neutral position to avoid crash
+        self.gpio.set_servo_angle_smooth(self.mag_dist_rot_servo_pin, self.mag_dist_servo_angle_neutral, 1000)
         # Move in negative direction until endstop is triggered
         for _ in range(max_steps):
             if not self.gpio.get_pin(endstop_pin):
@@ -104,7 +110,7 @@ class MagDistributor:
             raise RuntimeError(f"{axis} axis homing failed")
         self.logger.info(f"{axis} axis homed.")
 
-    def move_mag_distributor_at_pos(self, pos_target, rot_target, step_delay=200):
+    def move_mag_distributor_at_pos(self, pos_target, rot_target, servo_speed, step_delay=200):
         """
         Move the magazine distributor to the specified linear and rotational position.
         """
@@ -122,6 +128,9 @@ class MagDistributor:
             self.gpio.do_step(self.linear_dir_pin, self.linear_step_pin, linear_steps, step_delay, linear_dir)
             self.curr_pos_mm = pos_target
 
+        
+        self.gpio.set_servo_angle_smooth(self.mag_dist_rot_servo_pin, rot_target, servo_speed)
+
         # rot_steps = int(abs(rot_target - self.curr_rot_deg) * self.full_rot_steps / 360)
         # rot_dir = True if rot_target > self.curr_rot_deg else False
         # if rot_steps > 0:

robot_control/src/utils/config.py

@@ -133,6 +133,10 @@ class GPIOConfig(BaseModel):
     mag_dist_rot_en_pin: int = Field(default=0, ge=-1, le=27)
     mag_dist_rot_limit_pin: int = Field(ge=-1, le=27)
     mag_dist_rot_servo_pin: int = Field(ge=-1, le=27)
+    mag_dist_servo_angle_magazin: float = Field(ge=0, le=180)
+    mag_dist_servo_angle_neutral: float = Field(ge=0, le=180)
+    mag_dist_servo_angle_feeder: float = Field(ge=0, le=180)
+    mag_dist_servo_angle_defeeder: float = Field(ge=0, le=180)
 
     mag_dist_sensor_pin: int = Field(ge=-1, le=27)