feat: added voltage_test playground to meaure cell voltage

flask_app.py

@@ -0,0 +1,29 @@
+from flask import Flask, render_template, jsonify
+import status
+
+app = Flask(__name__)
+
+@app.route("/")
+def dashboard():
+    return render_template("dashboard.html")
+
+@app.route("/api/status")
+def get_status():
+    with status.status_lock:
+        return jsonify({"loader_status": status.loader_status})
+
+@app.route("/api/home", methods=["POST"])
+def start_homing():
+    with status.status_lock:
+        status.loader_status = "homing"
+    # Start async homing in background (your loader code)
+    return jsonify({"success": True})
+
+@app.route("/api/start", methods=["POST"])
+def start_loader():
+    with status.status_lock:
+        status.start_flag = True
+    return jsonify({"success": True})
+
+def start_flask():
+  app.run(host="0.0.0.0", port=5000, debug=False, use_reloader=False)

main.py

@@ -9,9 +9,20 @@ from robot_control.src.robot.pump_controller import PumpController
 from robot_control.src.api.gpio import PiGPIO, MockGPIO
 from robot_control.src.utils.logging import setup_logging
 from robot_control.src.robot.mag_distributor import MagDistributor
-
+#==============================================================
+import threading
+import status
+from flask_app import start_flask
+#==============================================================
 class LoaderSystem:
     def __init__(self):
+        #==============================================================
+        # Your original initialization code...
+        self.logger = logging.getLogger(__name__)
+        # Example: set status at init
+        with status.status_lock:
+            status.loader_status = "initialized"
+        #==============================================================
         # Load configuration
         self.config = ConfigParser().config
         setup_logging(self.config)
@@ -63,6 +74,34 @@ class LoaderSystem:
         Main entry point for running the loader system.
         Starts all main async loops and ensures cleanup on exit.
         """
+        #==============================================================
+        # 1. Wait until homing is requested
+        while True:
+            with status.status_lock:
+                if status.homing_done:  
+                    break
+            await asyncio.sleep(0.5)
+
+        with status.status_lock:
+            status.loader_status = "homing"
+
+        # Perform homing before anything else
+        await self.mag_distributor.home()
+
+        with status.status_lock:
+            status.loader_status = "homed"
+        
+        # 2. Wait until start is requested
+        while True:
+            with status.status_lock:
+                if status.start_flag:  
+                    break
+            await asyncio.sleep(0.5)  
+
+        with status.status_lock:
+            # Update the shared variable
+            status.loader_status = "running"
+        #==============================================================
         await self.controller.connect()
         try:
             await asyncio.gather(
@@ -73,7 +112,10 @@ class LoaderSystem:
         finally:
             self.cleanup()
             self.logger.info("Cleaning up resources...")
-
+            #==============================================================
+            with status.status_lock:
+                status.loader_status = "completed"
+            #==============================================================
     async def _poll_i2c_channels(self):
         """
         Periodically poll I2C channels for sensor readings.
@@ -139,5 +181,11 @@ class LoaderSystem:
         self.gpio.cleanup()  # Ensure PumpController cleans up gpio
 
 if __name__ == "__main__":
+    #==============================================================
+    # Start Flask in background
+    flask_thread = threading.Thread(target=start_flask, daemon=True)
+    logging.info("Flask app started in background...")
+    #==============================================================
+
     loader_system = LoaderSystem()
     asyncio.run(loader_system.run())

playgrounds/datamatrix_playground.py

@@ -1,6 +1,11 @@
 from picamera2 import Picamera2
 import cv2
+import time
 from pylibdmtx.pylibdmtx import decode
+from robot_control.src.api.gpio import PiGPIO, MockGPIO
+
+# Initialize GPIO
+gpio = PiGPIO(out_pins=[13])  # or MockGPIO() for testing
 
 # Initialize camera
 picam2 = Picamera2()
@@ -10,34 +15,42 @@ picam2.start()
 # Your configured ROI
 x, y, w, h = 100, 0, 260, 450  # replace with your new values
 
+# Move servo to start position
+gpio.set_servo_angle_smooth(13, 15, 500)
+time.sleep(2)
+gpio.set_servo_angle_smooth(13, 88, 500)
 
 print("Press Enter to capture and decode a frame. Ctrl+C to exit.")
 
-while True:
-    input("Capture frame now...")
-    
-    # Capture frame
-    frame = picam2.capture_array()
-    
-    # Crop to ROI
-    roi = frame[y:y+h, x:x+w]
-    
-    # Convert to grayscale + enhance contrast
-    gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
-    gray = cv2.equalizeHist(gray)
-    
-    # Optional: save the cropped ROI to check visually
-    cv2.imwrite("roi_test.jpg", gray)
-    
-    # Decode DataMatrix
-    decoded = decode(gray)
-    
-    if decoded:
-        print("DataMatrix detected!")
-        for d in decoded:
-            print("Decoded content:", d.data.decode('utf-8'))
-    else:
-        print("No DataMatrix detected in this frame.")
-
-picam2.stop()
+try:
+    while True:
+        input("Capture frame now...")
+        
+        # Capture frame
+        frame = picam2.capture_array()
+        
+        # Crop to ROI
+        roi = frame[y:y+h, x:x+w]
+        
+        # Convert to grayscale + enhance contrast
+        gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
+        gray = cv2.equalizeHist(gray)
+        
+        # Optional: save the cropped ROI to check visually
+        cv2.imwrite("roi_test.jpg", gray)
+        
+        # Decode DataMatrix
+        decoded = decode(gray)
+        
+        if decoded:
+            print("DataMatrix detected!")
+            for d in decoded:
+                print("Decoded content:", d.data.decode('utf-8'))
+        else:
+            print("No DataMatrix detected in this frame.")
 
+except KeyboardInterrupt:
+    print("\nExiting...")
+finally:
+    picam2.stop()
+    gpio.cleanup()

playgrounds/integration_test.py

@@ -122,13 +122,15 @@ class LoaderSystem:
 
             await self.mag_distributor.home()
             self.mag_distributor.mag_to_feeder()
-            # await self.feeder_queue.put(1)
-            # await self.controller.prepare_feeder_cell()
-            # await self.controller.pick_cell_from_feeder()
+            await self.feeder_queue.put(1)
+            await self.controller.prepare_feeder_cell()
+            # await self.controller.pick_c
+            # ell_from_feeder()
             # cell = Cell(id=1, status=CellStatus.WAITING)
             # await self.controller.insert_cell_to_slot(cell, self.test_drop_slot)
             # await self.controller.pick_cell_from_slot(self.test_pickup_slot)
             # await self.controller.dropoff_cell()
+            # self.mag_distributor.defeeder_to_mag(self.config.defeeder_magazines[0])
 
             # await wait_for_enter()
             # # Feeding with MagDistributor

playgrounds/voltage_test.py

@@ -0,0 +1,204 @@
+import asyncio
+import logging
+import time
+from robot_control.src.utils.config import ConfigParser
+from robot_control.src.api.gpio import PiGPIO, MockGPIO
+from robot_control.src.api.i2c_handler import I2C, MockI2C
+from robot_control.src.vendor.mcp3428 import MCP3428
+
+# Configure logging
+logging.basicConfig(
+    level=logging.INFO,
+    format="%(asctime)s [%(levelname)s] %(name)s: %(message)s"
+)
+
+logger = logging.getLogger(__name__)
+
+# Voltage conversion factor (from controller.py)
+VOLTAGE_CONVERSION_FACTOR = 5.1455
+
+class VoltageTest:
+    def __init__(self):
+        self.config = ConfigParser().config
+        
+        # Initialize GPIO (include servo pin)
+        gpio_config = self.config.gpio
+        if gpio_config.debug:
+            self.gpio = MockGPIO()
+            logger.info("Using MockGPIO for testing")
+        else:
+            self.gpio = PiGPIO(out_pins=[gpio_config.measure_probe_oping, gpio_config.measure_servo_pin])
+            logger.info("Using real GPIO")
+        
+        # Initialize I2C
+        i2c_device_class = MCP3428 if not self.config.i2c.debug else MockI2C
+        self.i2c = I2C(i2c_device_class)
+        self.i2c.initialize()
+        logger.info(f"I2C initialized with {i2c_device_class.__name__}")
+        
+        self.io_conf = self.config.gpio
+
+    async def measure_cell_voltage(self) -> float:
+        """
+        Measure cell voltage by moving servo and deploying probe
+        """
+        if self.io_conf.measure_probe_oping < 0:
+            logger.warning("No probe pin configured, cannot measure voltage")
+            return 0.0
+        
+        try:
+            # Move servo to start position first
+            logger.info(f"Moving servo to start position ({self.io_conf.measure_servo_angle_start}°)...")
+            self.gpio.set_servo_angle_smooth(
+                self.io_conf.measure_servo_pin, 
+                self.io_conf.measure_servo_angle_start, 
+                1000
+            )
+            await asyncio.sleep(1.0)  # Wait for servo to reach position
+            
+            # Move servo to probe position
+            logger.info(f"Moving servo to probe position ({self.io_conf.measure_servo_angle_probe}°)...")
+            self.gpio.set_servo_angle_smooth(
+                self.io_conf.measure_servo_pin, 
+                self.io_conf.measure_servo_angle_probe, 
+                500
+            )
+            await asyncio.sleep(1.0)  # Wait for servo to reach position
+            
+            logger.info("Deploying measurement probe...")
+            self.gpio.set_pin(self.io_conf.measure_probe_oping, 1)
+            await asyncio.sleep(0.1)  # Wait for probe to deploy
+            
+            logger.info("Reading voltage from I2C channel 1...")
+            raw_value = await self.i2c.read_channel(1)
+            voltage = raw_value * VOLTAGE_CONVERSION_FACTOR
+
+            await asyncio.sleep(0.5)  
+            
+            logger.info("Retracting measurement probe...")
+            self.gpio.set_pin(self.io_conf.measure_probe_oping, 0)
+            
+            # Move servo back to start position
+            logger.info(f"Moving servo back to start position ({self.io_conf.measure_servo_angle_start}°)...")
+            self.gpio.set_servo_angle_smooth(
+                self.io_conf.measure_servo_pin, 
+                self.io_conf.measure_servo_angle_start, 
+                1000
+            )
+            await asyncio.sleep(1.0)  # Wait for servo to return
+            
+            logger.info(f"Raw I2C value: {raw_value}")
+            logger.info(f"Calculated voltage: {voltage:.3f}V")
+            
+            return voltage
+            
+        except Exception as e:
+            logger.error(f"Error measuring voltage: {str(e)}")
+            # Make sure probe is retracted and servo is in start position on error
+            try:
+                self.gpio.set_pin(self.io_conf.measure_probe_oping, 0)
+                self.gpio.set_servo_angle_smooth(
+                    self.io_conf.measure_servo_pin, 
+                    self.io_conf.measure_servo_angle_start, 
+                    1000
+                )
+            except:
+                pass
+            return 0.0
+
+    def check_cell_voltage(self, voltage: float) -> bool:
+        """
+        Check if the cell voltage is within acceptable range
+        """
+        min_voltage = abs(self.config.feeder.min_voltage)
+        
+        if voltage < min_voltage:
+            logger.warning(f"Cell voltage too low: {voltage:.3f}V < {min_voltage}V")
+            return False
+        if voltage < 0:
+            logger.warning(f"Cell has wrong polarity: {voltage:.3f}V")
+            return False
+        
+        logger.info(f"Cell voltage is good: {voltage:.3f}V >= {min_voltage}V")
+        return True
+
+    async def continuous_voltage_test(self, interval: float = 2.0):
+        """
+        Continuously measure voltage at specified intervals
+        """
+        logger.info(f"Starting continuous voltage measurement (interval: {interval}s)")
+        logger.info("Press Ctrl+C to stop...")
+        
+        try:
+            measurement_count = 0
+            while True:
+                measurement_count += 1
+                logger.info(f"\n--- Measurement #{measurement_count} ---")
+                
+                voltage = await self.measure_cell_voltage()
+                is_good = self.check_cell_voltage(voltage)
+                
+                status = "GOOD" if is_good else "BAD"
+                logger.info(f"Result: {voltage:.3f}V - Status: {status}")
+                
+                await asyncio.sleep(interval)
+                
+        except KeyboardInterrupt:
+            logger.info("\nStopping voltage test...")
+
+    async def single_voltage_test(self):
+        """
+        Perform a single voltage measurement
+        """
+        logger.info("Performing single voltage measurement...")
+        
+        voltage = await self.measure_cell_voltage()
+        is_good = self.check_cell_voltage(voltage)
+        
+        status = "GOOD" if is_good else "BAD"
+        logger.info(f"\nFinal Result: {voltage:.3f}V - Status: {status}")
+        
+        return voltage, is_good
+
+    def cleanup(self):
+        """
+        Cleanup resources
+        """
+        logger.info("Cleaning up...")
+        self.gpio.cleanup()
+
+async def main():
+    voltage_test = VoltageTest()
+    
+    try:
+        print("\nVoltage Test Options:")
+        print("1. Single measurement")
+        print("2. Continuous measurement (every 2 seconds)")
+        print("3. Custom interval continuous measurement")
+        
+        choice = input("\nEnter your choice (1-3): ").strip()
+        
+        if choice == "1":
+            await voltage_test.single_voltage_test()
+        elif choice == "2":
+            await voltage_test.continuous_voltage_test(2.0)
+        elif choice == "3":
+            try:
+                interval = float(input("Enter measurement interval in seconds: "))
+                await voltage_test.continuous_voltage_test(interval)
+            except ValueError:
+                logger.error("Invalid interval, using default 2 seconds")
+                await voltage_test.continuous_voltage_test(2.0)
+        else:
+            logger.error("Invalid choice, performing single measurement")
+            await voltage_test.single_voltage_test()
+            
+    except KeyboardInterrupt:
+        logger.info("Test interrupted by user")
+    except Exception as e:
+        logger.error(f"Test failed: {str(e)}")
+    finally:
+        voltage_test.cleanup()
+
+if __name__ == "__main__":
+    asyncio.run(main())

robot_control/config/config.yaml

@@ -29,17 +29,17 @@ measurement_devices:
 
 feeder:
   robot_pos: [23, 720, 47]
-  mag_pos: 
-    pos_mm: 46
+  mag_pos:
+    pos_mm: 45
     rot_deg: 155
   min_voltage: 2.0
   max_num_cells: 10
 
 defeeder:
   robot_pos: [12, 526, 72]
-  mag_pos: 
+  mag_pos:
     pos_mm: 8
-    rot_deg: 120
+    rot_deg: 115
   max_num_cells: 10
 
 feeder_magazines:
@@ -60,7 +60,7 @@ feeder_magazines:
 defeeder_magazines:
   - mag_pos:
       pos_mm: 297
-      rot_deg: 90
+      rot_deg: 5
     max_num_cells: 70
     health_range: [60, 100]
     name: accept
@@ -78,10 +78,10 @@ defeeder_magazines:
     name: error
 
 mag_distributor:
-    debug: False
-    max_speed_mmmin: 1000
-    home_speed_mmmin: 600
-    length_mm: 500
+  debug: False
+  max_speed_mmmin: 1000
+  home_speed_mmmin: 600
+  length_mm: 500
 
 mqtt:
   broker: localhost # or debug
@@ -101,7 +101,7 @@ vision:
   frame_rate: 30
   exposure: 0.1
   gain: 1.0
-  bbox: [170, 190, 240, 170]  # (x, y, width, height)
+  bbox: [170, 190, 240, 170] # (x, y, width, height)
 
 vacuum:
   min_pressure_bar: 0.13
@@ -115,10 +115,10 @@ gpio:
   pump_pin: 17
   valve_pin: 27
 
-  measure_probe_oping: 22  
+  measure_probe_oping: 22
   measure_servo_pin: 13
-  measure_servo_angle_start: 15 
-  measure_servo_angle_probe: 88
+  measure_servo_angle_start: 15
+  measure_servo_angle_probe: 90
   measure_servo_angle_end: 130
 
   mag_dist_pos_dir_pin: 11
@@ -132,10 +132,10 @@ gpio:
   mag_dist_rot_limit_pin: -1
 
   mag_dist_rot_servo_pin: 6
-  mag_dist_servo_angle_magazin: 5 
+  mag_dist_servo_angle_magazin: 5
   mag_dist_servo_angle_neutral: 60
   mag_dist_servo_angle_feeder: 155
-  mag_dist_servo_angle_defeeder: 60 
+  mag_dist_servo_angle_defeeder: 60
 
   mag_dist_sensor_pin: -1
 

robot_control/src/robot/mag_distributor.py

@@ -210,11 +210,15 @@ class MagDistributor:
         magazine.current_num_cells += 1  # Increment cell count in the magazine
 
         # Move to defeeder position
+        self.logger.warning("Function started")
         pos = self.config.defeeder.mag_pos
+        #defeeder,neutral
         self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
         time.sleep(0.5)
+        #defeeder, defeeder
         self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg, 1000)
         time.sleep(0.5)
+        #defeeder, neutral
         self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
 
 
@@ -224,11 +228,15 @@ class MagDistributor:
             return
 
         # Move to target magazine position
-        self.move_mag_distributor_at_pos(magazine.mag_pos.pos_mm, 0)
-        self.move_mag_distributor_at_pos(magazine.mag_pos.pos_mm, magazine.mag_pos.rot_deg)
+        pos = self.config.defeeder_magazines[0].mag_pos
+        #defeeder_magazines, neutral
+        self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
+        #defeeder_magazines, defeeder_magazines
+        self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg, 1000)
         time.sleep(1)
-        self.move_mag_distributor_at_pos(magazine.mag_pos.pos_mm, 0)
-
+        self.move_mag_distributor_at_pos(pos.pos_mm, self.mag_dist_servo_angle_neutral, 1000)
+        #defeeder_magazines, defeeder_magazines
+        
         # Optionally check for successful placement (if sensor logic applies)
         self.logger.info(f"Cell collected from defeeder and moved to magazine {magazine.name}.")
 

robot_control/src/vision/datamatrix.py

@@ -86,7 +86,7 @@ class DataMatrixReader:
 
         if self.bbox:
             #x, y, w, h = self.bbox
-            x, y, w, h = 80, 0, 220, 450 #Added values for bbox here as i couldnt update config
+            x, y, w, h = 100, 0, 260, 450 #Updated to match working playground values
             frame = frame[y:y+h, x:x+w]
             # Convert to grayscale, apply histogram equalization, blur, and adaptive thresholding
             frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

status.py

@@ -0,0 +1,6 @@
+import threading
+
+status_lock = threading.Lock()
+loader_status = "idle"  # Shared variable for status update
+start_flag = False      # Start flag
+homing_done = False     # Homing flag

templates/dashboard.html

@@ -0,0 +1,74 @@
+<!DOCTYPE html>
+<html lang="en">
+  <head>
+    <meta charset="UTF-8" />
+    <meta name="viewport" content="width=device-width, initial-scale=1.0" />
+    <title>Loader Dashboard</title>
+
+    <!-- Bootstrap CSS -->
+    <link
+      href="https://cdn.jsdelivr.net/npm/bootstrap@5.3.3/dist/css/bootstrap.min.css"
+      rel="stylesheet"
+    />
+
+    <!-- React + ReactDOM + Babel -->
+    <script
+      src="https://cdn.jsdelivr.net/npm/react@18/umd/react.development.js"
+      crossorigin
+    ></script>
+    <script
+      src="https://cdn.jsdelivr.net/npm/react-dom@18/umd/react-dom.development.js"
+      crossorigin
+    ></script>
+    <script src="https://cdn.jsdelivr.net/npm/@babel/standalone/babel.min.js"></script>
+  </head>
+  <body>
+    <div class="container mt-4">
+      <h1>Loader Dashboard</h1>
+      <div id="loader-controls"></div>
+      <!-- React component will render here -->
+    </div>
+
+    <script type="text/babel">
+      const { useState, useEffect } = React;
+
+      function LoaderControls() {
+        const [status, setStatus] = useState("idle");
+
+        useEffect(() => {
+          const interval = setInterval(async () => {
+            const res = await fetch("/api/status");
+            const data = await res.json();
+            setStatus(data.loader_status);
+          }, 1000);
+          return () => clearInterval(interval);
+        }, []);
+
+        const startHoming = async () => {
+          await fetch("/api/home", { method: "POST" });
+        };
+
+        const startLoader = async () => {
+          await fetch("/api/start", { method: "POST" });
+        };
+
+        return (
+          <div>
+            <p>Status: {status}</p>
+            <button onClick={startHoming} disabled={status === "homed"}>
+              Homing
+            </button>
+            <button onClick={startLoader} disabled={status !== "homed"}>
+              Start Automated Sequence
+            </button>
+          </div>
+        );
+      }
+
+      const root = ReactDOM.createRoot(
+        document.getElementById("loader-controls")
+      );
+      root.render(<LoaderControls />);
+    </script>
+  </body>
+</html>