Programa de demostración de Ophyra Racing en modo Seguidor de Línea.
Por defecto el programa es configurado para seguir una línea color negro en fondo blanco, sin embargo, puede ser cambiado para que detecte una pista con los colores invertidos.
El programa permite la comunicación inalámbrica y el control de velocidad mediante el dispositivo de Bluetooth incorporado en Ophyra-Ra, a través de una aplicación para dispositivos móviles Androide (la aplicación será liberada en los próximos días para su descarga).
Cuenta con un control de tipo PD (Proporcional-Derivativo) para el seguimiento de la línea controlando de manera diferencial los motores. El control No hace uso de los Encoders de los motores, por lo que no es necesario contar con motores de eje extendido.
El programa realiza todo el tiempo una “calibración de sensores” mediante el muestreo a 43KHz de cada sensor, esto es para evitar la interferencia de cambios en la iluminación sobre la pista en tiempo real. Debido a esta característica no es necesario pre-calibrar los sensores antes de correr sobre la pista.
El programa también cuenta con una rutina que hace recuperar la trayectoria de Ophyra Racing en caso de que se salga de la línea.
#Demo de Velocidad V2.3.
#LENGUAJE: Python
#PLATAFORMA: Ophyra-Racing.
#AUTOR: Fernando Quinones Novelo
#DESCRIPCION: Programa de demostracion de seguidor de linea
#con control a distancia por medio conexion Bluetooh con una app Androide.
#El programa tiene control de fondo de la Pista (Blanco-Negro) por
#defecto esta configurado para Pista Negra-Fondo Blanco
#El programa incluye un control PD sintanizado para correr hasta máximo
#40% del ciclo de trabajo de los motores(aproximadamente 1.5m/s).
from pyb import Pin
from pyb import delay
from pyb import ADC
from pyb import Timer
from pyb import UART
import gc
import time
import math
import array
from ophyra_mpu60 import MPU6050
#----------SECCION DE CREACION DE VARIABLES Y OBJETOS DE CONTROL---------
#Configuracion de los LED de Ophyra
LI1 = Pin(Pin.cpu.E12, Pin.OUT_PP)
LI2 = Pin(Pin.cpu.E13, Pin.OUT_PP)
LI3 = Pin(Pin.cpu.E14, Pin.OUT_PP)
LI4 = Pin(Pin.cpu.E15, Pin.OUT_PP)
LI5 = Pin(Pin.cpu.D0, Pin.OUT_PP)
LI6 = Pin(Pin.cpu.D1, Pin.OUT_PP)
LI7 = Pin(Pin.cpu.D2, Pin.OUT_PP)
LI8 = Pin(Pin.cpu.D3, Pin.OUT_PP)
#Configuracion de los switch de proposito general de OphyraRacing
SW1 = Pin('PD5', Pin.IN, Pin.PULL_UP)
SW2 = Pin('PD4', Pin.IN, Pin.PULL_UP)
#Configuracion de los SensoresOpticos de OphyraRacing
S1 = ADC('PB1')
S2 = ADC('PB0')
S3 = ADC('PC5')
S4 = ADC('PC4')
S5 = ADC('PA7')
S6 = ADC('PA6')
S7 = ADC('PA5')
S8 = ADC('PA4')
VBAT=ADC('PC0')
#Timer que controla la frecuencia de muestreo de los sensores
tiempo=Timer(8,freq=43000)
#Senales de control del Puente H
PWMT = Timer(4, freq=15000) #Creando el timer general de control de PWM
STBY = Pin(Pin.cpu.B12, Pin.OUT_PP)#Pines de control del Motor A
AIN1 = Pin(Pin.cpu.B15, Pin.OUT_PP)
AIN2 = Pin(Pin.cpu.D8, Pin.OUT_PP)
PWMA = PWMT.channel(2, Timer.PWM, pin=Pin.cpu.D13, pulse_width_percent=0) #TMR4_CH2
BIN1 = Pin(Pin.cpu.B13, Pin.OUT_PP) #Pines de control del Motor B
BIN2 = Pin(Pin.cpu.B14, Pin.OUT_PP)
PWMB = PWMT.channel(1, Timer.PWM, pin=Pin.cpu.D12, pulse_width_percent=0) #TMR4_CH1
#Activando pines de control para que los motores giren hacia adelante.
AIN1.on()
AIN2.off()
BIN1.on()
BIN2.off()
#Desactivando la activacion general del puente H.
STBY.off()
#Creando el objeto serial para comunicacion con el BlueTooth
BTH = UART(3, 115200)
BTH.init(115200, bits=8, parity=None, stop=1, timeout=0)
bandera=array.array('b',[0])#utilizadad para activar/desactivar el avance del carrito
#Constantes del control PD
kp=1.13#Constante Proporcional
kd=26.6#Constante Derivativo
ke=20 #Constante de crecimiento exponencial
#variables de velocidad y correccion
vel=20#Variable principal de control de velocidad (valores de 0 a 40)
#Expresado en porcentaje de ciclo de trabajo
vrng=0#rango de velocidad maxima
vfin=array.array('f',[0])#velocidad ajustada entre la base y el rango
corr=array.array('f',[0])#correcion de velocidad
errD=array.array('f',[0])#error de grupo de sensores Derecho
errI=array.array('f',[0])#error de grupo de sensores Izquierdo
err=array.array('f',[0])#error global de posicion
errAnt=array.array('f',[0])#error anterior (para computar la diferencial)
d=array.array('f',[0])#error diferencial (err-errAnt)
#variables de deteccion de la posicion de los sensores
suma=array.array('f',[0])#suma total de los valores de los sensores
humb=0#humbral computado de los sensores para detectar el contraste
mx=0#Lectura maxima de los sensores(detecta el color negro)
mn=0#Lectura minima de los sensores(detecta el color blanco)
rng=array.array('i',[0])
#iniciando las velocidades de los motores
motA=array.array('f',[vel])
motB=array.array('f',[vel])
#configurando el color del fondo de la pista.
fondo=1 #1=fondo blanco, 0=fondo negro
#Variables globales que guardan 8 muestras de cada sensor
DS=array.array('I',[0,0,0,0,0,0,0,0])
B1=array.array('I',[0,0,0,0,0,0,0,0])
B2=array.array('I',[0,0,0,0,0,0,0,0])
B3=array.array('I',[0,0,0,0,0,0,0,0])
B4=array.array('I',[0,0,0,0,0,0,0,0])
B5=array.array('I',[0,0,0,0,0,0,0,0])
B6=array.array('I',[0,0,0,0,0,0,0,0])
B7=array.array('I',[0,0,0,0,0,0,0,0])
B8=array.array('I',[0,0,0,0,0,0,0,0])
#variable que guarda la posicion de la linea con respecto de los sensores
senbin=array.array('B',[0,0,0,0,0,0,0,0])
#-------DECLARACION DE FUNCIONES ESPECIALES ----------------------------------------
#Funcion que muestrea los 8 sensores
#cada sensor es muestreado 8 veces y sus muestras son gardadas en los vectore B#
@micropython.native
def Leer_Sensores():
#Funcion de muestreo de cada sensor a 43KHz
ADC.read_timed_multi((S1,S2,S3,S4,S5,S6,S7,S8),(B1,B2,B3,B4,B5,B6,B7,B8),tiempo)
#sumatoria de los valores muestreados de cada sensor
DS[0]=sum(B1)
DS[1]=sum(B2)
DS[2]=sum(B3)
DS[3]=sum(B4)
DS[4]=sum(B5)
DS[5]=sum(B6)
DS[6]=sum(B7)
DS[7]=sum(B8)
#calculo del valor promedio de las meustras en cada sensor
DS[0]=DS[0]>>3 #el promedio es realizado mediante un corrimiento de 3 posiciones
DS[1]=DS[1]>>3 #a la derecha
DS[2]=DS[2]>>3
DS[3]=DS[3]>>3
DS[4]=DS[4]>>3
DS[5]=DS[5]>>3
DS[6]=DS[6]>>3
DS[7]=DS[7]>>3
#Funcion que muestra en los LEDs indicadores la posicion de la linea con respecto
#de los sensores
@micropython.viper
def Mostrar_Sensores():
LI1.value(senbin[0])
LI2.value(senbin[1])
LI3.value(senbin[2])
LI4.value(senbin[3])
LI5.value(senbin[4])
LI6.value(senbin[5])
LI7.value(senbin[6])
LI8.value(senbin[7])
#Funcion que apaga todos los LEDs indicadores
@micropython.viper
def Apaga_LEDs():
LI1.value(0)
LI2.value(0)
LI3.value(0)
LI4.value(0)
LI5.value(0)
LI6.value(0)
LI7.value(0)
LI8.value(0)
#Encuentra el humbral apropiado de contraste en cada muestreo de los sensores
@micropython.native
def Calibrar_Sensores():
#encontrando el color negro
mx=max(DS)
#encontrando el blanco
mn=min(DS)
#calculando el rango entre los dos valores
rng[0]=mx-mn
#calculando la media entre los valores
humb=((rng[0])/2)+mn
#Detectando la linea segun el ultimo humbral
for i in range(0,8):
if fondo==0:#fondo Negro
if DS[i]<humb:
senbin[i]=1#linea
else:
senbin[i]=0#fondo
else:#fondo Blanco
if DS[i]<humb:
senbin[i]=0#linea
else:
senbin[i]=1#fondo
#calculo de la distribucion de la liena en los sensores
#los sensores estan divididos en dos regiones
#(4 sensores Derecho y 4 sensores Izquierdo)
suma[0]=DS[0]*senbin[0]+DS[1]*senbin[1]+DS[2]*senbin[2]+DS[3]*senbin[3]
+DS[4]*senbin[4]+DS[5]*senbin[5]+DS[6]*senbin[6]+DS[7]*senbin[7]
errI[0]= (25*DS[0]*senbin[0]+12.25*DS[1]*senbin[1]+6.25*DS[2]*senbin[2]+1.5*DS[3]*senbin[3])/suma[0]
errD[0]= (25*DS[7]*senbin[7]+12.25*DS[6]*senbin[6]+6.25*DS[5]*senbin[5]+1.5*DS[4]*senbin[4])/suma[0]
#calculo del error ente ambas regiones.
err[0]=errD[0]-errI[0]
#Funcion que cambia el sentido de giro de los motores
@micropython.native
def Signo_Motores():
if motA[0]<=0:
AIN1.off()
AIN2.on()
else:
AIN1.on()
AIN2.off()
if motB[0]<=0:
BIN1.off()
BIN2.on()
else:
BIN1.on()
BIN2.off()
motA[0]=abs(motA[0])
motB[0]=abs(motB[0])
if motA[0]>101:
motA[0]=100
if motB[0]>101:
motB[0]=100
PWMA.pulse_width_percent(motA[0])
PWMB.pulse_width_percent(motB[0])
#Funcion que ejecuta una correccion en caso de que se salda de la linea
@micropython.native
def Ultima_posicion():
motA[0]=abs(motA[0])
motB[0]=abs(motB[0])
if errAnt[0]<0: #si se salio de la linea, entra el corrector
AIN1.off() #del ultimo giro para retomar la pista
AIN2.on()
BIN1.on()
BIN2.off()
else:
AIN1.on()
AIN2.off()
BIN1.off()
BIN2.on()
PWMA.pulse_width_percent(motA[0])
PWMB.pulse_width_percent(motB[0])
#--------------------------CICLO PRINCIPAL DE CONTROL---------------------------------
try:
while True:
Leer_Sensores()
Calibrar_Sensores()
#se verifica si el rango entre los sensores es menor que 300,
#si se cumple, significa que se salió de la pista.
#si es mayor a 300 ejecuta el control del PD.
if rng[0]>300:
d[0]=err[0]-errAnt[0]#calculando la diferencial entre el
#error actual menos el error anterior
errAnt[0]=err[0]#guardando el error actual
corr[0]=kp*err[0]+kd*d[0]#Control PD
#funcion exponecial que sirve para frenar en curvas y
#acelerar en rectas.
vfin[0]=vel+vrng*math.exp(-ke*abs(kp*err[0]))
motA[0]=vfin[0]+corr[0]#Actualizando la velociada de los motores
motB[0]=vfin[0]-corr[0]
Signo_Motores()
Mostrar_Sensores()
else:
Apaga_LEDs()
Ultima_posicion()
entrada=BTH.read()#Leyendo datos provenientes del Bluetooth
if entrada != None and entrada!=b'A' and entrada!=b'B':
vel=int.from_bytes(entrada,'big')#actualiza la velocidad
if entrada == b'A':#activa el avance del carrito
STBY.on()
bandera[0]=1
if entrada == b'B':#detiene el avance del carrito
STBY.off()
bandera[0]=0
#detecta el switch1 (Boton 1 de proposito general)
#presionando el boton se activa/desactiva el avance del carrito
if SW1.value()==0:
delay(300)
if bandera[0]==1:
STBY.off()
bandera[0]=0
elif bandera[0]==0:
STBY.on()
d[0]=0
errAnt[0]=0
bandera[0]=1
#muy importante no borrar
gc.collect()#realiza un recoleccion de basura en la memoria RAM
#------------------------------------------------------------------------------------
finally:
STBY.off()#seccion que se ejecuta en caso de que el programa se detenga.
LI1.off()
LI2.off()
LI3.off()
LI4.off()
LI5.off()
LI6.off()
LI7.off()
LI8.off()