UC-GRUPO DE ESTUDIO-INTELIGENCIA ARTIFICIAL / Mbed 2 deprecated RobLaberinto001

ROB LAB

Dependencies:   HCSR04 mbed

main.cpp@0:9be8543766b6, 2017-07-27 (annotated)

Committer:
jhony
Date:
Thu Jul 27 20:09:52 2017 +0000
Revision:
0:9be8543766b6
ASD

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jhony 0:9be8543766b6 1 #include "mbed.h"
jhony 0:9be8543766b6 2 #include "hcsr04.h"
jhony 0:9be8543766b6 3
jhony 0:9be8543766b6 4 //Serial pc(USBTX, USBRX);
jhony 0:9be8543766b6 5 //Sensores de Distancia
jhony 0:9be8543766b6 6 HCSR04 usensorA(p10,p11);
jhony 0:9be8543766b6 7 HCSR04 usensorD(p6,p5);
jhony 0:9be8543766b6 8 HCSR04 usensorI(p20,p19);
jhony 0:9be8543766b6 9 //Motores
jhony 0:9be8543766b6 10 DigitalOut STBY(p15);//p7
jhony 0:9be8543766b6 11 PwmOut PWMA(p26);//p21
jhony 0:9be8543766b6 12 DigitalOut AIN1(p30);//p5
jhony 0:9be8543766b6 13 DigitalOut AIN2(p29);//p6
jhony 0:9be8543766b6 14 PwmOut PWMB(p25);//p22
jhony 0:9be8543766b6 15 DigitalOut BIN1(p14);//p8
jhony 0:9be8543766b6 16 DigitalOut BIN2(p13);//p9
jhony 0:9be8543766b6 17 //Interrupcion
jhony 0:9be8543766b6 18
jhony 0:9be8543766b6 19 unsigned int distA, distD, distI;
jhony 0:9be8543766b6 20
jhony 0:9be8543766b6 21 void girarIzquierda()
jhony 0:9be8543766b6 22 {
jhony 0:9be8543766b6 23 BIN1 = 1;
jhony 0:9be8543766b6 24 BIN2 = 0;
jhony 0:9be8543766b6 25 PWMB = 2;
jhony 0:9be8543766b6 26 AIN1 = 0;
jhony 0:9be8543766b6 27 AIN2 = 1;
jhony 0:9be8543766b6 28 PWMA = 2;
jhony 0:9be8543766b6 29 }
jhony 0:9be8543766b6 30
jhony 0:9be8543766b6 31 void girarDerecha()
jhony 0:9be8543766b6 32 {
jhony 0:9be8543766b6 33 BIN1 = 0;
jhony 0:9be8543766b6 34 BIN2 = 1;
jhony 0:9be8543766b6 35 PWMB = 2;
jhony 0:9be8543766b6 36 AIN1 = 1;
jhony 0:9be8543766b6 37 AIN2 = 0;
jhony 0:9be8543766b6 38 PWMA = 2;
jhony 0:9be8543766b6 39 }
jhony 0:9be8543766b6 40
jhony 0:9be8543766b6 41 void adelante()
jhony 0:9be8543766b6 42 {
jhony 0:9be8543766b6 43 BIN1 = 1;
jhony 0:9be8543766b6 44 BIN2 = 0;
jhony 0:9be8543766b6 45 PWMB = 2;
jhony 0:9be8543766b6 46 AIN1 = 1;
jhony 0:9be8543766b6 47 AIN2 = 0;
jhony 0:9be8543766b6 48 PWMA = 2;
jhony 0:9be8543766b6 49 }
jhony 0:9be8543766b6 50
jhony 0:9be8543766b6 51 void verificarSalida()
jhony 0:9be8543766b6 52 {
jhony 0:9be8543766b6 53 if(distA < distD && distA < distI)
jhony 0:9be8543766b6 54 {
jhony 0:9be8543766b6 55 if(distD > distI)
jhony 0:9be8543766b6 56 {
jhony 0:9be8543766b6 57 girarDerecha();
jhony 0:9be8543766b6 58 }
jhony 0:9be8543766b6 59 else
jhony 0:9be8543766b6 60 if(distI>distD)
jhony 0:9be8543766b6 61 {
jhony 0:9be8543766b6 62 girarIzquierda();
jhony 0:9be8543766b6 63 }
jhony 0:9be8543766b6 64 }
jhony 0:9be8543766b6 65 else
jhony 0:9be8543766b6 66 {
jhony 0:9be8543766b6 67 adelante();
jhony 0:9be8543766b6 68 }
jhony 0:9be8543766b6 69 }
jhony 0:9be8543766b6 70
jhony 0:9be8543766b6 71 int main()
jhony 0:9be8543766b6 72 {
jhony 0:9be8543766b6 73 //pc.baud(115200);
jhony 0:9be8543766b6 74 //pc.printf("Hello World from FRDM-K64F board.\n");
jhony 0:9be8543766b6 75
jhony 0:9be8543766b6 76 while(1)
jhony 0:9be8543766b6 77 {
jhony 0:9be8543766b6 78 usensorI.start();
jhony 0:9be8543766b6 79 wait_ms(500);
jhony 0:9be8543766b6 80 distI=usensorI.get_dist_cm();
jhony 0:9be8543766b6 81 //pc.printf("IZQUIERDA cm:%ld",distI );
jhony 0:9be8543766b6 82
jhony 0:9be8543766b6 83 usensorA.start();
jhony 0:9be8543766b6 84 wait_ms(500);
jhony 0:9be8543766b6 85 distA=usensorA.get_dist_cm();
jhony 0:9be8543766b6 86 //pc.printf("ADELANTE cm:%ld",distA );
jhony 0:9be8543766b6 87
jhony 0:9be8543766b6 88 usensorD.start();
jhony 0:9be8543766b6 89 wait_ms(500);
jhony 0:9be8543766b6 90 distD=usensorD.get_dist_cm();
jhony 0:9be8543766b6 91 //pc.printf("DERECHA cm:%ld",distD );
jhony 0:9be8543766b6 92
jhony 0:9be8543766b6 93 if(distA > 20 && distD>20 && distI>20)
jhony 0:9be8543766b6 94 {
jhony 0:9be8543766b6 95 adelante();
jhony 0:9be8543766b6 96 }
jhony 0:9be8543766b6 97 if(distA > 10 && distD<10 && distI<10)
jhony 0:9be8543766b6 98 {
jhony 0:9be8543766b6 99 adelante();
jhony 0:9be8543766b6 100 }
jhony 0:9be8543766b6 101 if(distD>10 && distI<10)
jhony 0:9be8543766b6 102 {
jhony 0:9be8543766b6 103 girarDerecha();
jhony 0:9be8543766b6 104 }
jhony 0:9be8543766b6 105 if(distI > 10 && distD<10 && distA<10)
jhony 0:9be8543766b6 106 {
jhony 0:9be8543766b6 107 girarIzquierda();
jhony 0:9be8543766b6 108 }
jhony 0:9be8543766b6 109 if(distA < 10 && distD<10 && distI<10)
jhony 0:9be8543766b6 110 {
jhony 0:9be8543766b6 111 verificarSalida();
jhony 0:9be8543766b6 112 }
jhony 0:9be8543766b6 113
jhony 0:9be8543766b6 114 }
jhony 0:9be8543766b6 115 }
jhony 0:9be8543766b6 116