modulo2e3.v2

// modulo2e3.c - Criado pelo studio UNO 2.3
// 5 November 2015 1:27:39 pm
 
#include <uno.h>
 
 
// Defines
#define branco 255
#define lado_direito 1
#define lado_esquerdo 0
#define linha 0
#define pista 255
 
// Declaracao das Variaveis
unsigned char ace_x;
unsigned char ace_y;
unsigned char ace_z;
int enc_dir;
int enc_esq;
unsigned char ir_1;
unsigned char ir_2;
unsigned char ir_3;
unsigned char ir_4;
unsigned char ir_5;
unsigned char joy_c;
unsigned char joy_x;
unsigned char joy_y;
unsigned char joy_z;
unsigned char lado;
unsigned char pan_servo;
unsigned char set_garra;
unsigned char tilt_servo;
 
// Prototipos
void frente(void); 
void gira_esq_180(void); 
void le_segui(void); 
void joystick(void); 
void le_enconder(void); 
void pantilt(void); 
void le_joy(void); 
void freio(void); 
void seguelinha(void); 
void resgate(void); 
void curva_esq_90(void); 
void inicia(void); 
 
//Funcoes do Usuario
// 324pulsos*6,18mm/pulso=2002,32 aproximadamente 2m
void frente(void) 
{
     while(((enc_esq < 324) || (enc_dir < 324))) {
         // Para descobrir o número de pulsos basta dividir a
         // distância desejada em milimetros por 6,18
         // 
         // ou
         // 
         // distância desejada em centimetros por 0,618
         // 
         // ou
         // 
         // distância desejada em metros por 0,00618
         le_enconder();
         if ((enc_esq > enc_dir)) {
             __motor_curso(__LEFT_TURN);
         }
         if ((enc_esq < enc_dir)) {
             __motor_curso(__RIGHT_TURN);
         }
         if ((enc_esq == enc_dir)) {
             __motor_curso(__FORWARD);
         }
     }
     freio();
}
 
// rotaciona o robo a esquerda 180º
void gira_esq_180(void) 
{
     __SmartSensorCmd(1, 2, 9, 0);
     __motor_curso(__LEFT_ROTATE);
     while((__EncoderGetValue(2) < 34)) {
         __delay(5);
         while(!(__EncoderGetValue(2) >= 34)) { __delay(10); }
         __delay(5);
     }
     freio();
}
 
void le_segui(void) 
{
     ir_1 = __SeguidorGetDigital(1, 1);
     __delay(5);
     ir_2 = __SeguidorGetDigital(1, 2);
     __delay(5);
     ir_3 = __SeguidorGetDigital(1, 3);
     __delay(5);
     ir_4 = __SeguidorGetDigital(1, 4);
     __delay(5);
     ir_5 = __SeguidorGetDigital(1, 5);
     __delay(5);
}
 
// Le e controla o Robo com o Controle Wii
void joystick(void) 
{
     inicia();
     while(TRUE) {
         le_joy();
         if ((joy_y == 255)) {
             __motor_curso(__FORWARD);
         }
         if ((joy_y == 0)) {
             __motor_curso(__BACKWARD);
         }
         if ((joy_x == 255)) {
             __motor_curso(__RIGHT_TURN);
         }
         if ((joy_x == 0)) {
             __motor_curso(__LEFT_TURN);
         }
         if ((joy_c == 1)) {
             __PWM1 = 180;
         }
         if ((joy_z == 1)) {
             __PWM1 = 40;
         }
         if ((ace_y > 100)) {
             freio();
         }
     }
 
}
 
void le_enconder(void) 
{
     enc_esq = __EncoderGetValue(1);
     __delay(5);
     enc_dir = __EncoderGetValue(2);
     __delay(5);
}
 
// Controla o servo e o pantilt com o teclado
void pantilt(void) 
{
     while(TRUE) {
         if ((__BOTTOM_KEY && (tilt_servo <= 175))) {
             tilt_servo += 5;
             while(!!(__BOTTOM_KEY)) { __delay(10); }
         }
         if ((__UP_KEY && (tilt_servo >= 5))) {
             while(!!(__UP_KEY)) { __delay(10); }
             tilt_servo -= 5;
         }
         if ((__LEFT_KEY && (pan_servo <= 175))) {
             pan_servo += 5;
             while(!!(__LEFT_KEY)) { __delay(10); }
         }
         if ((__RIGHT_KEY && (pan_servo >= 5))) {
             while(!!(__RIGHT_KEY)) { __delay(10); }
             pan_servo -= 5;
         }
         __PWM1 = set_garra;
         __PWM2 = pan_servo;
         __PWM3 = tilt_servo;
     }
 
}
 
void le_joy(void) 
{
     ace_x = __ControllerGetAccelerometer_X(1);
     __delay(10);
     ace_y = __ControllerGetAccelerometer_Y(1);
     __delay(10);
     ace_z = __ControllerGetAccelerometer_Z(1);
     __delay(10);
     joy_x = __ControllerGetJoystick_X(1);
     __delay(10);
     joy_y = __ControllerGetJoystick_Y(1);
     __delay(10);
     joy_c = __ControllerGetButton_C(1);
     __delay(10);
     joy_z = __ControllerGetButton_Z(1);
     __delay(10);
}
 
void freio(void) 
{
     __motor_curso(__REVERSE);
     __delay(10);
     __motor_curso(__STOP);
}
 
void seguelinha(void) 
{
     if (((ir_2 == linha) && ((ir_3 == linha) && (ir_4 == linha)))) {
         if ((lado == lado_direito)) {
             __motor_curso(__LEFT_ROTATE);
         }
         else {
             __motor_curso(__RIGHT_ROTATE);
         }
     }
     if (((ir_2 == pista) && ((ir_3 == linha) && (ir_4 == pista)))) {
         __motor(1, 900);
         __motor(2, 900);
     }
     if (((ir_2 == linha) && ((ir_3 == linha) && (ir_4 == pista)))) {
         __motor(1, 700);
         __motor(2, 900);
         lado = lado_direito;
     }
     if (((ir_2 == pista) && ((ir_3 == linha) && (ir_4 == linha)))) {
         __motor(1, 900);
         __motor(2, 700);
         lado = lado_esquerdo;
     }
     if (((ir_2 == linha) && ((ir_3 == pista) && (ir_4 == pista)))) {
         __motor(1, 0);
         __motor(2, 900);
         lado = lado_direito;
     }
     if (((ir_2 == pista) && ((ir_3 == pista) && (ir_4 == linha)))) {
         __motor(1, 900);
         __motor(2, 0);
         lado = lado_esquerdo;
     }
 
}
 
void resgate(void) 
{
     __PWM1 = 180;
     while((__analog_in(0) < 200)) {
         le_segui();
         seguelinha();
     }
     freio();
     __PWM1 = 45;
     __beep();
     __delay(1000);
     gira_esq_180();
     while(!(((ir_1 == linha) && ((ir_2 == linha) && ((ir_3 == linha) && ((ir_4 == linha) && (ir_5 == linha))))))) {
         le_segui();
         seguelinha();
     }
     __PWM1 = 180;
     __beep();
}
 
// Faz uma curva a esquerda em 90º
void curva_esq_90(void) 
{
     __SmartSensorCmd(1, 2, 9, 0);
     __motor_curso(__LEFT_TURN);
     while((__EncoderGetValue(2) < 34)) {
         __delay(5);
         while(!(__EncoderGetValue(2) >= 34)) { __delay(10); }
         __delay(5);
     }
     freio();
}
 
void inicia(void) 
{
     enc_esq = 0;
     enc_dir = 0;
     __SmartSensorCmd(2, 1, 14, 0);
     __SmartSensorCmd(1, 1, 8, 0);
     __SmartSensorCmd(1, 1, 9, 0);
     __SmartSensorCmd(1, 2, 9, 0);
     bitset(__TRIS_PWM, __DIGITAL1);
     bitset(__TRIS_PWM, __DIGITAL2);
     bitset(__TRIS_PWM, __DIGITAL3);
     set_garra = 180;
     pan_servo = 90;
     tilt_servo = 90;
}
 
//Funcao Principal
void main(void)
{
     __inicializa;
     inicia();
     resgate();
     while(TRUE) { }
}