labirinto com rota conhecida

// labirinto com rota conhecida.c - Criado pelo studio UNO 2.3
// 3 October 2014 12:51:49 pm
 
#include <uno.h>
 
 
// Declaracao das Variaveis
unsigned char cruzamento;
unsigned int direito;
unsigned int esquerdo;
unsigned char flag;
unsigned char lado;
unsigned int meio;
 
// Prototipos
void perde_dir(void); 
void incrementa(void); 
void perde_esq(void); 
 
//Funcoes do Usuario
void perde_dir(void) 
{
     __motor_curso(__RIGHT_TURN);
     __delay(150);
     __motor_curso(__RIGHT_ROTATE);
     __delay(300);
     while(!!(__SeguidorGetDigital(1, 4))) { __delay(10); }
}
 
void incrementa(void) 
{
     cruzamento++;
     __lcd_goto(0x7f, 1);
     __lcd_puts("%c", cruzamento);
     if ((cruzamento == 1)) {
         perde_dir();
     }
     if ((cruzamento == 2)) {
         perde_esq();
     }
     if ((cruzamento == 3)) {
         perde_dir();
     }
     if ((cruzamento == 4)) {
         perde_esq();
     }
     if ((cruzamento == 5)) {
         __motor_curso(__STOP);
         __beep();
         while(1) {}
     }
 
}
 
void perde_esq(void) 
{
     __motor_curso(__LEFT_TURN);
     __delay(150);
     __motor_curso(__LEFT_ROTATE);
     __delay(300);
     while(!!(__SeguidorGetDigital(1, 2))) { __delay(10); }
}
 
//Funcao Principal
void main(void)
{
     __inicializa;
     __SmartSensorCmd(2, 1, 14, 0);
     __backlight(__ON);
     cruzamento = 0;
     flag = 0;
     __lcd_goto(0x7f, 1);
     __lcd_puts("%c", cruzamento);
     while(TRUE) {
         esquerdo = __SeguidorGetDigital(1, 2);
         meio = __SeguidorGetDigital(1, 3);
         direito = __SeguidorGetDigital(1, 4);
         if (((esquerdo == 255) && ((meio == 0) && (direito == 255)))) {
             __motor_curso(__FORWARD);
         }
         if (((esquerdo == 0) && ((meio == 255) && (direito == 255)))) {
             __motor_curso(__LEFT_TURN);
             lado = 0;
         }
         if (((esquerdo == 255) && ((meio == 255) && (direito == 0)))) {
             __motor_curso(__RIGHT_TURN);
             lado = 1;
         }
         if (((esquerdo == 0) && ((meio == 0) && (direito == 255)))) {
             __motor(1, 950);
             __motor(2, 1023);
             lado = 0;
         }
         if (((esquerdo == 255) && ((meio == 0) && (direito == 0)))) {
             __motor(1, 1023);
             __motor(2, 950);
             lado = 1;
         }
         if (((esquerdo == 255) && ((meio == 255) && (direito == 255)))) {
             if ((lado == 0)) {
                 __motor_curso(__LEFT_ROTATE);
             }
             else {
                 __motor_curso(__RIGHT_ROTATE);
             }
         }
         if (((esquerdo == 255) || ((meio == 255) || (direito == 255)))) {
             flag = 0;
         }
         if (((esquerdo == 0) && ((meio == 0) && (direito == 0)))) {
             __motor_curso(__FORWARD);
             if ((flag == 0)) {
                 if ((!(__SeguidorGetDigital(1, 1)) || !(__SeguidorGetDigital(1, 5)))) {
                     incrementa();
                     flag = 1;
                 }
             }
         }
     }
 
     while(TRUE) { }
}