Recursos y Tutoriales de Ophyra-Racing

Se muestra como se puede acceder a los sensores infrarrojos de Ophy-Ra. Los sensores están conectados a las entradas analógicas del ADC del Microcontrolador. El Microcontrolador cuenta con un ADC de 12bits de resolución; el voltaje de referencia para el ADC es de 3.3V interno.

Los sensores son de tipo analógico y sus valores se muestran en la terminal PuTTY como números enteros, que pueden variar desde 0 hasta 4096.

#Ejemplo 1.
#LENGUAJE: Python
#PLATAFORMA: Ophyra-Racing.
#DESCRIPCION: Ejemplo de configuracion del ADC para la lectura de los 8 sensores infrerojos.

#Seccion de importacion de librerias 
from pyb import ADC
#Creando Objetos ADC de cada sensor Infrarojo de Ophy-Ra.
S1 = ADC('PB1')
S2 = ADC('PB0')
S3 = ADC('PC5')
S4 = ADC('PC4')
S5 = ADC('PA7')
S6 = ADC('PA6')
S7 = ADC('PA5')
S8 = ADC('PA4')
#Bucle principal
while True:      
        sen1=ADC.read(S1) #Lectura del sensor 1
        sen2=ADC.read(S2)
        sen3=ADC.read(S3)
        sen4=ADC.read(S4)
        sen5=ADC.read(S5)
        sen6=ADC.read(S6)
        sen7=ADC.read(S7)
        sen8=ADC.read(S8) #Lectura del sensor 8
        #impresion en la terminal de los valores de cada sensor.
        print('S1={:d} S2={:d} S3={:d} S4={:d} S5={:d} S6={:d} S7={:d} S8={:d}'.format(sen1,sen2,sen3,sen4,sen5,sen6,sen7,sen8))

Se  muestra el funcionamiento de los motores de Ophy-Ra. Los motores están conectados directamente a la salida del puente H controlador TB6612FNG. En el programa se observa la configuración y la relación de pines que controlan los motores.

   
#Ejemplo 2.
#LENGUAJE: Python
#PLATAFORMA: Ophyra-Racing.
#DESCRIPCION:
#Ejemplo de configuracion y activacion de los Motores en Ophyra-Racing.
#El puente H en Ophyra-Racing es el integrado TB6612FNG.
#El ejemplo activa los motores en el sentido hacia adelante por 2 segundos
#y posteriormente cambia el sentido. El ciclo de trabajo de la señal PWM es fijada al 20%.
#La frecuencia del PWM es fijada en 15KHz para evitar la resonancia en los motores. 
#Motor A es el motor hubicado a la derecha.

#Seccion de importacion de librerias.
from pyb import Pin
from pyb import Timer
from pyb import delay

#Senales de control del Puente H
PWMT = Timer(4, freq=15000)#Creando el timer que controla la señal de PWM.

#Pin B12 del microcontrolador, que controla el Standby del puente H TB6612FNG
STBY = Pin(Pin.cpu.B12, Pin.OUT_PP)

#Pines B15 y D8 del microcontrolador, que controla el sentido de giro del motor A
AIN1 = Pin(Pin.cpu.B15, Pin.OUT_PP)
AIN2 = Pin(Pin.cpu.D8, Pin.OUT_PP)

#TMR4_CH2 (canal 2 del timer 4) pin D13 que controla el PWM del motor A
PWMA = PWMT.channel(2, Timer.PWM, pin=Pin.cpu.D13, pulse_width_percent=0)

#Pines B13 y B14 del microcontrolador, que controla el sentido de giro del motor B
BIN1 = Pin(Pin.cpu.B13, Pin.OUT_PP)
BIN2 = Pin(Pin.cpu.B14, Pin.OUT_PP)

#TMR4_CH1 (canal 1 del timer 4) pin D12 que controla el PWM del motor B
PWMB = PWMT.channel(1, Timer.PWM, pin=Pin.cpu.D12, pulse_width_percent=0)

#Ajustando el ciclo de trabajo del 20% en ambos motores
PWMA.pulse_width_percent(20)
PWMB.pulse_width_percent(20)
try:
        while True:
                AIN1.on()      #Ajustando sentido de ambos motores hacia adelante
                AIN2.off()
                BIN1.on()
                BIN2.off()
                STBY.on()      #Activando ambos motores.
                print('Motores hacia adelante')
                delay(2000)    #tiempo de espera 2seg
                STBY.off()     #Desactivando ambos motores.
                print('Apagando motores')
                delay(500)     #tiempo de espera 0.5seg
                AIN1.off()     #Ajustando sentido de ambos motores hacia adelante
                AIN2.on()
                BIN1.off()
                BIN2.on()
                STBY.on()      #Activando ambos motores.
                print('Motores hacia atras')
                delay(2000)    #tiempo de espera 2seg
                STBY.off()     #Desactivando ambos motores.
                print('Apagando motores')
                delay(500)     #tiempo de espera 0.5seg
finally:
        #Seccion que se ejecuta cuando en la terminal se presiona "ctrl+C"
        PWMA.pulse_width_percent(0)  #Apagando la señal PWM en ambos motores
        PWMB.pulse_width_percent(0)
        STBY.off()                   #Desactivando ambos motores.

Se muestra como controlar los LEDs indicadores de Ophy-Ra, mediante un programa de demostración que muestra el barrido de una luz que avanza a través ellos.

Los LEDs son de propósito general y pueden utilizarse para los fines que el programador convenga. Por ejemplo pueden utilizarse para indicar la posición de la línea respecto de los sensores cuando Ophy-Ra esté sobre la pista.

#Ejemplo 3.
#LENGUAJE: Python
#PLATAFORMA: Ophyra-Racing.
#DESCRIPCIÓN:
#Ejemplo de configuracion y activacion de los LEDs indicadores de Ophyra-Racing.
#Los LEDs son de propósito general y se pueden utilizar de acuerdo a las necesidades 
#del usuario. El ejemplo activa los LEDs en una secuencia. Muestra también que pines 
#del microcontrolador controlan los leds.

#Sección de importación de librerías.
from pyb import Pin
from pyb import delay

#Configuración de los LED de Ophyra
#Configurando como salida (modo push-pull) E12=Led Indicador 1
LI1 = Pin(Pin.cpu.E12, Pin.OUT_PP)
#Configurando como salida (modo push-pull) E13=Led Indicador 2
LI2 = Pin(Pin.cpu.E13, Pin.OUT_PP)
#Configurando como salida (modo push-pull) E14=Led Indicador 3
LI3 = Pin(Pin.cpu.E14, Pin.OUT_PP)
#Configurando como salida (modo push-pull) E15=Led Indicador 4
LI4 = Pin(Pin.cpu.E15, Pin.OUT_PP)
#Configurando como salida (modo push-pull) D0=Led Indicador 5
LI5 = Pin(Pin.cpu.D0, Pin.OUT_PP)
#Configurando como salida (modo push-pull) D1=Led Indicador 6
LI6 = Pin(Pin.cpu.D1, Pin.OUT_PP)
#Configurando como salida (modo push-pull) D2=Led Indicador 7
LI7 = Pin(Pin.cpu.D2, Pin.OUT_PP)
#Configurando como salida (modo push-pull) D3=Led Indicador 8
LI8 = Pin(Pin.cpu.D3, Pin.OUT_PP)

try:
        while True:
                x=1
                for i in range(0,16):
                        #activando el LED
                        LI1.value(1 & x) #(realizando un enmascarado del valor)
                        LI2.value(2 & x)
                        LI3.value(4 & x)
                        LI4.value(8 & x)
                        LI5.value(16 & x)
                        LI6.value(32 & x)
                        LI7.value(64 & x)
                        LI8.value(128 & x)
                        if i<8:
                                x<<=1    #corrimiento a la izquierda una posición
                        else:
                                x>>=1    #corrimiento a la derecha una posición
                        delay(30)
finally:
        LI1.value(0)
        LI2.value(0)
        LI3.value(0)
        LI4.value(0)
        LI5.value(0)
        LI6.value(0)
        LI7.value(0)
        LI8.value(0)

Ophy-Ra incorpora dos Encoders de Cuadratura Magnéticos diseñados para sensar las revoluciones de cada motor. Cada encoder está formado por un par de sensores magnéticos de efecto Hall TLE4946-2K empotrados en la placa principal de Ophy-Ra. En conjunto los sensores detectan el cambio de polaridad magnética, de los discos magnéticos insertados en los ejes de los motores. Cada disco magnético tiene 6 pares de polos magnéticos Norte-Sur, por lo que el Encoder puede detectar hasta 6 posiciones por cada revolución del disco magnético. Los motores de Ophy-Ra tiene una reducción mecánica de engranes con una relación de 10:1, lo que significa que por cada 10 vueltas que de el eje del motor el eje completará una. Esto quiere decir que se re requiere sensar la posición de la llanta de Ophy-Ra se debera tener en cuenta esta relación. En otras palabras, el Encoder magnético entregará 60 posiciones del disco magnético por cada vuelta que complete el eje de las llantas.

Observaciones: Este ejemplo se diseño para calibrar los Encoders de Ophy-Ra. Una vez que lo hayas cargado el ejemplo, corre el programa y observa en la terminal PuTTY el valor de cada Encoder. Gira lentamente las llantas y observarás como los valores de leídos del Encoder irán cambiando dependiendo en que sentido gires las llantas. El objetivo es mover o deslizar los discos magnéticos hacia afuera o adentro del eje del motor, para lograr que los sensores detecten el campo correctamente el giro de las llantas; de tal manera, que cuando se giran las llantas hacia adelante las lecturas en cada Encoder se incrementen y que cuando se giren hacia atrás las lecturas disminuyan.

#Ejemplo 4.
#LENGUAJE: Python
#PLATAFORMA: Ophyra-Racing.
#DESCRIPCION:
#Ejemplo de configuracion y lectura de los Encoders de Ophyra-Racing.
#Los Encoder son de tipo magnetico o efecto hall, modelo .
#El ejemplo muestra como configurarlos y realizar una lectura.
#Gira las ruedas para detectar los pulsos enviados por los sensores.
#Este ejemplo se utiliza para calibrar las distancias de los discos magneticos de
#cada motor.

#Seccion de importacion de librerias.
from pyb import Pin
from pyb import Timer

#Sensores Magneticos (encoders)
#Activando el Timer 1 como contador (cuenta maxima hasta 100, puede ser cambiado)
ENCA = Timer(1,prescaler=1, period=100)
#Configurando el canal 1 del Timer 1 en modo Encoder (motor Izquierdo).
ENCA.channel(1,mode=Timer.ENC_AB)
#Activando el Timer 2 como contador (cuenta maxima hasta 100, puede ser cambiado)
ENCB = Timer(2,prescaler=1, period=100)
#Configurando el canal 1 del Timer 2 en modo Encoder (motor Derecho).
ENCB.channel(1,mode=Timer.ENC_AB)
#Activando los pines en modo encoder (sensor hall A1 )
ENCA_CH1 = Pin('PE9', Pin.AF_PP,pull=Pin.PULL_NONE,af=Pin.AF1_TIM1)
#Activando los pines en modo encoder (sensor hall A2 )
ENCA_CH2 = Pin('PE11',Pin.AF_PP,pull=Pin.PULL_NONE,af=Pin.AF1_TIM1)
#Activando los pines en modo encoder (sensor hall B1 )
ENCB_CH1 = Pin('PA0', Pin.AF_PP,pull=Pin.PULL_NONE,af=Pin.AF1_TIM2)
#Activando los pines en modo encoder (sensor hall B2 )
ENCB_CH2 = Pin('PA1', Pin.AF_PP,pull=Pin.PULL_NONE,af=Pin.AF1_TIM2)

#Reseteando los Encoders a una cuenta de cero
ENCA.counter(0)
ENCB.counter(0)

while True:
        A=ENCA.counter()#Lee los pulsos del encoder del motor Izquierdo
        B=ENCB.counter()#Lee los pulsos del encoder del motor Derecho
        #imprime el valor de de los encoders.
        print('A=',A,'B=',B)
        #Cuando la cuenta de los Encoders llega hasta 100 se resetea a cero automaticamente

Se muestra un ejemplo de como configurar y acceder al sensor MPU6050, que es un sensor Acelerómetro y Giroscopio de 6 grados de libertad. Este sensor puede utilizarse para medir la vibraciones presente en Ophy-Ra cuando avanza. También puede usarse como sensor adicional para el mapeo de rutas o posicionamiento del robot.

#Ejemplo 5.
#LENGUAJE: Python
#PLATAFORMA: Ophyra-Racing.
#DESCRIPCION:
#Ejemplo de configuracion y lectura del sensor Acelerometro-Giroscopio de Ophyra-Racing.
#El sensor es un acelerometro-giroscopio de 3 ejes, modelo MPU6050.
#El ejemplo muestra como leer los 3 ejes coordinados de ambos Acel y Gyro.

#Seccion de importacion de librerias.
from ophyra_mpu60 import MPU6050
from pyb import delay

#Configurando el sensor Acelerómetro-Giroscopio
SAG = MPU6050()

#(1er parámetro configura la sensibilidad del acelerómetro,
# valores aceptados son: 2, 4, 8 y 16 G) 
#(2er parámetro configura la sensibilidad del giroscopio, 
#valores aceptados son: 250,500,1000 y 2000 º/seg)
SAG.init(2,250)

while True:
        ax=SAG.accX()  #Metodo para leer el sensor en el eje X.Acelerometros
        ay=SAG.accY()  #Metodo para leer el sensor en el eje Y.
        az=SAG.accZ()  #Metodo para leer el sensor en el eje Z.
        gx=SAG.gyrX()  #Metodo para leer el sensor en el eje X. Giroscopios
        gy=SAG.gyrY()  #Metodo para leer el sensor en el eje Y.
        gz=SAG.gyrZ()  #Metodo para leer el sensor en el eje Z.

        #Imprimiendo los valores en la terminal.
        print('ax={:.3f}\tay={:.3f}\taz={:.3f}\t'.format(ax,ay,az))
        print('gx={:.3f}\tgy={:.3f}\tgz={:.3f}\t'.format(gx,gy,gz))
        delay(100)

Demuestra el funcionamiento del dispositivo Bluetooth estableciendo comunicación con una aplicación en un teléfono movil. Ophy-Ra cuenta con un par de jumpres, ubicados en el extremo inferior-derecha de la placa principal (como se muestra en la foto).

El jumper superior se utiliza para Encender/Apagar el modulo. El jumper inferior se utiliza para poner en modo de configuración (por comandos AT) al módulo.

El módulo Bluetooth de Ophy-Ra está directamente conectado al puerto serial número 3 del microcontrolador, lo que permite enviar y recibir comandos a distancia, para por ejemplo, controlar el momento en el que el robot arranca o se detenga en el recorrido, entre otras funciones, como: configurar la velocidad, la estabilidad del control de tracción, etc.

#Ejemplo 6.
#LENGUAJE: Python
#PLATAFORMA: Ophyra-Racing.
#DESCRIPCION:
#Ejemplo de comunicacion con la unidad de bluetooth HC-05 de Ophyra-Racing.
#El ejemplo muestra como comunicarse con el bluetooth a traves
#del puerto serial 3 del microcontrolador.
#El ejemplo enciende el LED Indicador 1 cuando recibe por el Bluetooth
#un caracter de la letra A, y lo apaga cuando recibe un caracter B.
#Asi mismo, se envian mensajes del estatus del LED por el Bluetooth.
#Se requiere de una aplicacion que permita enviar y recibir caracteres por
#Bluetooth en modo de comunicación Serial.

#Seccion de importacion de librerias.
from pyb import UART
from pyb import Pin
from pyb import delay

#Configuracion de los LED de Ophyra
LI1 = Pin(Pin.cpu.E12, Pin.OUT_PP)

#Creando el objeto serial para comunicacion con el BlueTooth
#Puerto serial 3, Velocidad de configuracion comun 9600
BTH = UART(3, 9600) 
BTH.init(9600, bits=8, parity=None, stop=1, timeout=0)

#Mensajes que se envian por le bluetooth
a='LED ON' 
b='LED OFF'
while True:

        entrada=BTH.read()      #Lee los datos provenientes del Bluetooth
        if entrada == b'A':
                LI1.on()        #Enciende el LED Indicador 1
                BTH.write(a)    #Envia hacia el Bluetooth el mensaje de 'LED ON' 
                print('LED encendido')#Imprime en la consola el mensaje
        elif entrada == b'B':
                LI1.off()       #Apaga el LED Indicador 1
                BTH.write(a)    #Envia hacia el Bluetooth el mensaje de 'LED OFF' 
                print('LED apagado')#Imprime en la consola el mensaje
        delay(100)              #Tiempo de espera(opcional)

En este ejemplo se muestra como puede configurarse el Bluetooth HC-05 de Ophyra. Mediante la terminal PuTTY se pueden enviar comandos AT al módulo.

Lo primero es activar el modo de configuración y comandos AT del Bluetooth HC-05. Para ello, primero debemos puentear con un jumper los pines de configuración, antes de encender el módulo (Observar las imagen).

Posteriormente encienda el módulo y ponga a correr el programa en la terminal. A continuación envíe los comandos AT que considere necesarios.

#Ejemplo 6B.
#LENGUAJE: Python
#PLATAFORMA: Ophyra-Racing.
#DESCRIPCION:
#Programa puente de comunicacion entre HC-05 de Ophyra-Racing
#y la terminal de usuario.
#Este programa permite configurar el Bluetooth mediante comandos AT.
#Para ello es necesario poner al modulo en modo AT2, en este modo la
#velocidad de transmision es a 38400 baudios.

from pyb import UART
from pyb import delay

#Creando el objeto serial para comunicación con el Bluetooth
BTH = UART(3) #Puerto serial 3
#Configurando la velocidad de 38400 baudios por defecto
BTH.init(38400, bits=8, parity=None, stop=1, timeout=1000)

while True:
        m=input('Comando=')     #Espera a que el usuario introdusca un comando AT.
        m=m+'\r\n'              #Agregando los terminadores de salto de linea y retorno de carro.
        BTH.write(m)            #Enviando el comando al Bluetooth
        resp=BTH.read()         #Esperando la respuesta del Bluetooth
        if resp!=None:          #Si el modulo responde se imprime la respuesta
                print(resp)
        delay(400)              #Tiempo de espera (opcional)

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()