Motores

Los motores usados tanto para las ruedas como para la cámara son de Parallax.

El esquema que siguen es el siguiente:

Software

Antes de ejecutar cualquier cosa relacionada con los motores es necesario ejecutar el siguiente comando:

sudo pigpiod

El repositorio de Julio que trata acerca de Pibot-Vision podemos ver funciones que permiten el funcionamiento de los motores en sus diferentes posibles opciones: hacia delante, hacia atrás, derecha e izquierda. Concretamente dichas funciones se encuentran dentro de follow person. Dentro de mi repositorio he incluido una versión modificada de dicho código:

El fichero es test_motors.py:

import argparse
import sys, traceback
import numpy as np
import threading
import sys, tty, termios, time, pigpio

servos = [4,18] # Usamos los servos conectados a los pines GPIO 4(der) y 18(izq)
dit = pigpio.pi()

def motor1_forward():
    dit.set_servo_pulsewidth(servos[0], 1600)

def motor1_reverse():
    dit.set_servo_pulsewidth(servos[0], 1300)

def motor1_stop():
    dit.set_servo_pulsewidth(servos[0], 1525)

def motor2_forward():
    dit.set_servo_pulsewidth(servos[1], 1400)

def motor2_reverse():
    dit.set_servo_pulsewidth(servos[1], 1700)

def motor2_stop():
    dit.set_servo_pulsewidth(servos[1], 1510)

def forward (tiempo):
    print ("Avanzando")
    motor1_forward()
    motor2_forward()
    time.sleep(tiempo)

def reverse (tiempo):
    print("Retrocediendo")
    motor1_reverse()
    motor2_reverse()
    time.sleep(tiempo)

def turn_left (tiempo):
    print("Avanzando izquierda")
    dit.set_servo_pulsewidth(servos[0], 1540) # derecha
    dit.set_servo_pulsewidth(servos[1], 1490) # izquierda
    time.sleep(tiempo)

def turn_right (tiempo):
    print("Avanzando derecha")
    dit.set_servo_pulsewidth(servos[0], 1550) # derecha
    dit.set_servo_pulsewidth(servos[1], 1500) # izquierda
    time.sleep(tiempo)

def stop (tiempo):
    print("DETENIENDO")
    motor1_stop()
    motor2_stop()
    time.sleep(tiempo)
    for s in servos: # stop servo pulses
        dit.set_servo_pulsewidth(s, 0)
        dit.stop()

if __name__ == "__main__":
    
    forward(10)

En el momento de ejecutar el código, se queda bloqueado en un bucle infinito cuando en realidad debería ir hacia delante solo 10 segundos.

Seguí investigando y es importante tener en cuenta cómo funciona los servomotores, qué es la frecuencia y qué es PWM. Tras una profunda investigación, encontré el siguiente enlace que lo explica detalladamente.

Adapté el código de ejemplo para poder cumplir todas mis necesidades:

El fichero es test_motors3.py:

#!/usr/bin/env python3
#-- coding: utf-8 --
import RPi.GPIO as GPIO
import time

#Set function to calculate percent from angle
def angle_to_percent (angle) :
    if angle > 180 or angle < 0 :
        return False

    start = 4
    end = 12.5
    ratio = (end - start)/180 #Calcul ratio from angle to percent

    angle_as_percent = angle * ratio

    return start + angle_as_percent

GPIO.setmode(GPIO.BOARD) #Use Board numerotation mode
GPIO.setwarnings(False) #Disable warnings

#Use pin 4 for PWM signal
pwm_gpio_left = 7
#Use pin 18 for PWM signal
pwm_gpio_right = 12
frequence = 50

GPIO.setup(pwm_gpio_left, GPIO.OUT)
pwm_left = GPIO.PWM(pwm_gpio_left, frequence)
GPIO.setup(pwm_gpio_right, GPIO.OUT)
pwm_right = GPIO.PWM(pwm_gpio_right, frequence)

#Go ahead
pwm_left.start(angle_to_percent(180))
pwm_right.start(angle_to_percent(0))
time.sleep(1)

#Go back
pwm_left.start(angle_to_percent(0))
pwm_right.start(angle_to_percent(180))
time.sleep(1)

#Go ahead to the left
pwm_left.start(angle_to_percent(180))
pwm_right.start(angle_to_percent(45))
time.sleep(1)

#Go ahead to the right
pwm_left.start(angle_to_percent(135))
pwm_right.start(angle_to_percent(0))
time.sleep(1)

#Close GPIO & cleanup
pwm_left.stop()
pwm_right.stop()
GPIO.cleanup()

A continuación podrás ver un video que demuestra su funcionamiento

La función angle_to_percentage() no realiza un movimento notable cuando quiere hacer giros por lo que, tuve que cambiar la lógica. Usando conceptos de PWM y dutycycle (fuente 1 y fuente 2) pude entender que estaba pasando y solucionarlo. Aquí se puede ver 1 gráfica y un diagrama que demuestran la relación asignada para cada caso. Todo el código se encuentra en py.

Si nos damos cuenta existe una relación lineal entre PWM y duty cycle:

12% -> 180º

2% -> 0º

Finalmente usando la ecuación de la recta podemos calcular la relación de grados -> % que estamos buscando:

El código de ahead.py (el resto de códigos son iguales) es:

# -*- coding: utf-8 -*-
import RPi.GPIO as GPIO
import time

## ESQUEMA PARA LA RELACIÓN ÁNGULO/MOVIMIENTO DE LOS SERVOS:
### ADELANTE ->      M_izq = 180º, M_Der = 0º
### GIRO DERECHA->   M_izq = 180º, M_Der = 90º
### GIRO IZQUIERDA-> M_izq = 90º, M_Der = 180º
### ATRÁS->          M_izq = 0º, M_Der = 180º

## 12% -> 180º
##  2% ->   0º 

def angle2dutycycle(angle):
    x1 = 180
    y1 = 12
    x2 = 0
    y2 = 2
    c = y2
    
    dutycycle = ((y1 - y2) / (x1 - x2)) * angle + c
    return dutycycle

GPIO.setmode(GPIO.BOARD)

#Usa pin 4 
pwm_gpio_left = 7
#Usa pin 18 
pwm_gpio_right = 12
frequence = 50

GPIO.setup(pwm_gpio_left, GPIO.OUT)
pwm_left = GPIO.PWM(pwm_gpio_left, frequence)
GPIO.setup(pwm_gpio_right, GPIO.OUT)
pwm_right = GPIO.PWM(pwm_gpio_right, frequence)

#Va hacia adelante (180º, 0º)
pwm_left.start(angle2dutycycle(180))
pwm_right.start(angle2dutycycle(0))
time.sleep(0.15)
pwm_left.stop()
pwm_right.stop()
GPIO.cleanup()