SanAntN21

// sanantn21.c - Criado pelo studio UNO 2.3
// 15 August 2016 12:56:40 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 IrDirSeg;
unsigned char IrEsq;
unsigned char IrEsqSeg;
unsigned char IrMeioSeg;
unsigned int Pulsos;
int Velocidade;
 
// Prototipos
void LeEncoders(void); 
void EmFrente(void); 
void SegueLinaAnalogico(void); 
void zeraEnc(void); 
void VisualizaSeguidor(void); 
void Trajeto(void); 
void CurvaDireita(void); 
void retinho(void); 
void freio(void); 
void CurvaEsquerda(void); 
void LeSeguidor(void); 
 
//Funcoes do Usuario
void LeEncoders(void) 
{
     EncEsq = __EncoderGetValue(1);
     EncDir = __EncoderGetValue(2);
     __delay(5);
}
 
void EmFrente(void) 
{
     zeraEnc();
     // NP=Disância/ 0,618.em centímetros
     Pulsos = (Distancia / 0.618);
     while(((EncEsq < Pulsos) || (EncDir < Pulsos))) {
         retinho();
     }
 
}
 
void SegueLinaAnalogico(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 zeraEnc(void) 
{
     __SmartSensorCmd(1, 1, 9, 0);
     __SmartSensorCmd(1, 2, 9, 0);
     EncEsq = 0;
     EncDir = 0;
}
 
void VisualizaSeguidor(void) 
{
     __lcd_goto(0x7f, 1);
     __lcd_puts("%b", __SeguidorGetDigital(1, 1));
     __delay(5);
     __lcd_goto(0x7f, 2);
     __lcd_puts("%b", __SeguidorGetDigital(1, 2));
     __delay(5);
     __lcd_goto(0x7f, 3);
     __lcd_puts("%b", __SeguidorGetDigital(1, 3));
     __delay(5);
     __lcd_goto(0x7f, 4);
     __lcd_puts("%b", __SeguidorGetDigital(1, 4));
     __delay(5);
     __lcd_goto(0x7f, 5);
     __lcd_puts("%b", __SeguidorGetDigital(1, 5));
     __delay(5);
}
 
void Trajeto(void) 
{
     Velocidade = 1023;
     Distancia = 44;
     EmFrente();
     Velocidade = 700;
     Angulo = 45;
     CurvaEsquerda();
     Velocidade = 900;
     Distancia = 88;
     EmFrente();
     Velocidade = 1023;
     Angulo = 45;
     CurvaDireita();
     Velocidade = 800;
     Distancia = 44;
     EmFrente();
     Velocidade = 1023;
     Angulo = 180;
     CurvaEsquerda();
     freio();
}
 
void CurvaDireita(void) 
{
     zeraEnc();
     // 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();
}
 
void retinho(void) 
{
     LeEncoders();
     if ((EncEsq > EncDir)) {
         __motor(1, 0);
         __motor(2, Velocidade);
     }
     if ((EncEsq < EncDir)) {
         __motor(1, Velocidade);
         __motor(2, 0);
     }
     if ((EncEsq == EncDir)) {
         __motor(1, Velocidade);
         __motor(2, Velocidade);
     }
 
}
 
void freio(void) 
{
     __motor_curso(__REVERSE);
     __delay(30);
     __motor_curso(__STOP);
}
 
void CurvaEsquerda(void) 
{
     zeraEnc();
     // 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 LeSeguidor(void) 
{
     IrEsqSeg = __SeguidorGetDigital(1, 2);
     __delay(5);
     IrMeioSeg = __SeguidorGetDigital(1, 3);
     __delay(5);
     IrDirSeg = __SeguidorGetDigital(1, 4);
     __delay(5);
}
 
//Funcao Principal
void main(void)
{
     __inicializa;
     __SmartSensorCmd(2, 1, 14, 0);
     Fuga = 0;
     while(TRUE) {
         LeSeguidor();
         if (((IrEsqSeg == Branco) && ((IrMeioSeg == Linha) && (IrDirSeg == Branco)))) {
             __motor_curso(__FORWARD);
         }
         // Saindo para Direita= 1
         if (((IrEsqSeg == Linha) && ((IrMeioSeg == Branco) && (IrDirSeg == Branco)))) {
             Fuga = 1;
             __motor(1, 0);
             __motor(2, 950);
         }
         if (((IrEsqSeg == Branco) && ((IrMeioSeg == Branco) && (IrDirSeg == Linha)))) {
             Fuga = 0;
             __motor(1, 950);
             __motor(2, 0);
         }
         if (((IrEsqSeg == Linha) && ((IrMeioSeg == Linha) && (IrDirSeg == Branco)))) {
             __motor(1, 0);
             __motor(2, 750);
         }
         if (((IrEsqSeg == Branco) && ((IrMeioSeg == Linha) && (IrDirSeg == Linha)))) {
             __motor(1, 750);
             __motor(2, 0);
         }
         if (((IrEsqSeg == Branco) && ((IrMeioSeg == Branco) && (IrDirSeg == Branco)))) {
             if ((Fuga == 0)) {
                 __motor(1, 950);
                 __motor(2, 0);
             }
             else {
                 __motor(1, 0);
                 __motor(2, 950);
             }
         }
     }
 
     while(TRUE) { }
}