N2SantoAntonio

// n2santoantonio.c - Criado pelo studio UNO 2.3
// 17 October 2016 1:11:20 pm
 
#include <uno.h>
 
 
// Defines
#define Branco 255
#define Linha 0
 
// Declaracao das Variaveis
unsigned int Angulo;
unsigned int Distancia;
int EncDir;
int EncEsq;
unsigned char Fuga;
unsigned char IrDir;
unsigned char IrDir1Seg;
unsigned char IrDirSeg;
unsigned char IrEsq;
unsigned char IrEsq1Seg;
unsigned char IrEsqSeg;
unsigned char IrMeioSeg;
unsigned char Parar;
unsigned int Pulsos;
int Velocidade;
unsigned char ZonaSegura;
 
// Prototipos
void Reto(void); 
void ZeraEndoders(void); 
void Freio(void); 
void Trajeto(void); 
void CurvaEsquerda(void); 
void SegueLinhaAnalogico(void); 
void LeSeguidor(void); 
void EmFrente(void); 
void LeEncoders(void); 
void CurvaDireita(void); 
void GiraEsquerda(void); 
void SegueLinha(void); 
void GiraDireita(void); 
 
//Funcoes do Usuario
void Reto(void) 
{
     LeEncoders();
     if ((EncEsq > EncDir)) {
         CurvaEsquerda();
     }
     if ((EncEsq < EncDir)) {
         CurvaDireita();
     }
     if ((EncEsq == EncDir)) {
         __motor(1, Velocidade);
         __motor(2, Velocidade);
     }
 
}
 
void ZeraEndoders(void) 
{
     __SmartSensorCmd(1, 1, 9, 0);
     __SmartSensorCmd(1, 2, 9, 0);
     EncEsq = 0;
     EncDir = 0;
}
 
void Freio(void) 
{
     __motor_curso(__REVERSE);
     __delay(30);
     __motor_curso(__STOP);
}
 
void Trajeto(void) 
{
     Velocidade = 1023;
     Distancia = 44;
     EmFrente();
     Velocidade = 700;
     Angulo = 45;
     GiraEsquerda();
     Velocidade = 900;
     Distancia = 88;
     EmFrente();
     Velocidade = 1023;
     Angulo = 45;
     GiraDireita();
     Velocidade = 800;
     Distancia = 44;
     EmFrente();
     Velocidade = 1023;
     Angulo = 180;
     GiraEsquerda();
     Freio();
}
 
void CurvaEsquerda(void) 
{
     __motor(1, -500);
     __motor(2, Velocidade);
}
 
void SegueLinhaAnalogico(void) 
{
     __motor_curso(__FORWARD);
     while(TRUE) {
         if ((__SeguidorGetAnalog(1, 2) < IrEsq)) {
             __motor_curso(__LEFT_TURN);
         }
         if ((__SeguidorGetAnalog(1, 4) < IrDir)) {
             __motor_curso(__RIGHT_TURN);
         }
     }
 
}
 
void LeSeguidor(void) 
{
     IrEsq1Seg = __SeguidorGetDigital(1, 1);
     __delay(5);
     IrEsqSeg = __SeguidorGetDigital(1, 2);
     __delay(5);
     IrMeioSeg = __SeguidorGetDigital(1, 3);
     __delay(5);
     IrDirSeg = __SeguidorGetDigital(1, 4);
     __delay(5);
     IrDir1Seg = __SeguidorGetDigital(1, 5);
     __delay(5);
}
 
void EmFrente(void) 
{
     ZeraEndoders();
     // NP=Disância/ 0,618.em centímetros
     Pulsos = (Distancia / 0.618);
     while(((EncEsq < Pulsos) || (EncDir < Pulsos))) {
         Reto();
     }
 
}
 
void LeEncoders(void) 
{
     EncEsq = __EncoderGetValue(1);
     EncDir = __EncoderGetValue(2);
     __delay(5);
}
 
void CurvaDireita(void) 
{
     __motor(1, Velocidade);
     __motor(2, -500);
}
 
void GiraEsquerda(void) 
{
     ZeraEndoders();
     // NP=Angulo/ 4,9
     Pulsos = (Angulo / 4.9);
     __motor(1, (Velocidade * -1));
     __motor(2, Velocidade);
     while((__EncoderGetValue(2) < Pulsos)) {
         while(!(__EncoderGetValue(2) >= Pulsos)) { __delay(10); }
         __delay(5);
     }
     Freio();
}
 
void SegueLinha(void) 
{
     LeSeguidor();
     if (((IrEsqSeg == Branco) && ((IrMeioSeg == Linha) && (IrDirSeg == Branco)))) {
         __motor_curso(__FORWARD);
     }
     // Saindo para Direita= 1
     if (((IrEsqSeg == Linha) && ((IrMeioSeg == Branco) && (IrDirSeg == Branco)))) {
         Fuga = 1;
         Velocidade = 950;
         CurvaEsquerda();
     }
     if (((IrEsqSeg == Branco) && ((IrMeioSeg == Branco) && (IrDirSeg == Linha)))) {
         Fuga = 0;
         Velocidade = 950;
         CurvaDireita();
     }
     if (((IrEsqSeg == Linha) && ((IrMeioSeg == Linha) && (IrDirSeg == Branco)))) {
         Velocidade = 750;
         CurvaEsquerda();
     }
     if (((IrEsqSeg == Branco) && ((IrMeioSeg == Linha) && (IrDirSeg == Linha)))) {
         Velocidade = 750;
         CurvaDireita();
     }
     if (((IrEsqSeg == Branco) && ((IrMeioSeg == Branco) && (IrDirSeg == Branco)))) {
         Velocidade = 950;
         if ((Fuga == 0)) {
             CurvaDireita();
         }
         else {
             CurvaEsquerda();
         }
     }
     if (((IrEsqSeg == Linha) && ((IrMeioSeg == Linha) && (IrDirSeg == Linha)))) {
         if (((IrEsq1Seg == Linha) && (IrDir1Seg == Linha))) {
             __beep();
             if ((Parar == 0)) {
                 ZonaSegura++;
             }
             if (((ZonaSegura == 2) || (ZonaSegura == 4))) {
                 Parar = 1;
             }
         }
     }
 
}
 
void GiraDireita(void) 
{
     ZeraEndoders();
     // NP=Angulo/ 4,9
     Pulsos = (Angulo / 4.9);
     __motor(1, Velocidade);
     __motor(2, (Velocidade * -1));
     while((__EncoderGetValue(1) < Pulsos)) {
         while(!(__EncoderGetValue(1) >= Pulsos)) { __delay(10); }
         __delay(5);
     }
     Freio();
}
 
//Funcao Principal
void main(void)
{
     __inicializa;
     __SmartSensorCmd(2, 1, 14, 0);
     __SmartSensorCmd(1, 1, 8, 0);
     bitset(__TRIS_PWM, __DIGITAL1);
     __PWM1 = 180;
     ZonaSegura = 0;
     for (int __i0 = 0; __i0 < 2; ++__i0) {
         Fuga = 0;
         while((__analog_in(0) < 500)) {
             SegueLinha();
         }
         Freio();
         __PWM1 = 10;
         __delay(500);
         Angulo = 180;
         Velocidade = 1023;
         GiraEsquerda();
         Parar = 0;
         while((Parar == 0)) {
             SegueLinha();
         }
         Freio();
         __PWM1 = 180;
         __delay(1000);
         __motor_curso(__BACKWARD);
         __delay(500);
         CurvaDireita();
     }
     __beep();
     while(TRUE) { }
}