// 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) { }
}