N3Dohms_lokao

// n3dohms_lokao.c - Criado pelo studio UNO 2.3
// 30 May 2016 7:36:38 pm
 
#include <uno.h>
 
 
// Defines
#define tempo 10
 
// Declaracao das Variaveis
unsigned char AceX;
unsigned char AceY;
unsigned char AceZ;
unsigned char BtC;
unsigned char BtZ;
unsigned char JoyX;
unsigned char JoyY;
int eixo_x;
int eixo_y;
unsigned char garra;
int motor_dir;
int motor_esq;
 
// Prototipos
void LeRF(void); 
void controlejoy(void); 
void ajusta_limites(void); 
 
//Funcoes do Usuario
void LeRF(void) 
{
     JoyX = __ControllerGetJoystick_X(1);
     __delay(tempo);
     JoyY = __ControllerGetJoystick_Y(1);
     __delay(tempo);
     AceX = __ControllerGetAccelerometer_X(1);
     __delay(tempo);
     AceY = __ControllerGetAccelerometer_Y(1);
     __delay(tempo);
     AceZ = __ControllerGetAccelerometer_Z(1);
     __delay(tempo);
     BtC = __ControllerGetButton_C(1);
     __delay(tempo);
     BtZ = __ControllerGetButton_Z(1);
     __delay(tempo);
}
 
void controlejoy(void) 
{
     while(TRUE) {
         LeRF();
         if ((JoyX < 126)) {
             __motor_curso(__LEFT_ROTATE);
         }
         if ((JoyX > 130)) {
             __motor_curso(__RIGHT_ROTATE);
         }
         if ((JoyY < 126)) {
             __motor_curso(__BACKWARD);
         }
         if ((JoyY > 130)) {
             __motor_curso(__FORWARD);
         }
         if (((JoyX == 128) && (JoyY == 128))) {
             __motor_curso(__STOP);
         }
         if ((BtC == 1)) {
             __PWM1 = 180;
         }
         if ((BtZ == 1)) {
             __PWM1 = 40;
         }
     }
 
}
 
void ajusta_limites(void) 
{
     if ((motor_esq < -1023)) {
         motor_esq = -1023;
     }
     if ((motor_esq > 1023)) {
         motor_esq = 1023;
     }
     if ((motor_dir < -1023)) {
         motor_dir = -1023;
     }
     if ((motor_dir > 1023)) {
         motor_dir = 1023;
     }
 
}
 
//Funcao Principal
void main(void)
{
     __inicializa;
     garra = 180;
     bitset(__TRIS_PWM, __DIGITAL1);
     __PWM1 = garra;
     while(TRUE) {
         LeRF();
         eixo_x = ((JoyX - 128) * 8);
         eixo_y = ((JoyY - 128) * 8);
         motor_esq = (eixo_x + eixo_y);
         motor_dir = (eixo_y - eixo_x);
         ajusta_limites();
         __motor(1, motor_esq);
         __motor(2, motor_dir);
         if (((BtC == 1) && (garra > 0))) {
             garra -= 5;
         }
         if (((BtZ == 1) && (garra < 0))) {
             garra += 5;
         }
         __PWM1 = garra;
     }
 
     while(TRUE) { }
}