//Función que revisa los stacks izq y der para saber en qué dirección colocar al robot luego del backtracking void revisarStacks(Stack *stder,Stack *stizq,Stack *stcamino){ if(top(stder)==1&&top(stizq)==0) { pop(stcamino); derecha(); push(stcamino, -2); paso(); } if(top(stder)==0&&top(stizq)==1) { pop(stcamino); izquierda(); push(stcamino, -1); paso(); } if(top(stder)==1&&top(stizq)==1) { pop(stder); push(stder,0); derecha(); push(stcamino, -2); paso(); } }
//Función que revisa el stack de camino para regresar al robot hasta a la última bifurcación que encontró void backtracking(Stack *stder,Stack *stizq,Stack *stcamino) { int valor=0; while(top(stcamino)!=0) { if(top(stcamino)>0) { valor=top(stcamino); pop(stcamino); valor=valor*(-1); drive_goto(valor,valor); } if(top(stcamino)==-1) { pop(stcamino); derecha(); } if(top(stcamino)==-2) { pop(stcamino); izquierda(); } } revisarStacks(stder,stizq,stcamino); }
void loop() { // // // if (!digitalread(I)) izquierda(); // if (!digitalread(D)) derecha(); ServoWrite(LI,500); // enciende los led y varia su brillo ServoWrite(LD,20); adelante(); }
int main() // Main function { low(26); low(27); // Add startup code here. while(1) { if(revisarDerecha()==1 && revisarAdelante()==1){ derecha(); } while(revisarAdelante()==1&&revisarDerecha()==0){ recto(); } if(revisarAdelante()==0&&revisarDerecha()==1){ derecha(); } if(revisarAdelante()==0&&revisarDerecha()==0){ izquierda(); } } }
cuadrado cuadrado:: dameCuadradoDerecho(GLdouble angulo) { //GLdouble h=sqrt(2)*distancia/2.0; GLdouble h= distancia*cos(3.1416*angulo/180.0); cuadrado derecha(esquina_superior_derecha,h); derecha.setEsquinaInferiorDerecha(esquina_superior_derecha); derecha.crearCuadradoDerecha(direccion-90-angulo); if (red!=0) derecha.setRed(red-0.005); else derecha.setRed(0); if (green!=1) derecha.setGreen(green+0.005); else derecha.setGreen(1); if (blue!=0) derecha.setBlue(blue-0.005); else derecha.setBlue(0); return derecha; }
//Función main que tiene un while infinito para llamar a todas las funciones en cada ciclo, para que el robot resuelva el laberinto int main() { //Se crean los stacks Stack *stder = createStack(100); Stack *stizq = createStack(100); Stack *stcamino = createStack(100); while(1){ revisar(); magia(stder,stizq,stcamino); if(ade==1){ paso(); } else if(der==1){ derecha(); paso(); } else if(izq==1){ izquierda(); paso(); } } }