Robot laberinto Raúl Antona Fresno PE21

Transcripción

1 Faunos Laberinto (Robot laberinto) RAÚL ANTONA FRESNO

2 Índice: Página INTRODUCCIÓN DEL PROYECTO. 2 2 DIAGRAMA EN BLOQUES EXPLICACIÓN DEL DIAGRAMA EN BLOQUES. 7 3 ESQUEMA ELECTRICO GENERAL EXPLICACIÓN DEL ESQUEMA ELECTRICO GENERAL. 9 4 FUENTE DE ALIMENTACION REGULADA A 9V/5V SISTEMA DE PROTECCIÓN SISTEMA DE ELIMINACIÓN DE RUIDOS. 4.3 REGULADOR DE TENSIÓN. 5 ADAPTADOR DE SEÑALES PC-μC. 6 SENSORES. 3 6.ESTUDIO DE LA RECTA DEL GP2D PROGRAMA Y DIAGRAMA DEL DRIVER GP2D PROGRAMA Y DIAGRAMA DE LA REPRESENTA CIÓN DE DISTACIAS EN EL DISPLAY. 8 7 CONTROL DE POTENCIA CONTROL DE GIRO Y VELOCIDAD TABLAS DE FUNCIONAMIENTO MOTOR DERECHO MOTOR IZQUIERDO PROGRAMAS Y DIAGRAMAS DE CONTROL DE VELOCIDAD VELOCIDAD MOTOR IZQUIERDO VELOCIDAD MOTOR DERECHO CONTROL DE GIRO DEL MOTOR DERECHO CONTROL DE GIRO DEL MOTOR IZQUIERDO SISTEMA DE CONTROL: 32 8.LA PANTALLA LCD HDD PATILLAJE DEL LDC DIAGRAMA Y PROGRAMA DE PRUEBA DEL LCD RECEPTOR DE DATOS CEBEK C PIC OTROS BREVE EXPLICACIÓN DEL PROGRAMA PRINCIPAL ESQUEMA ELECTRICO DE SIMULACIÓN DIAGRAMA Y PROGRAMA PRINCIPAL. 39 2

3 0 FABRICACIÓN DE PLACAS FABRICACIÓN PLACAS CARA BOTTOM FABRICACIÓN CARA TOP FABRICACIÓN CARA COMPONENTES. 62 DISEÑO MECÁNICO Lista de Componentes Desarrollo del Proyecto REFERENCIAS ANEXOS. 68 3

4 INTRODUCCIÓN DEL PROYECTO: El proyecto que he realizado a lo largo del segundo curso del módulo de desarrollos de productos eléctricos, es un robot laberinto. Este robot tiene la siguiente función: una vez introducido en un laberinto por una de las cuatro entradas, tiene que ser capaz de salir por la entrada contraria autónomamente, en el menor tiempo posible. El laberinto tiene unas proporciones de 3,6 x 3,6 y el interior del laberinto está compuesto por calles verticales y horizontales, con una separación entre paredes de 40 cm. aproximadamente y un espesor de,5 cm. Cada calle tiene una altura de 25 cm. y está dispuesta en ángulo de 90º. El color del material es blanco. Ejemplo: Las medidas están ajustadas entre las máximas determinadas por el consejo regulador de Madridbot: Los robots han de poseer unas dimensiones máximas, durante todo el transcurso de la prueba, de 20 cm. de ancho, 30 cm. de largo y 25 cm. de alto, siendo libre el peso que puedan tener. 4

5 El proyecto desde el comienzo ha estado separado en varios bloques, tanto el robot en si, como la forma de trabajo. Todo el trabajo se ha dividido en tres asignaturas, las cuales han sido orientadas conjuntamente a la realización del robot. En la asignatura de Fabricación y diseño de placas : en ámbito de diseño, hemos aprendido ha realizar las placas con unas medidas determinadas y a distribuir los componentes de forma que estén alineados para que ocupen el menor espacio sacando el mayor rendimiento posible. En fabricación se han puesto practica conocimientos teóricos, comprobando en las placas del robot los distintos métodos; desde los más caseros hasta métodos casi profesionales. En Mantenimiento de equipos eléctricos : hemos realizado gran diversidad de prácticas, las cuales nos han permitido aprender el funcionamiento de los equipos de trabajo, comprobando mediante ensayos el funcionamiento de las partes del robot; desde las situaciones más favorables hasta las condiciones más desfavorables. Es importante tener en cuenta, que sin los contenidos de las asignaturas de electrónica digital y analógica del pasado curso (en las que se impartieron los componentes utilizados en el robot) no habría sido posible comprender el funcionamiento de dicho proyecto. Desarrollo de proyectos : esta asignatura se ha enfocado al conocimiento de la programación en pic, para lograr que el robot realice de forma autónoma el laberinto sirviéndose de un programa adecuado. Es importante señalar la unión que existe entre ésta asignatura y la asignatura de Técnicas de programación, ya que dicha asignatura trató la programación en lenguaje C; conocimiento necesario para realizar muchas de las funciones específicas del pic. 5

6 2 DIAGRAMA EN BLOQUES 6

7 2. EXPLICACIÓN DEL DIAGRAMA EN BLOQUES: Con el diagrama en bloques se mantiene una vista ordenada y esquematizada de las partes que forman el robot. Cada bloque de éste puede ser abierto. Dentro de cada uno encontramos los esquemas eléctricos de cada parte: En el bloque de adaptador de señales PC-μC y regulador 9V/5V, se encuentran los esquemas eléctricos de la alimentación de los circuitos y los componentes necesarios para realizar la transmisión de datos desde el ordenador al robot. Dentro del bloque de sensores, encontramos los sensores con los que el robot detecta las paredes del laberinto y se orientara en cada parte del circuito. En el bloque de potencia están situados los circuitos con los que manejamos el control de los motores. El bloque de sistema de control se encarga de enviar, recibir y administrar las señales al resto de bloques. Esta compuesto por el microprocesador, encargado de administrar las señales, y el receptor de señales cebek, por el cual podemos recibir señales AM desde un emisor externo. Haciendo una burda comparación con el cuerpo humano, para ver de los avances tecnológicos y lo que significan estos; podríamos decir que el bloque regulador 9V/5V correspondería a nuestro corazón que envía la alimentación necesaria al resto de bloques. El bloque de sensores podrían ser los ojos que envían información al cerebro. En nuestro caso el bloque de sistema de control, quien administra dicha información y la envía a las extremidades, el bloque de control de potencia. 7

8 3 ESQUEMA ELECTRICO GENERAL: 8

9 3. EXPLICACIÓN DEL ESQUEMA ELECTRICO GENERAL: El esquema eléctrico, representa abiertas cada una de las partes de los bloques con todos sus componentes. Esta división ha servido para la fabricación de, ya que los componentes se han dividido en distintas placas, para poder tener algunas placas comunes con otros compañeros y el robot quede con un tamaño más reducido. Las uniones entre las distintas partes del robot, por las que alimentaremos los componentes y enviaremos las señales se han realizado con fajas de diez, excepto la alimentación de los motores que al estar alimentados con distintas tensiones, se han hecho con cables flexibles. 9

10 4 FUENTE DE ALIMENTACION REGULADA A 9V/5V: La fuente de alimentación regulada es necesaria para alimentar las distintas partes del circuito. Con una batería de 9 voltios y este diseño, he conseguido los 5 voltios necesarios para alimentar los componentes del robot y obtener aproximadamente 9 voltios para alimentar los motores. El LM350, es un regulador ajustable con capsulado TO3 capaz de suministrar 3 A; este componente debido a la corriente que suministra es recomendable ponerlo con un disipador o dejar en la placa una superficie de cobre libre, que pueda disipar el calor. 4. SISTEMA DE PROTECCIÓN: D es un diodo de protección, que sirve para proteger los componentes, en caso de conectar erróneamente la batería; este diodo permite el paso de corriente si se polariza en directa, para ello el positivo de la pila tiene que estar conectado al ánodo del diodo. D2, se conecta en polarización inversa y sirve para proteger el LM350K en caso de que haya una diferencia de potencial; esto puede ocurrir en el apagado, que haya una tensión más positiva en la salida que en la entrada, en este caso el diodo se polariza en directa evitando que la corriente vaya por el LM350K. D4 es un diodo que se pone para proteger el LM350K cuando queramos trabajar con tensiones superiores a 25V o por si tenemos tensiones inversas. D3 es un diodo tipo led que sirve para informar que la fuente esta encendida, para evitar fundir el diodo hemos limitado la corriente con el resistencia de 220Ω 0

11 4.2 SISTEMA DE ELIMINACIÓN DE RUIDOS Y FILTROS. Los condensadores C2 y C4 son condensadores que sirven para la eliminación de ruidos, estos condensadores no sirven de filtros de tensión debido a que tienen un valor muy pequeño, son solamente filtros de ruido. C3 y C son condensadores de filtro, que lo que hacen es que tengamos una tensión continua más pura. 4.3 REGULADOR DE TENSIÓN: La regulación con el LM350K se hace con R38, RV, y C5; R38 es una resistencia que sirve para aumentar el suministro de corriente al potenciómetro, con RV se consigue variar la tensión de salida (VDD3) y con C5 con seguimos estabilizar la tensión de salida, para que no se produzcan saltos. V2_B es el la conexión donde se obtiene aproximadamente los 9V. VDD3_B es la conexión de salida 5 ADAPTADOR DE SEÑALES PC-μC. Para la transmisión de datos del ordenador hasta el μc se utiliza el puerto serie del ordenador, para ello se han tenido que adaptar las señales que nos envía el ordenador; este tipo de conversiones son del tipo RS232, se trata de adaptar las señales RS a TTL.

12 Para convertir las señales de RS a TTL he utilizado el integrado MAX232, este integrado necesita cuatro condensadores y una fuente de alimentación de 5V (VDD3_B), C3 y C2 adaptan el nivel de voltaje tomado de la alimentación de +5V a +0V, C y C4 invierten los niveles de voltaje para que se puedan obtener -0V, estos niveles de voltaje son utilizados para realizar la adaptación de los voltajes RS232 y se encuentran dentro de los rangos permitidos por la norma RS232. La conversión RS232 trabaja con lógica negativa esto quiere decir: que las conversiones son como se muestran en la siguiente tabla: N.L. TTL Tensión N.L. RS Tensión 5V -0V 0 0V 0 +0V Nota: Las placas de transmisión de datos y la fuente de alimentación se han fabricado las dos sobre la misma superficie. 2

13 6 SENSORES: El sensor que se ha utilizado para detectar las paredes ha sido UN sensor SHARP modelo GP2D2. El sensor GP2D2 es un sensor de infrarrojos, que trabaja de forma analógica, esto significa que nos da una tensión con respecto a la distancia a la que se encuentre de la pared. 3

14 6.ESTUDIO DE LA RECTA DEL GP2D2: 4

15 Para poder trabajar correctamente con el sensor GP2D2; se ha tenido que hacer un estudio previo del voltaje, que nos daba el sensor en función de la distancia a la que se encontraba de la pared, como no era proporcional se ha tenido que linealizar la curva del sensor para saber cual es la distancia en un determinado punto; eso se ha realizado utilizando la ecuación de la recta. L X 2 X X Y 2 Y Y 2 Y El sensor GP2D2 tiene un problema, en distancias inferiores a 8CM el rango de tensiones se repiten para varias distancias, por eso ha resultado imposible linealizar esa parte de la curva y los rangos comprendido entre 0CM y menores de 8CM han sido capados, igualándolos a 8CM. En la parte contraria donde el sensor se encuentra a en unas distancias superiores a 55CM se han capado las distancias superiores, porque en valores superiores a 55CM el sensor es inestable. Hay que tener en cuenta que todos los valores comprendidos entre 8CM y 55CM, pueden tener un margen de error, ya que el sensor no tiene la misma eficacia cuando reflecta sobre un material negro o sobre un material blanco. Se ha introducido un condensador de 0 μf, el cual lo recomienda el fabricante para eliminar posibles ruidos. Después de aprender el funcionamiento del sensor GP2D2, tuve que crear una huella en PROTEUS, para poder trabajar en ISIS, ARES y crear un DRIVER con la ecuación de la recta para obtener la distancia a la que se encontraba el robot. Las huellas son las mostradas en la figura 5 y figura 6. La huella del ISIS no puede producir tensiones; a si que se ha creado un circuito que proporciona una tensión, con esta tensión se hace una conversión analógica que es introducida al micro por el PUERTO A, esta tensión sirve para obtener la distancia en la que se encuentra el robot, con el DRIVER apropiado. Con este circuito lo que se ha conseguido es una tensión variable entre 0,5V y 2,6V. 5

16 6.2 PROGRAMA Y DIAGRAMA DEL DRIVER GP2D2: //*****************************DRIVER DE CONTROL DEL SENSOR DE DISTANCIA Gp2d2, para una recta entre 0 cm. y 55 cm. //HACE LA CONVERISON DE TENSION PROCEDENTE DEL SENSOR Gp2d2 A UNA //DISTANCIA (SE OBTIENE UNA DISTANCIA EN FUNCION DE LA TENSIÓN). //SE INTRODUCE UN VALOR DE TENSIÓN (v) TIPO FLOAT Y DEVUELVE UNA DISTANCIA //(d)float. //****************************** DECLARACIÓN DEFUNCIONES********************** float Gp2d2_v_d(float); //******************************FUNCIÓNGp2d2_v_d******************************** float Gp2d2_v_d(float v){ float l; //declaración de la variable l. l=0; if(v>2.6) // preguntamos por el valor de v. l = 8; if(v<2.6&&v>=.9) // preguntamos por el valor de v. l = ((8-3.5)* (v -.9) /(2.6-.9)); //función de la recta. if(v<.9&&v>=.65) // preguntamos por el valor de v. 6

17 l = 6 + ((3.5-6)* (v-.65) / (.9-.65)); //función de la recta. if(v<.65&&v>=.38) // preguntamos por el valor de v. l = 20 + ((6-20)* (v-.38) / ( )); //función de la recta. if(v<.38&&v>=.5) // preguntamos por el valor de v. l = 25 + ((20-25)* (v-.5) / (.38-.5)); //función de la recta. if(v<.5&&v>=0.95) // preguntamos por el valor de v. l = 30 + ((25-30)* (v- 0.95) / ( )); //función de la recta. if(v<0.95&&v>=0.75) // preguntamos por el valor de v. l = 40 + ((30-40)* (v- 0.75) / ( )); //función de la recta. if(v<0.75&&v>=0.68) // preguntamos por el valor de v. l = 45 + ((40-45)* (v- 0.68) / ( )); //función de la recta. if(v<0.68&&v>=0.58) // preguntamos por el valor de v. l = 55 + ((45-55)* (v- 0.58) / ( )); //función de la recta. if(v<0.58) // preguntamos por el valor de v. l=55; return(l); //devolvemos l. 7

18 6.3 PROGRAMA Y DIAGRAMA DE LA REPRESENTACIÓN DE DISTACIAS EN EL DISPLAY. Una de las pruebas más importantes que se han realizado con el GP2D2 y su DRIVER, esta es la representación de las distancias y la tensión en la pantalla del LCD. //*************************Conversión Analógica Digital de la Patilla AN0 ******************* #include <6F876A.h> #device adc=0 //Conversor Analógico Digital de 0 bit el PIC 6F876A puede trabajar con 8 o 0 bit //de resolución. #FUSES XT,NOWDT #use delay(clock= ) #include <LCD.c> // Incluimos el driver LCD.c que contiene las funciones de control del LCD. #include <driver_gp2d2.c> //*******************************PROGRAMA PRINCIPAL***************************** 8

19 void main(){ int6 q; float d, v; setup_adc_ports(0); // Seleccionamos el Puerto A como entradas Analógicas. Mirar //ADCON. setup_adc(adc_clock_internal); // Fuente de reloj RC interno lcd_init(); while(){ set_adc_channel(0); delay_us(20); q = read_adc(); v = (5.0 * q) / 024.0; // Inicializamos el LCD. // Bucle infinito. // HabilitaciOn canal 0 "AN0" // Retardo de 20uS necesaria para respetar el tiempo //de //Adquisición Tad. // Lectura canal 0 "AN0" // Conversion a tensión del código digital "q". d=gp2d2_v_d(v); // Cargamos el driver GP2D2 en la variable. lcd_gotoxy(,); // Posicionamos el Cursor en la posición, línea. printf(lcd_putc, "voltios = %0.3fV", v); // Saltamos de línea y escribimos en el LCD //"VOLTAJE =" y 4 dígitos de "P" // en formato truncado de 4 dígitos con 3 decimales y //el //carácter "V". lcd_gotoxy(,2); printf(lcd_putc, "d = %0.4fcm",d ); // Saltamos de línea y escribimos en el LCD //"distancia //=" y 4 dígitos de "P" // en formato //truncado de 4 //dígitos con 3 decimales y el carácter //"V". 9

20 7 CONTROL DE POTENCIA: 7. CONTROL DE GIRO Y VELOCIDAD: Para el control de giro de los motores se ha utilizado el inversor de giro L298, que poniendo unas tensiones en las patillas de entradas IN, IN2, IN3, IN4; se puede controlar el paro de los motores, el giro de los motores en sentido horario o en contra horario. Los optoaclopadores se utilizan para independizar el circuito de control con el de potencia. Ejemplo: RA4_A= (N.L), no hay diferencia de potencial en U3, el fototransistor al corte, (N.L.) en In. RA4_A=0(N.L), hay diferencia de potencial en U3, el fototransistor saturado, 0(N.L.) en In. El circuito compuesto por los optoacopladores U7, U8 y los transistores Q, Q2, Q3, Q4, sirven para habilitar las patillas Vsens A y B. Se ha puesto un condensador para eliminar las señales que puedan producir ruido. Los diodos son de protección. 20

21 7.2 TABLAS DE FUNCIONAMIENTO: 7.2.MOTOR DERECHO: Entradas VsensA ENA In In2 0 X 0 X X 0 0 X X 0 0 Salidas OUT (N.L.) OUT2 (N.L.) Giro del motor Parado Parado Parado *Contra horario *horario Parado * Horario: sigue el sentido de las agujas del reloj. * Contra horario: contra del sentido de las agujas del reloj MOTOR IZQUIERDO: Entradas VsensA ENA In In2 0 X 0 X X 0 0 X X 0 0 Salidas OUT (N.L.) OUT2 (N.L.) Giro del motor Parado Parado Parado *horario *Contra horario Parado * Horario: sigue el sentido de las agujas del reloj. * Contra horario: contra del sentido de las agujas del reloj. 2

22 Para controlar la velocidad de giro de los motores, se ha utilizado PWM (Pulse Width Modulation), esto quiere decir modulación de la anchura del pulso. La PWM es generada por el pic e introducida al L298 por las Vsens. El funcionamiento de la PWM es similar a; como si metiéramos pulsos con un interruptor, cuanta mas duración tenga el pulso, más velocidad alcanzará en motor. La PWM ha servido para controlar el robot en las distintas partes del circuito, cuando el robot se encontraba en una recta dentro de unos rangos determinados, se ha aplicado la PWM máxima a los dos motores; en caso de que el robot se desplace hacia la izquierda, se corrige el movimiento disminuyendo la PWM en el motor derecho; si el robot se desplaza al lado contrario se disminuye la PWM del motor izquierdo. En caso de que el robot se encuentre en una curva la PWM es mucho menor o incluso se puede invertir el giro del motor, para que el giro sea mucho más brusco. 7.3 PROGRAMAS Y DIAGRAMAS DE CONTROL DE VELOCIDAD VELOCIDAD MOTOR IZQUIERDO. //*****************************************Laberinto ******************************* //*****************************************Directivas de preprocesado******************* #include <6F876A.h> #device adc=0 // Conversor Analógico Digital de 0 bit el PIC 6F876A puede trabajar con 8 //o 0 bit de resolución. 22

23 #FUSES XT,NOWDT #use delay(clock= ) #include <LCD.c> // Incluimos el driver LCD.c que contiene las funciones de control del LCD. #BYTE TRISB = 0x86 // TRISB en 86h. #BYTE portb = 0x06 // PORTB en 06h. #BYTE TRISC = 0x87 // TRISC en 87h. #BYTE portc = 0x07 // PORTC EN 07h. #BYTE TRISA = 0x85 // TRISA en 85h. #BYTE porta = 0x05 // PORTA en 05h. #BIT RB3 = 0x06.3 #BIT RC0 = 0x07.0 #BIT RC = 0x07. #BIT RC2 = 0x07.2 #BIT RC3 = 0x07.3 #BIT RC4 = 0x07.4 #BIT RC5 = 0x07.5 #BIT RC6 = 0x07.6 #BIT RC7 = 0x07.7 #BIT RA2 = 0x05.2 #BIT RA4 = 0x05.4 #BIT RA5 = 0x05.5 // DEFINO LA PATILLA 3 DEL PORTB COMO RB3. // DEFINO LA PATILLA 0 DEL PORTC COMO RC0. // DEFINO LA PATILLA DEL PORTC COMO RC. // DEFINO LA PATILLA 2 DEL PORTC COMO RC2. // DEFINO LA PATILLA 3 DEL PORTC COMO RC3. // DEFINO LA PATILLA 4 DEL PORTC COMO RC4. // DEFINO LA PATILLA 5 DEL PORTC COMO RC5. // DEFINO LA PATILLA 6 DEL PORTC COMO RC6. // DEFINO LA PATILLA 7 DEL PORTC COMO RC7. // DEFINO LA PATILLA 2 DEL PORTA COMO RA2. // DEFINO LA PATILLA 4 DEL PORTA COMO RA4. // DEFINO LA PATILLA 5 DEL PORTA COMO RA5. //**************************************Declaración funciones*************************** int6 TH =65535; //*******************************PROGRAMA PRINCIPAL****************************** void main(){ TRISB = 0B ; // Defines Puerto B como SALIDA de datos. TRISC = 0B ; // Defines Puerto C como SALIDA de datos. TRISA = 0B0000; // Defines Puerto A como ENTRADA de datos. //portb = 0B ; // Reseteamos el Puerto B RA2 = 0; RA4 = 0; RB3 = 0; RC0 = 0; RC5 = 0; RC6 = 0; RC7 = 0; setup_timer_2(t2_div_by_6,249,); // setup_timer(prescaler,pr2,postscaler) // Configuración timer2. Si el Periodo = 6mS ----> T = //6000uS 23

24 setup_ccp2(ccp_pwm); setup_ccp(ccp_pwm); TH=0; set_pwm_duty(th); set_pwm2_duty(th); TRISC = 0B ; // T = [PR2+] x Tcm x Postscaler x Prescaler // PR2 puede valer de 0 a 255. // Tcm es el tiempo de Ciclo Maquina. Tcm = 4/Fosc = 4/ //hz = 4uS. // Prescaler puede valer,4,6 // Postscaler puede valer. // 6000uS = [PR2+] x 4 x 6 x // PR2 =[T/(Tcm x Preescaler x Postscaler)]- // PR2 =[6000uS/(4uS x 6 x )]- = 249 (en C 249) // CCP2 en modo PWM (Salida por RC) motor izquierdo. // CCP en modo PWM (Salida por RC2) motor derecho. // Defines Puerto C como SALIDA de datos. while(true){ RC5=0; //MOTOR DERECHO HACIA ATRAS RC0=; TH = 000; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 900; set_pwm2_duty(th); //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 800; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 700; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 600; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 500; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 400; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 300; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 200; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 00; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 0; set_pwm2_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); 24

25 7.3.2 VELOCIDAD MOTOR DERECHO //*****************************************Laberinto ******************************* //*****************************************Directivas de preprocesado******************* #include <6F876A.h> #device adc=0 // Conversor Analógico Digital de 0 bit el PIC 6F876A puede trabajar con 8 //o 0 bit de resolución. #FUSES XT,NOWDT #use delay(clock= ) #include <LCD.c> // Incluimos el driver LCD.c que contiene las funciones de control del LCD. #BYTE TRISB = 0x86 // TRISB en 86h. #BYTE portb = 0x06 // PORTB en 06h. #BYTE TRISC = 0x87 // TRISC en 87h. #BYTE portc = 0x07 // PORTC EN 07h. #BYTE TRISA = 0x85 // TRISA en 85h. #BYTE porta = 0x05 // PORTA en 05h. #BIT RB3 = 0x06.3 #BIT RC0 = 0x07.0 #BIT RC = 0x07. #BIT RC2 = 0x07.2 // DEFINO LA PATILLA 3 DEL PORTB COMO RB3. // DEFINO LA PATILLA 0 DEL PORTC COMO RC0. // DEFINO LA PATILLA DEL PORTC COMO RC. // DEFINO LA PATILLA 2 DEL PORTC COMO RC2. 25

26 #BIT RC3 = 0x07.3 #BIT RC4 = 0x07.4 #BIT RC5 = 0x07.5 #BIT RC6 = 0x07.6 #BIT RC7 = 0x07.7 #BIT RA2 = 0x05.2 #BIT RA4 = 0x05.4 #BIT RA5 = 0x05.5 // DEFINO LA PATILLA 3 DEL PORTC COMO RC3. // DEFINO LA PATILLA 4 DEL PORTC COMO RC4. // DEFINO LA PATILLA 5 DEL PORTC COMO RC5. // DEFINO LA PATILLA 6 DEL PORTC COMO RC6. // DEFINO LA PATILLA 7 DEL PORTC COMO RC7. // DEFINO LA PATILLA 2 DEL PORTA COMO RA2. // DEFINO LA PATILLA 4 DEL PORTA COMO RA4. // DEFINO LA PATILLA 5 DEL PORTA COMO RA5. //**************************************Declaración funciones*************************** int6 TH =65535; //*******************************PROGRAMA PRINCIPAL****************************** void main(){ TRISB = 0B ; TRISC = 0B ; TRISA = 0B0000; //portb = 0B ; // Defines Puerto B como SALIDA de datos. // Defines Puerto C como SALIDA de datos. // Defines Puerto A como ENTRADA de datos. // Reseteamos el Puerto B RA2 = 0; RA4 = 0; RB3 = 0; RC0 = 0; RC5 = 0; RC6 = 0; RC7 = 0; setup_timer_2(t2_div_by_6,249,); // setup_timer(prescaler,pr2,postscaler) // Configuración timer2. Si el Periodo = 6mS ----> T = //6000uS // T = [PR2+] x Tcm x Postscaler x Prescaler // PR2 puede valer de 0 a 255. // Tcm es el tiempo de Ciclo Maquina. Tcm = 4/Fosc = 4/ //hz = 4uS. // Prescaler puede valer,4,6 // Postscaler puede valer. // 6000uS = [PR2+] x 4 x 6 x // PR2 =[T/(Tcm x Preescaler x Postscaler)]- // PR2 =[6000uS/(4uS x 6 x )]- = 249 (en C 249) setup_ccp2(ccp_pwm); // CCP2 en modo PWM (Salida por RC) motor izquierdo. setup_ccp(ccp_pwm); // CCP en modo PWM (Salida por RC2) motor derecho. TH=0; set_pwm_duty(th); set_pwm2_duty(th); TRISC = 0B ; // Defines Puerto C como SALIDA de datos. 26

27 while(true){ RA4=0; //MOTOR DERECHO HACIA ATRAS RB3=; TH = 000; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 900; set_pwm_duty(th); //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 800; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 700; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 600; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 500; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 400; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 300; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 200; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 00; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); TH = 0; set_pwm_duty(th); // ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR IZQ delay_ms(500); 27

28 7.3.3 CONTROL DE GIRO DEL MOTOR DERECHO: #include <6F876A.h> #fuses XT,NOWDT #use delay( clock = ) #include <LCD.c> // Incluye el fichero 6F876 al programa tiene que estar en la misma //carpeta del programa define funciones, patillas y registros. // Define la palabra de configuarción del microcontrolador PIC // Trabaja con un reloj tipo XT y no utiliza perro guardián Wathdog // Define la frecuencia del reloj de 4 MHz // Incluimos el driver kbd.c que contiene las funciones de control del //display. #BYTE TRISB = 0x86 // TRISB en 86h. #BYTE portb = 0x06 // PORTB en 06h. #BYTE TRISC = 0x87 // TRISC en 87h. #BYTE portc = 0x07 // PORTC EN 07h. #BYTE TRISA = 0x85 // TRISA en 85h. #BYTE porta = 0x05 // PORTA en 05h. #BIT RB3 = 0x06.3 #BIT RC0 = 0x07.0 #BIT RC = 0x07. #BIT RC2 = 0x07.2 #BIT RC3 = 0x07.3 #BIT RC4 = 0x07.4 #BIT RC5 = 0x07.5 #BIT RC6 = 0x07.6 #BIT RC7 = 0x07.7 #BIT RA4 = 0x05.4 // DEFINO LA PATILLA 3 DEL PORTB COMO RB3. // DEFINO LA PATILLA 0 DEL PORTC COMO RB3. // DEFINO LA PATILLA DEL PORTC COMO RB3. // DEFINO LA PATILLA 2 DEL PORTC COMO RB3. // DEFINO LA PATILLA 3 DEL PORTC COMO RB3. // DEFINO LA PATILLA 4 DEL PORTC COMO RB3. // DEFINO LA PATILLA 5 DEL PORTC COMO RB3. // DEFINO LA PATILLA 6 DEL PORTC COMO RB3. // DEFINO LA PATILLA 7 DEL PORTC COMO RB3. // DEFINO LA PATILLA 4 DEL PORTA COMO RB3. 28

29 void main(){ TRISB = 0B ; TRISC = 0B ; TRISA = 0B0000; portb = 0B ; RA4 = 0; RB3 = 0; RC0 = 0; RC = 0; RC2 = 0; RC5 = 0; RC6 = 0; RC7 = 0; // programa principal // Defines Puerto B como SALIDA de datos. // Defines Puerto C como SALIDA de datos. // Defines Puerto A como ENTRADA de datos. // Reseteamos el Puerto B //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL while(){ RA4=0; //MOTOR PARADO RB3=0; delay_ms(00); RA4=0; //MOTOR HACIA DELANTE RB3=; delay_ms(00); RA4=; //MOTOR HACIA ATRAS RB3=0; delay_ms(00); RA4=; //MOTOR PARADO RB3=; delay_ms(00); 29

30 7.3.4 CONTROL DE GIRO DEL MOTOR IZQUIERDO: #include <6F876A.h> #fuses XT,NOWDT #use delay( clock = ) #include <LCD.c> // Incluye el fichero 6F876 al programa tiene que estar en la misma //carpeta del programa define funciones, patillas y registros. // Define la palabra de configuarción del microcontrolador PIC // Trabaja con un reloj tipo XT y no utiliza perro guardián Wathdog // Define la frecuencia del reloj de 4 MHz // Incluimos el driver kbd.c que contiene las funciones de control del //display. #BYTE TRISB = 0x86 // TRISB en 86h. #BYTE portb = 0x06 // PORTB en 06h. #BYTE TRISC = 0x87 // TRISC en 87h. #BYTE portc = 0x07 // PORTC EN 07h. #BYTE TRISA = 0x85 // TRISA en 85h. #BYTE porta = 0x05 // PORTA en 05h. #BIT RB3 = 0x06.3 #BIT RC0 = 0x07.0 #BIT RC = 0x07. #BIT RC2 = 0x07.2 #BIT RC3 = 0x07.3 #BIT RC4 = 0x07.4 #BIT RC5 = 0x07.5 #BIT RC6 = 0x07.6 #BIT RC7 = 0x07.7 #BIT RA4 = 0x05.4 // DEFINO LA PATILLA 3 DEL PORTB COMO RB3. // DEFINO LA PATILLA 0 DEL PORTC COMO RC0. // DEFINO LA PATILLA DEL PORTC COMO RC. // DEFINO LA PATILLA 2 DEL PORTC COMO RB3. // DEFINO LA PATILLA 3 DEL PORTC COMO RC3. // DEFINO LA PATILLA 4 DEL PORTC COMO RC4. // DEFINO LA PATILLA 5 DEL PORTC COMO RC5. // DEFINO LA PATILLA 6 DEL PORTC COMO RC6. // DEFINO LA PATILLA 7 DEL PORTC COMO RC7. // DEFINO LA PATILLA 4 DEL PORTA COMO RA4. 30

31 void main(){ TRISB = 0B ; TRISC = 0B ; TRISA = 0B0000; portb = 0B ; RA4 = 0; RB3 = 0; RC0 = 0; RC = 0; RC2 = 0; RC5 = 0; RC6 = 0; RC7 = 0; // programa principal // Defines Puerto B como SALIDA de datos. // Defines Puerto C como SALIDA de datos. // Defines Puerto A como ENTRADA de datos. // Reseteamos el Puerto B //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL //RESETEO DE PATILLAS DE FORMA INDIVDUAL while(){ RC5=0; //MOTOR PARADO RC0=0; delay_ms(00); RC5=0; //MOTOR HACIA DELANTE RC0=; delay_ms(00); RC5=; //MOTOR HACIA ATRAS RC0=0; delay_ms(00); RC5=; //MOTOR PARADO RC0=; delay_ms(00); 3

32 8 SISTEMA DE CONTROL: El sistema de control es el cerebro del robot esta parte del robot se encarga de:. Guardar el programa. 2. Activar las distintas partes de robot. 3. Recibir la información del sistema sensorial y del exterior 4. Analizar y administrar dicha información, para que el robot funcione con respecto al programa. Las partes que forman el sistema de control son: 8.LA PANTALLA LCD HDD44700, tiene dos líneas de 6 caracteres cada una, la pantalla LCD nos ha servido para representar la distancia a la que se encuentra el robot. La conexión se ha realizado por el puerto B, porque se ha utilizado una librería genérica que tenia esta configuración. 32

33 8..PATILLAJE DEL LDC 8..2DIAGRAMA Y PROGRAMA DE PRUEBA DEL LCD. #include <6F876A.h> #fuses XT,NOWDT #use delay( clock = ) #include <LCD.c> // Incluye el fichero 6F876 al programa tiene que estar en la misma //carpeta del programa // define funciones, patillas y registros. // Define la palabra de configuarción del microcontrolador PIC // Trabaja con un reloj tipo XT y no utiliza perro guardián Wathdog // Define la frecuencia del reloj de 4 MHz // Incluimos el driver kbd.c que contiene las funciones de control del //display. #BYTE TRISB = 0x86 // TRISB en 86h. #BYTE portb = 0x06 // PORTB en 06h. 33

34 #BYTE TRISC = 0x87 // TRISC en 87h. #BYTE portc = 0x07 // PORTC EN 07h. #BYTE TRISA = 0x85 // TRISA en 85h. #BYTE porta = 0x05 // PORTA en 05h. void main(){ TRISB = 0B ; portb = 0B ; // Defines Puerto B como SALIDA de datos. // Reseteamos el Puerto B lcd_init(); // Inicializamos el LCD. while(){ lcd_gotoxy(,); // Posicionamos el Cursor en la posición, línea. printf(lcd_putc, "hello word"); // representación en display. 8.2RECEPTOR DE DATOS CEBEK C El receptor de datos es un circuito encargado de recibir datos desde el exterior, este circuito demodula una señal AM obteniendo los datos digitales comprendidos entre 20 Hz < y < 4 KHz, en un futuro servirá para realizar un coche teledirigido. 8.3 PIC. Es un microcontrolador PIC6F876A, se encarga de almacenar el programa que controla el robo, recibir y analizar la información de los sensores y con respecto a ellas enviar señales que controlen la velocidad y el giro de los motores, representar dicha información en el display y recibir información desde la tarjeta de radio frecuencia. El trabaja a una frecuencia de MHz y ejecuta una instrucción en 4uS. 8.4 OTROS. J6M.JM. Son conexiones I2C, estas conexiones se realizan a través de dos puertos RC3 y RC4, y son conexiones series de datos. INICIO. El pulsador inicio sirve para dar la orden al μc de que puede comenzar el programa. RESET. Este pulsador, sirve para volver a iniciar el programa, en caso de que el robot se quede bloqueado en alguna pared, y queremos reiniciar el programa activamos el pulsador. XM. Xm es el cristal de cuarzo, y junto con los condensadores sirve para dar la señal de reloj de MHz al μc. 34

35 JM, J3M, J5M. Son los jumper donde se conectan las fajas por las que se envían y reciben las señales de las otras partes de robot. J2M. J2M, sirve para seleccionar un programa, en caso de que tengamos mas de uno. J3M. J3M, sirve para poner el robot en modo teledirigido o para poner el robot dirigido por el programa. 9 BREVE EXPLICACIÓN DEL PROGRAMA PRINCIPAL. El programa principal consiste en analizar la información, que recibe de los sensores. Si el robot se encuentra entre 5,5cm y 7cm la PWM de los motores es la misma (TH=000). 35

36 Si el robot se aleja 2cm más de la pared, la PWM del motor derecho disminuye muy poco, pero en cambio si desplaza más de 2cm la PWM de dicho motor se reduce bastante. El caso contrario, si el robot está 2cm por debajo del rango inicial, la PWM del motor izquierdo se reduce muy poco, pero si es superior a dos 2cm la PWM disminuye más bruscamente. 36

37 En caso de que el robot de con una curva a derechas, la PWM del motor derecho casi es nula (TH=50), por el contrario en caso de un giro a la izquierda el motor izquierdo se invierte con una PWM muy baja (TH=00). 37

38 9.ESQUEMA ELECTRICO DE SIMULACIÓN: 38

39 9.2DIAGRAMA Y PROGRAMA PRINCIPAL. 39

40 40

41 4

42 42

43 //*******************************Laberinto *************************** //************************Directivas de preprocesado********************** #include <6F876A.h> #device adc=0 #FUSES XT,NOWDT #use delay(clock= ) // Conversor Analógico Digital de 0 bit el PIC 6F876A //puede trabajar con 8 o 0 bit de resolución. #include <LCD.c> // Incluimos el driver LCD.c que contiene las funciones //de control del LCD. #include <driver_gp2d2.c> #BYTE TRISB = 0x86 // TRISB en 86h. #BYTE portb = 0x06 // PORTB en 06h. #BYTE TRISC = 0x87 // TRISC en 87h. #BYTE portc = 0x07 // PORTC EN 07h. #BYTE TRISA = 0x85 // TRISA en 85h. #BYTE porta = 0x05 // PORTA en 05h. #BIT RB3 = 0x06.3 #BIT RC0 = 0x07.0 #BIT RC = 0x07. #BIT RC2 = 0x07.2 #BIT RC3 = 0x07.3 #BIT RC4 = 0x07.4 #BIT RC5 = 0x07.5 #BIT RC6 = 0x07.6 #BIT RC7 = 0x07.7 #BIT RA2 = 0x05.2 #BIT RA4 = 0x05.4 #BIT RA5 = 0x05.5 // DEFINO LA PATILLA 3 DEL PORTB COMO RB3: //MOTOR. // DEFINO LA PATILLA 0 DEL PORTC COMO RC0: //MOTOR. // DEFINO LA PATILLA DEL PORTC COMO RC: //VSENS_A. // DEFINO LA PATILLA 2 DEL PORTC COMO RC2: //VSENS_B. // DEFINO LA PATILLA 3 DEL PORTC COMO RC3: //I2C. // DEFINO LA PATILLA 4 DEL PORTC COMO RC4: //I2C. // DEFINO LA PATILLA 5 DEL PORTC COMO RC5: //MOTOR. // DEFINO LA PATILLA 6 DEL PORTC COMO RC6: //BL. // DEFINO LA PATILLA 7 DEL PORTC COMO RC7: //CONTROL MANDO PROGRAMA. // DEFINO LA PATILLA 2 DEL PORTA COMO RA2: //INICIO. // DEFINO LA PATILLA 4 DEL PORTA COMO RA4: //MOTOR. // DEFINO LA PATILLA 5 DEL PORTA COMO RA5: //CONTROL DE PROGRAMAS. #define DisFron 2 #define DisDech 2 #define DisIzqu 2 // CREO UNA CONSTANTE LLAMADA DISFRON //PARA HACER LA COMPARATIVA. // CREO UNA CONSTANTE LLAMADA DISDECH //PARA HACER LA COMPARATIVA. // CREO UNA CONSTANTE LLAMADA DISIZQU //PARA HACER LA COMPARATIVA. //******************************Declaración de funciones************* float Sensor_Derecho(void); float Sensor_Izquierdo(void); float Sensor_Frontal(void); void Motor_Derecho(void); void Motor_Derecho(void); void Motor_Derecho2(void); 43

44 void Motor_Derecho3(void); void Motor_Derecho4(void); void Motor_Derecho5(void); void Motor_Derecho6(void); void Motor_Derecho7(void); void Motor_Derecho8(void); void Motor_Derecho9(void); void Motor_Derecho0(void); void Motor_Derecho(void); void Motor_Derecho2(void); void Motor_Derecho3(void); void Motor_Derecho4(void); void Motor_Derecho5(void); void Motor_Derecho6(void); void Motor_Derecho7(void); void Motor_Derecho8(void); void Motor_Derecho9(void); void Motor_Derecho_Parado(void); void Motor_Izquierdo(void); void Motor_Izquierdo(void); void Motor_Izquierdo2(void); void Motor_Izquierdo3(void); void Motor_Izquierdo4(void); void Motor_Izquierdo5(void); void Motor_Izquierdo6(void); void Motor_Izquierdo7(void); void Motor_Izquierdo8(void); void Motor_Izquierdo9(void); void Motor_Izquierdo0(void); void Motor_Izquierdo(void); void Motor_Izquierdo2(void); void Motor_Izquierdo3(void); void Motor_Izquierdo4(void); void Motor_Izquierdo5(void); void Motor_Izquierdo6(void); void Motor_Izquierdo7(void); void Motor_Izquierdo8(void); void Motor_Izquierdo9(void); void Motor_Izquierdo_Atras(void); int6 TH =65535; //*********************PROGRAMA PRINCIPAL********************* void main(){ int giro= 0; float sf=0; float sd=0; setup_adc(adc_clock_internal); setup_adc_ports(0); TRISB = 0B ; // Fuente de reloj RC interno. // Seleccionamos el Puerto A como //entradas Analógicas. Mirar //ADCON. // Defines Puerto B como SALIDA //de datos. 44

45 TRISC = 0B ; TRISA = 0B0000; portb = 0B ; RA2 = 0; RA4 = 0; RB3 = 0; RC0 = 0; RC5 = 0; RC6 = 0; RC7 = 0; // Defines Puerto C como SALIDA //de //datos. // Defines Puerto A como //ENTRADA de //datos. // Reseteamos el Puerto B //Reseteo de patillas individuales //Reseteo de patillas individuales //Reseteo de patillas individuales //Reseteo de patillas individuales //Reseteo de patillas individuales //Reseteo de patillas individuales //Reseteo de patillas individuales lcd_init(); setup_timer_2(t2_div_by_6,249,); setup_ccp2(ccp_pwm); setup_ccp(ccp_pwm); set_pwm_duty(th); set_pwm2_duty(th); TRISC = 0B ; // Inicializamos el LCD. // setup_timer(prescaler,pr2,postscaler) // Configuración timer2. Si el Periodo = 6mS ----> //T = 6000uS // T = [PR2+] x Tcm x Postscaler x Prescaler // PR2 puede valer de 0 a 255. // Tcm es el tiempo de Ciclo Maquina. Tcm = //4/Fosc = 4/ hz = 4uS. // Prescaler puede valer,4,6 // Postscaler puede valer. // 6000uS = [PR2+] x 4 x 6 x // PR2 =[T/(Tcm x Preescaler x Postscaler)]- // PR2 =[6000uS/(4uS x 6 x )]- = 249 (en C //249) // CCP2 en modo PWM (Salida por RC) motor //izquierdo. // CCP en modo PWM (Salida por RC2) motor //derecho. // Defines Puerto C como SALIDA de datos. do{ sf=sensor_frontal(); sd=sensor_derecho(); lcd_gotoxy(,2); printf(lcd_putc, "Sd= %0.f cm",sd); lcd_gotoxy(,); printf(lcd_putc, "Sf= %0.f cm",sf); // Saltamos de línea y escribimos en el LCD //"VOLTAJE =" y 4 dígitos de "P" // en formato truncado de 4 dígitos con 3 //decimales y el carácter "V". //colocamos en el primer carácter de la primera línea //representamos por pantalla while(ra2==); while (){ // Bucle infinito. 45

46 sf=sensor_frontal(); sd=sensor_derecho(); lcd_gotoxy(,2); printf(lcd_putc, "Sd= %0.f cm",sd); // Saltamos de línea y escribimos en el LCD //"VOLTAJE =" y 4 dígitos de "P" // en formato truncado de 4 dígitos con 3 decimales y //el carácter "V". lcd_gotoxy(,); printf(lcd_putc, "Sf= %0.f cm",sf); //colocamos en el primer carácter de la primera línea // Saltamos de línea y escribimos en el LCD //"VOLTAJE =" y 4 dígitos de "P" // en formato truncado de 4 dígitos con 3 decimales y // carácter "V". if(giro== sf<20){ giro=; Motor_Izquierdo_Atras(); Motor_Derecho(); if(sf>=8) giro=0; //programa de giros a la izquierda. else{ if(sd>5.5&&sd<=7){ Motor_Derecho(); Motor_Izquierdo(); //***************************control motor derecho************************************* if(sd>7&&sd<=7.3){ Motor_Derecho(); Motor_Izquierdo(); if(sd>7.3&&sd<=7.6){ Motor_Derecho2(); Motor_Izquierdo(); if(sd>7.6&&sd<=7.9){ Motor_Derecho3(); Motor_Izquierdo(); if(sd>7.9&&sd<=8.2){ Motor_Derecho4(); Motor_Izquierdo(); if(sd>8.2&&sd<=8.5){ Motor_Derecho5(); Motor_Izquierdo(); if(sd>8.5&&sd<=8.8){ Motor_Derecho6(); //controles en recta //controles en recta 46

47 Motor_Izquierdo(); if(sd>8.8&&sd<=9.){ Motor_Derecho7(); Motor_Izquierdo(); if(sd>9.&&sd<=9.4){ Motor_Derecho8(); Motor_Izquierdo(); if(sd>9.4&&sd<=9.7){ Motor_Derecho9(); Motor_Izquierdo(); if(sd>9.7&&sd<=20){ Motor_Derecho0(); Motor_Izquierdo(); if(sd>20&&sd<=20.3){ Motor_Derecho(); Motor_Izquierdo(); if(sd>20.3&&sd<=20.6){ Motor_Derecho2(); Motor_Izquierdo(); if(sd>20.6&&sd<=20.9){ Motor_Derecho3(); Motor_Izquierdo(); /* if(sd>20.9&&sd<=2.2){ Motor_Derecho4(); Motor_Izquierdo(); if(sd>2.2&&sd<=2.5){ Motor_Derecho5(); Motor_Izquierdo(); if(sd>2.5&&sd<=22.5){ Motor_Derecho6(); Motor_Izquierdo(); if(sd>22.5&&sd<=23){ Motor_Derecho7(); Motor_Izquierdo(); if(sd>23&&sd<=24.5){ Motor_Derecho8(); Motor_Izquierdo(); if(sd>24.5&&sd<=25){ Motor_Derecho9(); Motor_Izquierdo(); */ if(sd>25){ //cuando se queda sin pared, giros a la derecha Motor_Derecho_Parado(); Motor_Izquierdo(); 47

48 //***************************control motor izq************************************ if(sd<5.5&&sd>=5.2){ Motor_Izquierdo(); Motor_Derecho(); if(sd<5.2&&sd>=4.9){ Motor_Izquierdo2(); Motor_Derecho(); //controles en recta if(sd<4.9&&sd>=4.6){ Motor_Izquierdo3(); Motor_Derecho(); if(sd<4.6&&sd>=4){ Motor_Izquierdo4(); Motor_Derecho(); if(sd<4&&sd>=3.7){ Motor_Izquierdo5(); Motor_Derecho(); if(sd<3.7&&sd>=3.4){ Motor_Izquierdo6(); Motor_Derecho(); if(sd<3.4&&sd>=3.){ Motor_Izquierdo7(); Motor_Derecho(); if(sd<3.&&sd>=2.8){ Motor_Izquierdo8(); Motor_Derecho(); if(sd<2.8&&sd>=2.5){ Motor_Izquierdo9(); Motor_Derecho(); if(sd<2.5&&sd>=2.2){ Motor_Izquierdo0(); Motor_Derecho(); if(sd<2.2&&sd>=.9){ Motor_Izquierdo(); Motor_Derecho(); if(sd<.9&&sd>=.6){ Motor_Izquierdo2(); Motor_Derecho(); if(sd<.6&&sd>=){ Motor_Izquierdo3(); Motor_Derecho(); if(sd<&&sd>=0.7){ 48

49 Motor_Izquierdo4(); Motor_Derecho(); if(sd<0.7&&sd>=0.4){ Motor_Izquierdo5(); Motor_Derecho(); if(sd<0.4&&sd>=9.6){ Motor_Izquierdo6(); Motor_Derecho(); if(sd<9.6&&sd>=9){ Motor_Izquierdo7(); Motor_Derecho(); if(sd<9&&sd>=8.5){ Motor_Izquierdo8(); Motor_Derecho(); if(sd<8.5&&sd>=8){ Motor_Izquierdo9(); Motor_Derecho(); //******************************* Función Sensor_Derecho ******************************* float Sensor_Derecho(){ int6 q; // variable de lectura de puerto float dech, //variable de resultado de la conversión de tensión a centímetros v; // variable donde guardamos la tensión recogida de la conversión //analógica set_adc_channel(3); delay_us(20); q = read_adc(); v = (5.0 * q) / 024.0; // Habilitación canal 3 "AN3" sensor derecho // Retardo de 20uS necesaria para respetar el tiempo de Adquisición //Tad. // Lectura canal 0 "AN3" // conversión a tensión del código digital "q". dech=gp2d2_v_d(v); return(dech); //driver del Gp2d2 para conversión de voltios a cemtimetos, //guardado en variable dech. // el resultado que devuelvo llamada. //******************************* Función Sensor_Izquierdo ****************************** float Sensor_Izquierdo(){ int6 q; // variable de lectura de puerto float izqu, // variable de resultado de la conversión de tensión a centímetros v; // variable donde guardamos la tensión recogida de la conversión //analógica set_adc_channel(0); delay_us(20); q = read_adc(); // Habilitación canal 0 "AN0" sensor izquierdo // Retardo de 20uS necesaria para respetar el tiempo de Adquisición //Tad. // Lectura canal 0 "AN0" 49

50 v = (5.0 * q) / 024.0; // conversión a tensión del código digital "q". izqu=gp2d2_v_d(v); return(izqu); //driver del Gp2d2 para conversión de voltios a cemtimetos, //guardado en variable izqu. // el resultado que devuelvo llamada. //******************************* Función Sensor_Frontal*************************** float Sensor_Frontal(){ int6 q; // variable de lectura de puerto float fron, // variable de resultado de la conversión de tensión a centímetros v; // variable donde guardamos la tensión recogida de la conversión //analógica set_adc_channel(); delay_us(20); q = read_adc(); v = (5.0 * q) / 024.0; // Habilitación canal "AN" sensor derecho // Retardo de 20uS necesaria para respetar el tiempo de Adquisición //Tad. // Lectura canal 0 "AN" // conversión a tensión del código digital "q". fron=gp2d2_v_d(v); return(fron); //driver del Gp2d2 para conversión de voltios a cemtimetos, //guardado en variable fron. // el resultado que devuelvo llamada. //********************************************************************************* //*********************************CONTROL DEMOTORES*************************** //*********************************************************************************** //*********************************Controles del Motor Derecho************************** //***********************************Función Motor_Derecho**************************** void Motor_Derecho(){ RA4=0; RB3=; TH = 000; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_ Derecho ************************** void Motor_Derecho(){ RA4=0; RB3=; TH = 990; //MOTOR DERECHO HACIA ATRAS 50

51 set_pwm2_duty(th); //ATACA A LA PATILLA RC, Y CONTROLAMOS EL //MOTOR DERECHO(ojo tengo que cambiar los cables de la //placa) //***********************************Función Motor_ Derecho 2************************* void Motor_Derecho2(){ RA4=0; RB3=; TH = 970; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho 3************************** void Motor_Derecho3(){ RA4=0; RB3=; TH = 950; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_ Derecho 4************************* void Motor_Derecho4(){ RA4=0; RB3=; TH = 930; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho5************************* void Motor_Derecho5(){ RA4=0; RB3=; TH = 90; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho6*********************** void Motor_Derecho6(){ RA4=0; RB3=; TH = 890; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO 5

52 //***********************************Función Motor_ Derecho 7******************** void Motor_Derecho7(){ RA4=0; RB3=; TH = 800; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor8******************************* void Motor_Derecho8(){ RA4=0; RB3=; TH = 770; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO(ojo tengo que cambiar los cables de la placa) //***********************************Función Motor9******************************* void Motor_Derecho9(){ RA4=0; RB3=; TH = 740; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho0************************ void Motor_Derecho0(){ RA4=0; RB3=; TH = 70; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho************************** void Motor_Derecho(){ RA4=0; RB3=; TH = 680; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO 52

53 //***********************************Función Motor_Derecho2*************************** void Motor_Derecho2(){ RA4=0; RB3=; TH = 650; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho3*************************** void Motor_Derecho3(){ RA4=0; RB3=; TH = 620; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho4*************************** void Motor_Derecho4(){ RA4=0; RB3=; TH = 590; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho5*************************** void Motor_Derecho5(){ RA4=0; RB3=; TH = 500; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho6************************** void Motor_Derecho6(){ RA4=0; RB3=; TH = 450; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO 53

54 //***********************************Función Motor_Derecho7*************************** void Motor_Derecho7(){ RA4=0; RB3=; TH = 400; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho8************************* void Motor_Derecho8(){ RA4=0; RB3=; TH = 350; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO //***********************************Función Motor_Derecho9************************* void Motor_Derecho9(){ RA4=0; RB3=; TH = 300; set_pwm2_duty(th); //MOTOR DERECHO HACIA ATRAS //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO // //***********************************FuncionMotor_Derecho_Parado*********************** void Motor_Derecho_Parado(){ RA4=0; RB3=; TH = 50; set_pwm2_duty(th); //MOTOR DERECHO HACIA DELANTE //ATACA A LA PATILLA RC, Y CONTROLAMOS EL MOTOR //DERECHO(ojo tengo que cambiar los cables de la placa) //*********************************Controles del Motor Izquierdo*************************** //*********************************Función Motor_Izquierdo***************************** void Motor_Izquierdo(){ RC5=; RC0=0; TH = 000; //MOTOR HACIA DELANTE 54