Caso práctico: logger de velocidad UGV con Raspberry Pi

Implementación

La solución se divide en dos archivos. El primero es el script de registro principal que interactúa con el hardware (o lo simula). El segundo es un script de análisis para procesar los datos del CSV.

1. Script de registro principal

Guarda el siguiente código como ugv_speed_logger.py. Este script utiliza clases adaptadoras para permitir la ejecución en un PC estándar sin hardware pasando la bandera --dry-run.

#!/usr/bin/env python3
"""
UGV Wheel Encoder Speed Logger
Drives a dual-motor chassis through a PWM sweep and logs encoder RPM to a CSV.
Supports --dry-run for offline validation.
"""

import argparse
import csv
import time
import sys

# ---------------------------------------------------------
# Mock Adapters for Offline Validation
# ---------------------------------------------------------

class MockSMBus:
    def __init__(self, bus_number):
        self.bus_number = bus_number
        self.registers = {}
        print(f"[MOCK] Initialized SMBus {bus_number}")

    def write_byte_data(self, address, register, value):
        self.registers[(address, register)] = value

class MockGPIO:
    BCM = "BCM"
    IN = "IN"
    OUT = "OUT"
    RISING = "RISING"
    HIGH = 1
    LOW = 0

    def __init__(self):
        self.callbacks = {}
        self.pins = {}
        print("[MOCK] Initialized GPIO")

    def setmode(self, mode):
        pass

    def setup(self, pin, mode):
        self.pins[pin] = mode

    def output(self, pin, state):
        pass

    def add_event_detect(self, pin, edge, callback):
        self.callbacks[pin] = callback

    def cleanup(self):
        print("[MOCK] GPIO cleaned up")

    def simulate_tick(self, pin):
        if pin in self.callbacks:
            self.callbacks[pin](pin)

# ---------------------------------------------------------
# Device Drivers
# ---------------------------------------------------------

class PCA9685:
    def __init__(self, bus, address=0x40):
        self.bus = bus
        self.address = address
        # Basic PCA9685 initialization (50Hz)
        self.bus.write_byte_data(self.address, 0x00, 0x10) # Sleep
        self.bus.write_byte_data(self.address, 0xFE, 121)  # Prescale for ~50Hz
        self.bus.write_byte_data(self.address, 0x00, 0x00) # Wake
        time.sleep(0.005)
        self.bus.write_byte_data(self.address, 0x00, 0xA1) # Auto-increment

    def set_pwm(self, channel, duty_percent):
        duty_percent = max(0, min(100, duty_percent))
        off_val = int((duty_percent / 100.0) * 4095)
        reg = 0x06 + (channel * 4)
        self.bus.write_byte_data(self.address, reg, 0)
        self.bus.write_byte_data(self.address, reg+1, 0)
        self.bus.write_byte_data(self.address, reg+2, off_val & 0xFF)
        self.bus.write_byte_data(self.address, reg+3, off_val >> 8)

class ChassisController:
    def __init__(self, gpio, i2c_bus, is_dry_run=False):
        self.gpio = gpio
        self.pca = PCA9685(i2c_bus)
        self.is_dry_run = is_dry_run

        # TB6612FNG Pins
        self.AIN1 = 5
        self.AIN2 = 6
        self.BIN1 = 13
        self.BIN2 = 19
        self.STBY = 26

        self.gpio.setmode(self.gpio.BCM)
        for pin in [self.AIN1, self.AIN2, self.BIN1, self.BIN2, self.STBY]:
            self.gpio.setup(pin, self.gpio.OUT)
            self.gpio.output(pin, self.gpio.LOW)

        # Enable motor driver and set forward direction
        self.gpio.output(self.STBY, self.gpio.HIGH)
        self.gpio.output(self.AIN1, self.gpio.HIGH)
        self.gpio.output(self.AIN2, self.gpio.LOW)
        self.gpio.output(self.BIN1, self.gpio.HIGH)
        self.gpio.output(self.BIN2, self.gpio.LOW)

    def set_speed(self, duty_percent):
        self.pca.set_pwm(0, duty_percent) # Left
        self.pca.set_pwm(1, duty_percent) # Right

    def stop(self):
        self.set_speed(0)
        self.gpio.output(self.STBY, self.gpio.LOW)

class EncoderLogger:
    def __init__(self, gpio, left_pin=17, right_pin=27, ticks_per_rev=20):
        self.gpio = gpio
        self.left_pin = left_pin
        self.right_pin = right_pin
        self.ticks_per_rev = ticks_per_rev

        self.left_ticks = 0
        self.right_ticks = 0

        self.gpio.setup(self.left_pin, self.gpio.IN)
        self.gpio.setup(self.right_pin, self.gpio.IN)

        self.gpio.add_event_detect(self.left_pin, self.gpio.RISING, callback=self._left_tick)
        self.gpio.add_event_detect(self.right_pin, self.gpio.RISING, callback=self._right_tick)

    def _left_tick(self, channel):
        self.left_ticks += 1

    def _right_tick(self, channel):
        self.right_ticks += 1

    def get_and_clear_ticks(self):
        lt = self.left_ticks
        rt = self.right_ticks
        self.left_ticks = 0
        self.right_ticks = 0
        return lt, rt

# ---------------------------------------------------------
# Main Execution
# ---------------------------------------------------------

def main():
    parser = argparse.ArgumentParser(description="UGV Speed Logger")
    parser.add_argument("--dry-run", action="store_true", help="Run without hardware")
    parser.add_argument("--output", type=str, default="speed_log.csv", help="CSV output file")
    parser.add_argument("--duration", type=int, default=10, help="Test duration in seconds")
    args = parser.parse_args()

    if args.dry_run:
        gpio = MockGPIO()
        i2c_bus = MockSMBus(1)
    else:
        try:
            import RPi.GPIO as rpi_gpio
            from smbus2 import SMBus
            gpio = rpi_gpio
            i2c_bus = SMBus(1)
        except ImportError:
            print("Error: Hardware libraries not found. Run with --dry-run or install them.")
            sys.exit(1)

    chassis = ChassisController(gpio, i2c_bus, args.dry_run)
    encoders = EncoderLogger(gpio)

    print(f"Starting sweep test. Logging to {args.output}")
    start_time = time.time()
    last_time = start_time

    try:
        with open(args.output, mode='w', newline='') as csv_file:
            writer = csv.writer(csv_file)
            writer.writerow(["Time_s", "PWM_Percent", "Left_Ticks", "Right_Ticks", "Left_RPM", "Right_RPM"])

            while True:
                current_time = time.time()
                elapsed = current_time - start_time

                if elapsed > args.duration:
                    break

                # Ramp PWM from 0 to 100
                pwm_target = (elapsed / args.duration) * 100.0
                chassis.set_speed(pwm_target)

                time.sleep(0.2)
                now = time.time()
                dt = now - last_time
                last_time = now

                # Simulate ticks for dry-run
                if args.dry_run:
                    if pwm_target > 20.0: # Mock deadband at 20%
                        sim_rpm = (pwm_target - 20.0) * 2.5
                        sim_ticks_per_sec = (sim_rpm * encoders.ticks_per_rev) / 60.0
                        ticks_to_add = int(sim_ticks_per_sec * dt)
                        for _ in range(ticks_to_add):
                            gpio.simulate_tick(encoders.left_pin)
                            gpio.simulate_tick(encoders.right_pin)

                left_t, right_t = encoders.get_and_clear_ticks()

                dt_min = dt / 60.0
                left_rpm = (left_t / encoders.ticks_per_rev) / dt_min if dt_min > 0 else 0
                right_rpm = (right_t / encoders.ticks_per_rev) / dt_min if dt_min > 0 else 0

                writer.writerow([round(elapsed, 2), round(pwm_target, 2), left_t, right_t, round(left_rpm, 2), round(right_rpm, 2)])
                print(f"Time: {elapsed:05.2f}s | PWM: {pwm_target:05.1f}% | L_RPM: {left_rpm:06.1f} | R_RPM: {right_rpm:06.1f}")

    except KeyboardInterrupt:
        print("\nTest interrupted by user.")
    finally:
        chassis.stop()
        gpio.cleanup()
        print("Test complete. Motors stopped.")

if __name__ == "__main__":
    main()

2. Script de análisis

Guarda el siguiente código como analyze_speed_log.py. Este script lee el CSV generado para extraer las métricas de rendimiento, calculando explícitamente la banda muerta y las RPM máximas.

#!/usr/bin/env python3
"""
Analyzes the UGV Speed Log CSV to determine motor deadband and max RPM.
"""

import csv
import argparse

def main():
    parser = argparse.ArgumentParser(description="Analyze UGV Speed Log")
    parser.add_argument("--input", type=str, default="speed_log.csv", help="Input CSV file")
    args = parser.parse_args()

    max_rpm = 0.0
    deadband_pwm = None

    try:
        with open(args.input, mode='r') as f:
            reader = csv.DictReader(f)
            for row in reader:
                pwm = float(row["PWM_Percent"])
                l_rpm = float(row["Left_RPM"])
                r_rpm = float(row["Right_RPM"])

                avg_rpm = (l_rpm + r_rpm) / 2.0

                if avg_rpm > max_rpm:
                    max_rpm = avg_rpm

                # The first time we observe sustained movement, record the PWM
                if deadband_pwm is None and avg_rpm > 5.0:
                    deadband_pwm = pwm

        print(f"--- Analysis Report for {args.input} ---")
        if deadband_pwm is not None:
            print(f"Estimated Motor Deadband Threshold: ~{deadband_pwm:.2f}% PWM")
        else:
            print("Estimated Motor Deadband Threshold: Not found (no movement logged).")
        print(f"Maximum Observed Speed: {max_rpm:.2f} RPM")

    except FileNotFoundError:
        print(f"Error: Could not find {args.input}. Run the logger script first.")

if __name__ == "__main__":
    main()