// jose da guiana.c - Criado pelo studio UNO 2.3
// 8 September 2016 12:44:09 pm
#include <uno.h>
// Defines
#define Branco 255
#define Caminhar 900
#define Preto 0
#define Rapido 1023
#define Tempsync 5
// Declaracao das Variaveis
unsigned int Angulo;
unsigned int DisDir;
unsigned int DisEsq;
unsigned int DisFren;
unsigned int Distancia;
int EncDir;
int EncEsq;
int Pulsos;
unsigned char Seg1;
unsigned char Seg2;
unsigned char Seg3;
unsigned char Seg4;
unsigned char Seg5;
unsigned int Velocidade;
// Prototipos
void LeSeguidores(void);
void RetoServo(void);
void zeraencoder(void);
void MeiaLua(void);
void giradireita(void);
void RetoDistancia(void);
void giraesqerda(void);
void Reto(void);
void LinhaReta(void);
void LeEncoder(void);
void freio(void);
//Funcoes do Usuario
void LeSeguidores(void)
{
Seg1 = __SeguidorGetDigital(1, 1);
__delay(Tempsync);
Seg2 = __SeguidorGetDigital(1, 2);
__delay(Tempsync);
Seg3 = __SeguidorGetDigital(1, 3);
__delay(Tempsync);
Seg4 = __SeguidorGetDigital(1, 4);
__delay(Tempsync);
Seg5 = __SeguidorGetDigital(1, 5);
__delay(Tempsync);
}
void RetoServo(void)
{
zeraencoder();
while((__analog_in(0) < 123)) {
Reto();
LeEncoder();
}
}
void zeraencoder(void)
{
__SmartSensorCmd(1, 1, 9, 0);
__SmartSensorCmd(1, 2, 9, 0);
EncDir = 0;
EncEsq = 0;
}
void MeiaLua(void)
{
zeraencoder();
__motor_curso(__LEFT_ROTATE);
Pulsos = (Angulo / 2.1);
while((__EncoderGetValue(2) < Pulsos)) {
while(!(__EncoderGetValue(2) >= Pulsos)) { __delay(10); }
}
}
void giradireita(void)
{
zeraencoder();
__motor_curso(__RIGHT_ROTATE);
Pulsos = (Angulo / 4.2);
// Np=Ângulo/4.19
while((__EncoderGetValue(1) < Pulsos)) {
while(!(__EncoderGetValue(1) >= Pulsos)) { __delay(10); }
}
}
void RetoDistancia(void)
{
zeraencoder();
Pulsos = (Distancia / 0.618);
while(((EncDir < Pulsos) || (EncEsq < Pulsos))) {
Reto();
LeEncoder();
}
}
void giraesqerda(void)
{
zeraencoder();
__motor_curso(__LEFT_ROTATE);
Pulsos = (Angulo / 4.2);
while((__EncoderGetValue(2) < Pulsos)) {
while(!(__EncoderGetValue(2) >= Pulsos)) { __delay(10); }
}
}
void Reto(void)
{
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 LinhaReta(void)
{
if (((Seg1 == Branco) && ((Seg2 == Preto) && (Seg5 == Branco)))) {
__motor(1, Caminhar);
__motor(2, Caminhar);
}
if (((Seg1 == Branco) && ((Seg2 == Preto) && (Seg5 == Preto)))) {
__motor(1, Caminhar);
__motor(2, 0);
}
if (((Seg1 == Preto) && ((Seg2 == Preto) && (Seg5 == Branco)))) {
__motor(1, 0);
__motor(2, Caminhar);
}
if (((Seg1 == Branco) && ((Seg2 == Branco) && (Seg5 == Branco)))) {
__motor(1, Caminhar);
__motor(2, Caminhar);
}
if (((Seg1 == Preto) && ((Seg2 == Branco) && (Seg5 == Branco)))) {
__motor(1, Rapido);
__motor(2, 0);
}
if (((Seg1 == Branco) && ((Seg2 == Branco) && (Seg5 == Branco)))) {
__motor(1, 0);
__motor(2, Rapido);
}
}
void LeEncoder(void)
{
EncEsq = __EncoderGetValue(1);
EncDir = __EncoderGetValue(2);
__delay(Tempsync);
}
void freio(void)
{
__motor_curso(__REVERSE);
__delay(40);
__motor_curso(__STOP);
}
//Funcao Principal
void main(void)
{
__inicializa;
__SmartSensorCmd(2, 1, 14, 0);
while(TRUE) {
LeSeguidores();
LinhaReta();
}
while(TRUE) { }
}