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

laberinto

Dependencies:   HCSR04 Motordriver mbed

main.cpp@0:7e3e917c6fa8, 2017-08-12 (annotated)

Committer:
Janice_Rafael
Date:
Sat Aug 12 16:17:11 2017 +0000
Revision:
0:7e3e917c6fa8
micromouse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Janice_Rafael 0:7e3e917c6fa8 1 #include "mbed.h"
Janice_Rafael 0:7e3e917c6fa8 2 #include "hcsr04.h"
Janice_Rafael 0:7e3e917c6fa8 3 #include "motordriver.h"
Janice_Rafael 0:7e3e917c6fa8 4
Janice_Rafael 0:7e3e917c6fa8 5 //Sensores de Distancia
Janice_Rafael 0:7e3e917c6fa8 6 HCSR04 usensorA(p10,p11);
Janice_Rafael 0:7e3e917c6fa8 7 HCSR04 usensorD(p6,p5);
Janice_Rafael 0:7e3e917c6fa8 8 HCSR04 usensorI(p20,p19);
Janice_Rafael 0:7e3e917c6fa8 9 //Motores
Janice_Rafael 0:7e3e917c6fa8 10 DigitalOut STBY(p15);
Janice_Rafael 0:7e3e917c6fa8 11 PwmOut PWMA(p26);
Janice_Rafael 0:7e3e917c6fa8 12 DigitalOut AIN1(p30);
Janice_Rafael 0:7e3e917c6fa8 13 DigitalOut AIN2(p29);
Janice_Rafael 0:7e3e917c6fa8 14 PwmOut PWMB(p25);
Janice_Rafael 0:7e3e917c6fa8 15 DigitalOut BIN1(p14);
Janice_Rafael 0:7e3e917c6fa8 16 DigitalOut BIN2(p13);
Janice_Rafael 0:7e3e917c6fa8 17 //Buttons
Janice_Rafael 0:7e3e917c6fa8 18 DigitalIn btn1(p16);
Janice_Rafael 0:7e3e917c6fa8 19 InterruptIn btn2(p17);
Janice_Rafael 0:7e3e917c6fa8 20
Janice_Rafael 0:7e3e917c6fa8 21 //Configuración de interrupciones
Janice_Rafael 0:7e3e917c6fa8 22 //p10,p11,p12,p13
Janice_Rafael 0:7e3e917c6fa8 23 InterruptIn INT0(p22); //Interrupción externa 0 CHA1
Janice_Rafael 0:7e3e917c6fa8 24 InterruptIn INT1(p21); //Interrupción externa 1 CHB1
Janice_Rafael 0:7e3e917c6fa8 25 InterruptIn INT2(p23); //Interrupción externa 2 CHA2
Janice_Rafael 0:7e3e917c6fa8 26 InterruptIn INT3(p24); //Interrupción externa 3 CHB2
Janice_Rafael 0:7e3e917c6fa8 27 int CNT1, CNT2; //Contadores de posición para encoders
Janice_Rafael 0:7e3e917c6fa8 28 bool flagCHA1, flagCHB1, flagCHA2, flagCHB2;
Janice_Rafael 0:7e3e917c6fa8 29
Janice_Rafael 0:7e3e917c6fa8 30 unsigned int distA, distD, distI;
Janice_Rafael 0:7e3e917c6fa8 31
Janice_Rafael 0:7e3e917c6fa8 32 long ti = 0;
Janice_Rafael 0:7e3e917c6fa8 33 void tiempo(double tiempoControlar, double tiempoInicial)
Janice_Rafael 0:7e3e917c6fa8 34 {
Janice_Rafael 0:7e3e917c6fa8 35 double ti_transcu = 0;
Janice_Rafael 0:7e3e917c6fa8 36 double t = clock();
Janice_Rafael 0:7e3e917c6fa8 37 while (tiempoControlar > ti_transcu)
Janice_Rafael 0:7e3e917c6fa8 38 {
Janice_Rafael 0:7e3e917c6fa8 39 ti_transcu = t - tiempoInicial;
Janice_Rafael 0:7e3e917c6fa8 40 t = clock();
Janice_Rafael 0:7e3e917c6fa8 41 }
Janice_Rafael 0:7e3e917c6fa8 42 }
Janice_Rafael 0:7e3e917c6fa8 43 void adelante(){
Janice_Rafael 0:7e3e917c6fa8 44 STBY = 1;
Janice_Rafael 0:7e3e917c6fa8 45 AIN1 = 1;
Janice_Rafael 0:7e3e917c6fa8 46 AIN2 = 0;
Janice_Rafael 0:7e3e917c6fa8 47 PWMA.write(0.32f);
Janice_Rafael 0:7e3e917c6fa8 48 BIN1 = 1;
Janice_Rafael 0:7e3e917c6fa8 49 BIN2 = 0;
Janice_Rafael 0:7e3e917c6fa8 50 PWMB.write(0.34f);
Janice_Rafael 0:7e3e917c6fa8 51 }
Janice_Rafael 0:7e3e917c6fa8 52 void giroDerecha(){
Janice_Rafael 0:7e3e917c6fa8 53 STBY = 1;
Janice_Rafael 0:7e3e917c6fa8 54 AIN1 = 1;
Janice_Rafael 0:7e3e917c6fa8 55 AIN2 = 0;
Janice_Rafael 0:7e3e917c6fa8 56 PWMA.write(0.29f);
Janice_Rafael 0:7e3e917c6fa8 57 BIN1 = 0;
Janice_Rafael 0:7e3e917c6fa8 58 BIN2 = 1;
Janice_Rafael 0:7e3e917c6fa8 59 PWMB.write(0.30f);
Janice_Rafael 0:7e3e917c6fa8 60 }
Janice_Rafael 0:7e3e917c6fa8 61 void giroIzquierda(){
Janice_Rafael 0:7e3e917c6fa8 62 STBY = 1;
Janice_Rafael 0:7e3e917c6fa8 63 AIN1 = 0;
Janice_Rafael 0:7e3e917c6fa8 64 AIN2 = 1;
Janice_Rafael 0:7e3e917c6fa8 65 PWMA.write(0.29f);
Janice_Rafael 0:7e3e917c6fa8 66 BIN1 = 1;
Janice_Rafael 0:7e3e917c6fa8 67 BIN2 = 0;
Janice_Rafael 0:7e3e917c6fa8 68 PWMB.write(0.30f);
Janice_Rafael 0:7e3e917c6fa8 69 }
Janice_Rafael 0:7e3e917c6fa8 70 /*
Janice_Rafael 0:7e3e917c6fa8 71 void corrDer(){
Janice_Rafael 0:7e3e917c6fa8 72 STBY = 1;
Janice_Rafael 0:7e3e917c6fa8 73 AIN1 = 1;
Janice_Rafael 0:7e3e917c6fa8 74 AIN2 = 0;
Janice_Rafael 0:7e3e917c6fa8 75 PWMA.write(0.1f);
Janice_Rafael 0:7e3e917c6fa8 76 }
Janice_Rafael 0:7e3e917c6fa8 77 void corrIzq(){
Janice_Rafael 0:7e3e917c6fa8 78 STBY = 1;
Janice_Rafael 0:7e3e917c6fa8 79 BIN1 = 1;
Janice_Rafael 0:7e3e917c6fa8 80 BIN2 = 0;
Janice_Rafael 0:7e3e917c6fa8 81 PWMB.write(0.05f);
Janice_Rafael 0:7e3e917c6fa8 82 }*/
Janice_Rafael 0:7e3e917c6fa8 83 void adelantet(long tmedir){
Janice_Rafael 0:7e3e917c6fa8 84 ti=clock();
Janice_Rafael 0:7e3e917c6fa8 85 adelante();
Janice_Rafael 0:7e3e917c6fa8 86 tiempo(tmedir,ti);
Janice_Rafael 0:7e3e917c6fa8 87 }
Janice_Rafael 0:7e3e917c6fa8 88 void giroDerechat(long tmedir){
Janice_Rafael 0:7e3e917c6fa8 89 ti=clock();
Janice_Rafael 0:7e3e917c6fa8 90 giroDerecha();
Janice_Rafael 0:7e3e917c6fa8 91 tiempo(tmedir,ti);
Janice_Rafael 0:7e3e917c6fa8 92 }
Janice_Rafael 0:7e3e917c6fa8 93 void giroIzquierdat(long tmedir){
Janice_Rafael 0:7e3e917c6fa8 94 ti=clock();
Janice_Rafael 0:7e3e917c6fa8 95 giroIzquierda();
Janice_Rafael 0:7e3e917c6fa8 96 tiempo(tmedir,ti);
Janice_Rafael 0:7e3e917c6fa8 97 }
Janice_Rafael 0:7e3e917c6fa8 98 void stopt(long tmedir){
Janice_Rafael 0:7e3e917c6fa8 99 ti=clock();
Janice_Rafael 0:7e3e917c6fa8 100 STBY = 0;
Janice_Rafael 0:7e3e917c6fa8 101 tiempo(tmedir,ti);
Janice_Rafael 0:7e3e917c6fa8 102 }
Janice_Rafael 0:7e3e917c6fa8 103 void verificarSalida()
Janice_Rafael 0:7e3e917c6fa8 104 {
Janice_Rafael 0:7e3e917c6fa8 105 //if(distA < distD && distA < distI)
Janice_Rafael 0:7e3e917c6fa8 106 if(distD >= distI)
Janice_Rafael 0:7e3e917c6fa8 107 giroDerechat(90); // 180 grados
Janice_Rafael 0:7e3e917c6fa8 108 else
Janice_Rafael 0:7e3e917c6fa8 109 if(distI > distD)
Janice_Rafael 0:7e3e917c6fa8 110 giroIzquierdat(90); // 180 grados
Janice_Rafael 0:7e3e917c6fa8 111 }
Janice_Rafael 0:7e3e917c6fa8 112 void btn2_recorrido() {
Janice_Rafael 0:7e3e917c6fa8 113 STBY = 0;
Janice_Rafael 0:7e3e917c6fa8 114 stopt(500);
Janice_Rafael 0:7e3e917c6fa8 115 }
Janice_Rafael 0:7e3e917c6fa8 116
Janice_Rafael 0:7e3e917c6fa8 117 int row = 10;
Janice_Rafael 0:7e3e917c6fa8 118 int col = 10;
Janice_Rafael 0:7e3e917c6fa8 119 int pos;
Janice_Rafael 0:7e3e917c6fa8 120 int posicion(int row, int col)
Janice_Rafael 0:7e3e917c6fa8 121 {
Janice_Rafael 0:7e3e917c6fa8 122 if(row > 5){
Janice_Rafael 0:7e3e917c6fa8 123 int nrow = 0;
Janice_Rafael 0:7e3e917c6fa8 124 nrow = (row - 6);
Janice_Rafael 0:7e3e917c6fa8 125 row = (5 - nrow);
Janice_Rafael 0:7e3e917c6fa8 126 }
Janice_Rafael 0:7e3e917c6fa8 127 if(col > 5){
Janice_Rafael 0:7e3e917c6fa8 128 int ncol = 0;
Janice_Rafael 0:7e3e917c6fa8 129 ncol = (col - 6);
Janice_Rafael 0:7e3e917c6fa8 130 col = (5 - ncol);
Janice_Rafael 0:7e3e917c6fa8 131 }
Janice_Rafael 0:7e3e917c6fa8 132
Janice_Rafael 0:7e3e917c6fa8 133 return (9 -(row - 1) - (col - 1));
Janice_Rafael 0:7e3e917c6fa8 134 }
Janice_Rafael 0:7e3e917c6fa8 135
Janice_Rafael 0:7e3e917c6fa8 136 int main(){
Janice_Rafael 0:7e3e917c6fa8 137
Janice_Rafael 0:7e3e917c6fa8 138 btn2.rise(&btn2_recorrido);//interrupcion para detener
Janice_Rafael 0:7e3e917c6fa8 139
Janice_Rafael 0:7e3e917c6fa8 140 while(1){
Janice_Rafael 0:7e3e917c6fa8 141 // recorrer laberinto
Janice_Rafael 0:7e3e917c6fa8 142 if(btn1.read()){ //Detect Switch Pres
Janice_Rafael 0:7e3e917c6fa8 143 stopt(400); // espera de 4 s
Janice_Rafael 0:7e3e917c6fa8 144 while(pos != 1){
Janice_Rafael 0:7e3e917c6fa8 145
Janice_Rafael 0:7e3e917c6fa8 146 distA=usensorA.distance(); wait_ms(90);
Janice_Rafael 0:7e3e917c6fa8 147 distI=usensorI.distance(); wait_ms(90);
Janice_Rafael 0:7e3e917c6fa8 148 distD=usensorD.distance(); wait_ms(90);
Janice_Rafael 0:7e3e917c6fa8 149
Janice_Rafael 0:7e3e917c6fa8 150 pos = posicion(row, col); // buscar meta
Janice_Rafael 0:7e3e917c6fa8 151
Janice_Rafael 0:7e3e917c6fa8 152 if(distA >= 10) {
Janice_Rafael 0:7e3e917c6fa8 153 if(distD > 10)
Janice_Rafael 0:7e3e917c6fa8 154 {
Janice_Rafael 0:7e3e917c6fa8 155 adelante();
Janice_Rafael 0:7e3e917c6fa8 156 giroDerechat(40); //90 grados
Janice_Rafael 0:7e3e917c6fa8 157 }
Janice_Rafael 0:7e3e917c6fa8 158
Janice_Rafael 0:7e3e917c6fa8 159 /*else
Janice_Rafael 0:7e3e917c6fa8 160 if(distD == distI){
Janice_Rafael 0:7e3e917c6fa8 161 }
Janice_Rafael 0:7e3e917c6fa8 162 else{
Janice_Rafael 0:7e3e917c6fa8 163 if(distD > distI){
Janice_Rafael 0:7e3e917c6fa8 164 corrDer(); //corrige a la derecha
Janice_Rafael 0:7e3e917c6fa8 165 }
Janice_Rafael 0:7e3e917c6fa8 166 else{
Janice_Rafael 0:7e3e917c6fa8 167 corrIzq();
Janice_Rafael 0:7e3e917c6fa8 168 }
Janice_Rafael 0:7e3e917c6fa8 169 stopt(10);
Janice_Rafael 0:7e3e917c6fa8 170 }*/
Janice_Rafael 0:7e3e917c6fa8 171 adelante(); // un cuadrante
Janice_Rafael 0:7e3e917c6fa8 172 stopt(10);
Janice_Rafael 0:7e3e917c6fa8 173 }
Janice_Rafael 0:7e3e917c6fa8 174 else
Janice_Rafael 0:7e3e917c6fa8 175 if(distA < 10)
Janice_Rafael 0:7e3e917c6fa8 176 {
Janice_Rafael 0:7e3e917c6fa8 177 stopt(10);
Janice_Rafael 0:7e3e917c6fa8 178 if(distD < 10 && distI > 10) {
Janice_Rafael 0:7e3e917c6fa8 179 adelante();
Janice_Rafael 0:7e3e917c6fa8 180 giroIzquierdat(50);
Janice_Rafael 0:7e3e917c6fa8 181 }
Janice_Rafael 0:7e3e917c6fa8 182 else
Janice_Rafael 0:7e3e917c6fa8 183 if(distI < 10 && distD > 10) {
Janice_Rafael 0:7e3e917c6fa8 184 adelante();
Janice_Rafael 0:7e3e917c6fa8 185 giroDerechat(40);
Janice_Rafael 0:7e3e917c6fa8 186 }
Janice_Rafael 0:7e3e917c6fa8 187 else
Janice_Rafael 0:7e3e917c6fa8 188 if(distI > 10 && distD > 10) {
Janice_Rafael 0:7e3e917c6fa8 189 adelante();
Janice_Rafael 0:7e3e917c6fa8 190 giroDerechat(40);
Janice_Rafael 0:7e3e917c6fa8 191 }
Janice_Rafael 0:7e3e917c6fa8 192 else
Janice_Rafael 0:7e3e917c6fa8 193 if(distD < 10 && distI < 10) {
Janice_Rafael 0:7e3e917c6fa8 194 verificarSalida();
Janice_Rafael 0:7e3e917c6fa8 195 }
Janice_Rafael 0:7e3e917c6fa8 196
Janice_Rafael 0:7e3e917c6fa8 197 adelante();
Janice_Rafael 0:7e3e917c6fa8 198 stopt(10);
Janice_Rafael 0:7e3e917c6fa8 199 }
Janice_Rafael 0:7e3e917c6fa8 200 }
Janice_Rafael 0:7e3e917c6fa8 201 }
Janice_Rafael 0:7e3e917c6fa8 202 }
Janice_Rafael 0:7e3e917c6fa8 203 }