'***************************************************************************** '* DIRECCIÓN NORTE *

Transcripción

1 '***************************************************************************** '* DIRECCIÓN NORTE * '* Programa en el que el robot SR1 sigue una trayectoria dirección hacia el norte hasta que se * '* encuentra un obstáculo en su camino, en ese mismo momento gira hasta que los * '* ultrasonidos dejan de detectar el obstáculo, una vez ha ocurrido esto el robot avanza una * '*distancia determinada, una vez ha avanzado para y gira la cámara hacia la dirección donde * '* se encontraba el obstáculo encendiéndola durante unos segundos para visualizarlo en un * '* monitor, pasado este tiempo apaga la cámara la coloca en su posición central y vuelve a * '* buscar el norte girando en la dirección más próxima a donde está situado este. * '***************************************************************************** Asignación de pines en el SR1 Const Txd As Byte = 1 'Pin Transmisión de datos Puerto Serie 1 Const Rxd As Byte = 2 'Pin recepción de datos Puerto Serie 1 Const Scl As Byte = 5 Const Sda As Byte = 6 Const Evc As Byte = 7 Const Buz As Byte = 8 Const Led As Byte = 9 Const IR1 As Byte = 10 Const IR2 As Byte = 11 Const IR3 As Byte = 12 'Pin del clock del bus I2C SCL 'Pin de datos del bus I2C SDA 'Pin para conectar la alimentación auxiliar 'Pin usado por el Beep 'Pin para activar los leds 'Pin de la entrada IR1 'Pin de la entrada IR2 'Pin de la entrada IR3 Const Se1 As Byte = 13 'Pin del Servo 1 Const Se2 As Byte = 14 'Pin del Servo 2 Const Se3 As Byte = 15 'Pin del Servo 3 Const Se4 As Byte = 16 'Pin del Servo 4 Const Inc As Byte = 17 Const Bur As Byte = 18 Const Bul As Byte = 19 Const Tem As Byte = 20 Const LRo As Byte = 25 'Pin usado por el sensor de inclinación 'Pin usado por el Bumper Derecho 'Pin usado por el Bumper Izquierdo 'Pin usado por sensor de temperatura 'Pin del led rojo del BasicX24

2 Const LVe As Byte = 26 'Pin del led verde del BasicX24 Constantes y variables generales Const ServoMax As Integer = 1843 'Valor Máximo del Servo 2 ms 2000/1.085 Const ServoMin As Integer = 921 'Valor Mínimo del servo 1 ms 1000/1.085 Const ServoAcel As Integer = 25 'Valor de aceleración Const ServoAce1 As Integer = 40 'Valor de aceleración servo 1 Const ServoAce2 As Integer = 40 'Valor de aceleración servo 2 Const SrfReg As Byte = 0 Const LuzReg As Byte = 1 Const RangoReg As Byte = 2 Const RangoCmd As Byte = 81 Const Sonar1 As Byte = &HE0 Const Brujula1 As Byte = &HC0 Const RumboReg As Byte = 2 Dim Se1Stop As Integer Dim Se2Stop As Integer Dim Se1Value As Integer Dim Se2Value As Integer Dim M As Long Dim I2cAck As Boolean Dim Lux As Byte Dim Dist As New UnsignedInteger Dim Busy As Boolean Dim Rprev As Integer Dim Rango As New UnsignedInteger Dim Rumbo As New UnsignedInteger 'Registro de comandos del SRF08 'Registro del sensor de luz del SRF08 'Registro de las mediciones del SRF08 'Registro de comandos 81 centímetros 'Dirección 0Xe0 del modulo SRF08 'Dirección del sensor brújula 'La dirección en registro 2 y 3(High:Low) 'Valor de parada Servo1 'Valor de parada Servo2 'Valor del Servo Servo1 'Valor del Servo Servo2 'Contador de impulsos 'Indicador de Ok 'Nivel de iluminación del SRF08 'Distancia 'Indica si el SRF08 está ocupado 'Valor de la distancia anterior 'Rango mínimo de detección 'Valor de rumbo de la brújula digital.

3 Dim DirACT As Integer Dim Acel1 As Integer Dim Acel2 As Integer Dim DiCam As Integer 'Valor de brújula en grados exactos 'Valor de aceleración servo1 en los giros 'Valor de aceleración servo2 en los giros 'Variable que indicara la dirección a la que girara la cámara Dim Se4Max As Integer 'Variable que guarda el valor máximo del servo 4 Dim Se4Min As Integer 'Variable que guarda el valor mínimo del servo 4 Dim Se4Med As Integer 'Variable que guarda el valor medio del servo 4 Dim Data As Byte 'Variable en la que guardaremos datos de los servos Programa principal Sub Main() Dim M As Integer Dim Veces As Integer Call ServoStop Rango = 30 Se1Value = Se1Stop Se2Value = Se2Stop Call PutPin(Led, 0) Call Beep_Beep 'Variable M creada como Integer(número entero) 'Variable Veces creada como Integer, usada como contador 'Recupera los valores de parada de los servos 'Este es el rango de detección en cm. 'Establece el valor inicial del servo1 como parado 'Establece el valor inicial del servo2 como parado 'Apaga los leds si están encendidos 'Hace el Beep de Inicio Call Norte Do Call Distancia If Lux < 60 Then Call PutPin(Led, 1) 'Hace una lectura de la distancia 'Comprueba el nivel de luz 'Si es bajo el nivel de luz enciende los leds Else Call PutPin(Led, 0) 'Si es alto el nivel de luz apaga los leds

4 If Dist < Rango Then Call PutPin(Lro, 0) Veces = Veces + 1 'Comprueba si hay un obstáculo en el rango 'Si lo hay enciende el led rojo 'Incrementa el contador de veces para que cambie el sentido de los giros del robot cuando detecta un obstáculo periódicamente If Veces > 4 Then Veces = 0 'Comprueba el contador para ver si ha de cambiar el giro 'Si ha entrado en el bucle resetea el contador para que no realice a partir de ahora el mismo giro Do Until Dist > Rango Call Distancia 'Gira hasta encontrar camino libre 'Inicia el proceso de medida If Veces > 2 Then Call Derecha 'Gira por la derecha Else Call Izquierda DiCam = 1 'Gira por la Izquierda 'Pone DiCam a 1 para saber que ha de girar la cámara a la izquierda Call Distancia 'Lee de nuevo la distancia Loop Call DireccionCamara(DiCam ) 'Llama a la subrutina DireccionCamara para indicarle el lugar donde estaba el obstáculo y girar la cámara hacia allí DiCam = 0 Call Adelante Call Norte 'Resetea la variable DiCam 'Llama a la subrutina adelante para hacer avanzar al robot 'Llama a la variable Norte para recalcular el rumbo Call PutPin(Lro, 1) 'Apaga el led rojo

5 Estas son las rutinas que comprueban los parachoques y la inclinación If Getpin(Bul) = 0 Then If Getpin(Bur) = 0 Then Call Atras Call GiroD 'Comprueba si esta el bumper izquierdo pulsado 'Comprueba si también está el bumper derecho 'da marcha atrás 'hace un giro para no repetir Else Call GiroD 'Si no solo hace el giro If Getpin(Bur) = 0 Then Call GiroI 'Comprueba si está el bumper derecho pulsado 'Hace el giro If Getpin(Inc) = 0 Then Call Beep If M > 5 Then Call Atras Call GiroD 'Comprueba el sensor de inclinación 'Hace una señal sonora simple 'Comprueba que está avanzando 'Si lo está, da marcha atrás 'Gira a la izquierda para no repetir 'Si no avanza es a causa de la inercia de la curva M = M + 1 Se1Value = Se1Value + ServoAcel Se2Value = Se2Value ServoAcel Call Pulsos 'Incrementa el contador de pulsos rectos 'Rueda derecha adelante 'Rueda izquierda adelante 'Manda los pulsos a los servos Loop

6 Subrutina para comprobar la dirección a la que ha de girar la cámara Sub DireccionCamara(ByVal Comprobar As Integer) Dim T As Integer If Comprobar = 1 Then Data = PersistentPeek(106) Se4Max = CInt(Data) * 10 For T = 1 To 20 Call PulseOut(Se4, Se4Max, 1) Call Sleep(10) 'Crea la variable T como Integer para usarla en bucles 'Gira cámara por la Izquierda 'Recupera el tercer valor 'Lo guarda como un integer 'Bucle usado para girar la cámara 'Manda el pulso al servo horizontal 'Retardo entre pulsos 10 x 1,95 ms Call PutPin(Evc, 1) Dim N As Integer For N = 1 To 'Enciende la cámara 'Bucle que mide el tiempo de encendido de la cámara 'Variable N que mide las veces que se ejecuta el bucle Call PutPin(Evc, 0) Data = PersistentPeek(105) Se4Med = CInt(Data) * 10 For T = 1 To 20 Call PulseOut(Se4, Se4Med, 1) Call Sleep(10) 'Apaga la cámara 'Recupera el segundo valor 'Lo guarda como un integer 'Bucle para poner la cámara a su posición central 'Manda el pulso al servo horizontal 'Retardo entre pulsos 10 x 1,95 ms Else Data = PersistentPeek(104) Se4Min = 800 CInt(Data) * 10 For T = 1 To 20 Call PulseOut(Se4, Se4Min, 1) Call Sleep(10) 'Recupera el primer valor 'Lo guarda como un integer 'Bucle usado para girar la cámara 'Pulso servo horizontal 'Retardo entre pulsos 10 x 1,95 ms

7 Call PutPin(Evc, 1) Dim M As Integer For M = 1 To 'Enciende la cámara 'Bucle que mide el tiempo de encendido de la cámara 'Variable M que mide las veces que se ejecuta el bucle Call PutPin(Evc, 0) Data = PersistentPeek(105) Se4Med = CInt(Data) * 10 For T = 1 To 20 Call PulseOut(Se4, Se4Med, 1) Call Sleep(10) 'Apaga la cámara 'Recupera el segundo valor 'Lo guarda como un integer 'Bucle para poner la cámara a su posición central 'Manda el pulso al servo horizontal 'Retardo entre pulsos 10 x 1,95 ms

8 Subrutina que hace un giro hacia el norte por el lado más corto Sub Norte() Acel1 = 80 Acel2 = 80 Call Dir If DirACT < 180 Then 'Establece un valor de aceleración 'rápido para los dos servos 'Comprueba el rumbo actual 'Si es más pequeño de 180 gira a la derecha Do Call Dir If DirACT > 345 Then 'Comprueba la dirección 'Cuando esta próximo al norte reduce la aceleración Acel1 = 40 'Frena el servo 1 Acel2 = 40 'Frena el servo 2 If DirACT < 10 Then 'Evita que se pase el 359 Exit Do Se1Value = Se1Stop + Acel1 Se2Value = Se2Stop + Acel2 Call Pulsos 'Frena la rueda derecha 'acelera la rueda izquierda 'Manda los pulsos a los servos Loop Else 'Si es mas de 180 gira por la izquierda Do Call Dir If DirACT < 15 Then 'Comprueba la dirección 'Cuando esta próximo al norte reduce la aceleración Acel1 = 40 'Frena el servo 1 Acel2 = 40 'Frena el servo 2 If DirACT > 350 Then 'Evita que se pase el 0

9 Exit Do 'saliendo del bucle Se1Value = Se1Stop Acel1 Se2Value = Se2Stop Acel2 Call Pulsos 'Adelante la rueda derecha 'atrás la rueda izquierda 'manda los pulsos a los servos Loop Subrutina que mide la dirección con el sensor brújula y lo asigna a DirACT para los giros Sub Dir() Rumbo = I2CWordRead(Brujula1, RumboReg) Rumbo = Rumbo \ 10 DirACT = CInt(Rumbo) 'Lee el registro de la dirección en grados 'Le quita los decimales 'Lo guarda en la variable DirACT Subrutina para que el robot avance recto Sub Adelante() Dim N As Integer Se1Value = Se1Stop Se2Value = Se2Stop For N = 1 To 75 Se1Value = Se1Value + ServoAce1 Se2Value = Se2Value ServoAce2 Call Pulsos 'Variable N inicializada como Integer 'Pone el servo 1 en su posición de parada 'Pone el servo 2 en su posición de parada 'Este número controla la cantidad del avance 'Avanza la rueda derecha 'Avanza la rueda izquierda 'Envía los impulsos a los servos

10 Subrutina que hace que el robot avance hacia la derecha Sub Derecha() Dim N as Byte For N = 1 to 2 Se1Value = Se1Value + ServoAcel Se2Value = Se2Value + ServoAcel Call Pulsos 'Variable N inicializada como Byte 'Bucle que hace girar progresivamente a la derecha 'Avanza la rueda derecha 'Retrocede la rueda izquierda 'Manda los pulsos a los servos Subrutina que hace que el robot avance hacia la izquierda Sub Izquierda() Dim N as Byte For N = 1 to 2 Se1Value = Se1Value ServoAcel Se2Value = Se2Value ServoAcel Call Pulsos 'Variable N inicializada como Byte 'Bucle que hace girar progresivamente a la izquierda 'Retrocede la rueda derecha 'Avanza la rueda izquierda 'Manda los pulsos a los servos

11 Subrutina que manda los pulsos a los servos Sub Pulsos() If Se1Value > ServoMax Then 'Verifica que el valor del servo 1 está dentro del límite máximo, y si lo sobrepasa lo limita Se1Value = ServoMax 'Coloca el servo 1 en su posición máxima If Se2Value > ServoMax Then 'Verifica que el valor del servo 2 está dentro del límite máximo, y si lo sobrepasa lo limita Se2Value = ServoMax 'Coloca el servo 2 en su posición máxima If Se1Value < ServoMin Then 'Verifica que el valor del servo 1 está dentro del límite mínimo, y si lo sobrepasa lo limita Se1Value = ServoMin 'Coloca el servo 1 en su posición mínima If Se2Value < ServoMin Then 'Verifica que el valor del servo 2 está dentro del límite mínimo, y si lo sobrepasa lo limita Se2Value = ServoMin 'Coloca el servo 2 en su posición mínima Call PulseOut(Se1, Se1Value, 1) Call PulseOut(Se2, Se2Value, 1) Call Sleep(10) 'Pulso servo izquierdo 'Pulso servo derecho 'Retardo entre pulsos 10 x 1,95 ms

12 Subrutina que mide la distancia y el nivel de luz del SRF08 y si el sensor está ocupado sale para no perder tiempo Sub Distancia() If Busy = False Then 'Si esta libre inicia el proceso de medida Call I2cByteWrite(Sonar1, SrfReg, RangoCmd) Busy = True Exit Sub 'Pone el indicador de ocupado 'Y sale para no perder más tiempo If I2CByteRead(Sonar1, SrfReg) = 255 Then 'Lee la versión del firmware para saber si ha terminado de medir, si no ha terminado sale Exit Sub Else Lux = I2CByteRead(Sonar1, LuzReg) Dist = I2CWordRead(Sonar1, RangoReg) Busy = False 'Si ha terminado lee los datos 'Lee el sensor de luz 'Lee la distancia medida 'Quita el indicador de ocupado

13 Subrutina que hace que el robot gire a la derecha Sub GiroD() Dim N As Byte M = 0 Se1Value = Se1Stop Se2Value = Se2Stop Call Sleep(100) For N = 1 To 5 'Creamos la variable N de tipo Byte 'Limpia el contador de impulsos 'Ponemos el servo 1 en su posición de parada 'Ponemos el servo 2 en su posición de parada 'Hace una pausa para que se paren los servos 'Al cambiar esta cantidad se cambia los grados de giro que también dependen del valor ServoAcel Se1Value = Se1Value + ServoAcel Se2Value = Se2Value + ServoAcel Call Pulsos 'Avanza la rueda derecha 'Retrocede la rueda izquierda 'Manda los pulsos a los servos Subrutina que hace que el robot gire a la izquierda Sub GiroI() Dim N As Byte Se1Value = Se1Stop Se1Value = Se2Stop Call Sleep(100) If M < 100 Then 'Creamos la variable N de tipo Byte 'Ponemos el servo 1 en su posición de parada 'Ponemos el servo 2 en su posición de parada 'Hace una pausa para que se paren los servos 'Si acaba de girar da una vuelta completa para evitar el Efecto esquina For N = 1 To 12 'Al cambiar esta cantidad se cambia los grados de giro que también dependen del valor ServoAcel Se1Value = Se1Value ServoAcel Se2Value = Se2Value ServoAcel 'Retrocede la rueda derecha 'Avanza la rueda izquierda

14 Call Pulsos 'Manda los pulsos a los servos Else For N = 1 To 5 Se1Value = Se1Value ServoAcel Se2Value = Se2Value ServoAcel Call Pulsos 'si no acaba de girar da un giro normal 'Bucle que efectúa el giro 'Retrocede la rueda derecha 'Avanza la rueda izquierda 'Manda los pulsos a los servos Subrutina que hace que el robot de marcha a atrás Sub Atras() Dim N As Byte M = 0 Se1Value = Se1Stop Se2Value = Se2Stop Call Sleep(100) For N = 1 To 20 Se1Value = Se1Value ServoAcel Se2Value = Se2Value + ServoAcel Call Pulsos 'Creamos la variable N de tipo Byte 'Limpia el contador de impulsos 'Ponemos el servo 1 en su posición de parada 'Ponemos el servo 2 en su posición de parada 'Hace una pausa para que se paren los servos 'Al cambiar esta cantidad se cambia los grados de giro 'Retrocede la rueda derecha 'Retrocede la rueda izquierda 'Manda los pulsos a los servos

15 Subrutina que recupera los valores de parada de los servos desde la posiciones 100 y 101 de la eeprom. Sub ServoStop() Dim Data As Byte Data = PersistentPeek(100) Se1Stop = CInt(Data) Data = PersistentPeek(101) Se2Stop = CInt(Data) 'Creamos la variable Data de tipo Byte 'Recupera el primer valor 'Lo guarda como un integer 'Recupera el segundo valor 'Lo guarda como un integer Subrutina para que el robot haga un Beep Beep por el altavoz Public Sub Beep_Beep() Dim FBase As Integer FBase = 2000 Call FreqOut(Buz, FBase, FBase, 200) Call FreqOut(Buz, 0, 0, 40) Call FreqOut(Buz, FBase, FBase, 200) 'Creamos la variable FBase de tipo Integer 'Esta es la frecuencia de resonancia del altavoz 'Da un tono 'Efectúa una pausa 'Hace otro tono Subrutina que hace un Beep por el altavoz Public Sub Beep() Dim FBase As Integer FBase = 2000 Call FreqOut(Buz, FBase, FBase, 200) 'Creamos la variable FBase de tipo Integer 'Esta es la frecuencia de resonancia del altavoz 'Emite un tono corto

16 '***************************** ' * Rutinas de comunicación I2C * '***************************** Subrutina que escribe un Byte en el Bus I2C Sub I2cByteWrite(ByVal I2cAddr As Byte, ByVal I2cReg As Byte, ByVal I2cData As Byte) Call I2cStart Call I2cOutByte(I2cAddr) Call I2cOutByte(I2cReg) Call I2cOutByte(I2cData) Call I2cStop 'Manda el STAR 'Envía la dirección 'Envía número de registro 'Envía el dato 'Manda el STOP Subrutina que lee un byte por el bus I2C Function I2CByteRead(ByVal I2cAddr As Byte, ByVal I2cReg As Byte) As Byte Call I2cStart Call I2cOutByte(I2cAddr) Call I2cOutByte(I2cReg) Call I2cStart I2cAddr = I2cAddr + 1 Call I2cOutByte(I2cAddr) I2cAck = False I2CByteRead = I2cInByte() Call I2cStop 'Manda el STAR 'Envía la dirección 'Envía número de registro 'Envía start 'Pone modo de lectura 'Envía la dirección de lectura 'Envía Nak 'Lee el byte con Nak 'Manda el STOP End Function

17 Subrutina que lee una palabra (2 Bytes) desde el Bus I2C Function I2CWordRead(ByVal I2cAddr As Byte, ByVal I2cReg As Byte) As UnsignedInteger Set I2CWordRead = New UnsignedInteger Call I2cStart Call I2cOutByte(I2cAddr) Call I2cOutByte(I2cReg) Call I2cStart I2cAddr = I2cAddr + 1 Call I2cOutByte(I2cAddr) I2cAck = True I2CWordRead = CuInt(I2cInByte() * 256) I2cAck = False I2CWordRead = I2CWordRead + CuInt(I2cInByte()) Call I2cStop 'Manda el STAR 'Envía la dirección 'Envía número de registro 'Envía start 'Pone modo de lectura 'Envía la dirección de lectura 'Envía Ack 'Lee el primer byte 'Envía Nak 'Lee el siguiente byte y lo suma 'Manda el STOP End Function Subrutina que envía un Byte por el bus I2C Sub I2cOutByte(I2cData As Byte) Call ShiftOut(Sda, Scl, 8, I2cData) Call PutPin(Sda, bxinputtristate) 'Saca el dato secuencialmente 'Pin de datos en alta impedancia Call PutPin(Scl, bxoutputhigh) 'Pone SCL a 1 Call PutPin(Scl, bxoutputlow) 'Pone SCL a 0

18 Subrutina que recibe un Byte por el Bus I2C Function I2cInByte() As Byte I2cInByte = ShiftIn(Sda, Scl, 8) If I2cAck = True Then 'Lee el dato de forma secuencial 'Indica que se va a recibir mas Call PutPin(Sda, bxoutputlow) 'Pone a Sda a 0 Else 'Si no se va a recibir mas Call PutPin(Sda, bxoutputhigh) 'Pone a Sda a 1 Call PutPin(Scl, bxoutputhigh) 'Pone a SCL a 1 Call PutPin(Scl, bxoutputlow) 'Pone a SCL a 0 End Function Subrutina que manda el comando Start de I2C combinando las líneas SDA y SCL Sub I2cStart() Call PutPin(Sda, bxoutputhigh) 'Pone a Sda a 1 Call PutPin(Scl, bxoutputhigh) 'Pone a SCL a 1 Call PutPin(Sda, bxoutputlow) 'Pone a Sda a 0 Call PutPin(Scl, bxoutputlow) 'Pone a SCL a 0 Subrutina que manda el Comando Stop del I2C combinando las líneas SDA y SCL Sub I2cStop() Call PutPin(Sda, bxoutputlow) 'Pone a Sda a 0 Call PutPin(Scl, bxoutputhigh) 'Pone a SCL a 1 Call PutPin(Sda, bxoutputhigh) 'Pone a Sda a 1