//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();
}
cuadrado cuadrado:: dameCuadradoIzquierdo(GLdouble angulo)
{
//GLdouble h=sqrt(2)*distancia/2.0;
GLdouble h=distancia*sin(3.1416*angulo/180.0);
cuadrado izquierda(esquina_superior_izquierda,h);
izquierda.creaCuadrado(direccion-90-angulo);
if (red!=0)
izquierda.setRed(red-0.1);
else
izquierda.setRed(0);
if (green!=1)
izquierda.setGreen(green+0.1);
else
izquierda.setGreen(1);
if (blue!=0)
izquierda.setBlue(blue-0.01);
else
izquierda.setBlue(0);
return izquierda;
}
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();
}
}
}
//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();
}
}
}