N3Dohms_Rudi

// n3dohms_rudi.c - Criado pelo studio UNO 2.3
// 20 June 2016 7:25:26 pm
 
#include <uno.h>
 
 
// Defines
#define VariaGarra 5
#define tempo 10
 
// Declaracao das Variaveis
unsigned char AceX;
unsigned char AceY;
unsigned char AceZ;
unsigned char BtC;
unsigned char BtZ;
unsigned char Garra;
unsigned int Graus;
unsigned char JoyX;
unsigned char JoyY;
int Motor_D;
int Motor_E;
unsigned int Pulsos;
int eixo_x;
int eixo_y;
 
// Prototipos
void ZeraEncoders(void); 
void gira_direita(void); 
void ControleJoy(void); 
void Freio(void); 
void LeRF(void); 
void ControleRF(void); 
void gira_esquerda(void); 
void ControleMotores(void); 
void AjusteLimites(void); 
 
//Funcoes do Usuario
void ZeraEncoders(void) 
{
     __SmartSensorCmd(1, 1, 9, 0);
     __SmartSensorCmd(1, 2, 9, 0);
}
 
void gira_direita(void) 
{
     Pulsos = (Graus / 4.9);
     __motor_curso(__RIGHT_ROTATE);
     while((__EncoderGetValue(1) < Pulsos)) {
         while(!(__EncoderGetValue(1) >= Pulsos)) { __delay(10); }
     }
     Freio();
     ZeraEncoders();
}
 
void ControleJoy(void) 
{
     if ((JoyX < 126)) {
         Graus = 3;
         gira_esquerda();
     }
     if ((JoyX > 130)) {
         Graus = 3;
         gira_direita();
     }
     if ((JoyY < 126)) {
         __motor_curso(__BACKWARD);
     }
     if ((JoyY > 130)) {
         __motor_curso(__FORWARD);
     }
     if (((JoyX == 128) && // NP=Angulo/4,9
(JoyY == 128))) {
         __motor_curso(__STOP);
     }
     if ((BtC == 1)) {
         __PWM1 = 180;
     }
     if ((BtZ == 1)) {
         __PWM1 = 40;
     }
 
}
 
void Freio(void) 
{
     __motor_curso(__REVERSE);
     __delay(30);
     __motor_curso(__STOP);
}
 
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 ControleRF(void) 
{
     eixo_x = ((JoyX - 128) * 8);
     eixo_y = ((JoyY - 128) * 8);
     Motor_E = (eixo_x + eixo_y);
     Motor_D = (eixo_y - eixo_x);
     AjusteLimites();
     ControleMotores();
}
 
void gira_esquerda(void) 
{
     Pulsos = (Graus / 4.9);
     __motor_curso(__LEFT_ROTATE);
     while((__EncoderGetValue(2) < Pulsos)) {
         while(!(__EncoderGetValue(2) >= Pulsos)) { __delay(10); }
     }
     Freio();
     ZeraEncoders();
}
 
void ControleMotores(void) 
{
     __motor(1, Motor_E);
     __motor(2, Motor_D);
     if (((BtC == 1) && (Garra < 180))) {
         Garra += VariaGarra;
     }
     if (((BtZ == 1) && (Garra > 0))) {
         Garra -= VariaGarra;
     }
     __PWM1 = Garra;
}
 
void AjusteLimites(void) 
{
     if ((Motor_E < -1023)) {
         Motor_E = -1023;
     }
     if ((Motor_E > 1023)) {
         Motor_E = 1023;
     }
     if ((Motor_D < -1023)) {
         Motor_D = -1023;
     }
     if ((Motor_D > 1023)) {
         Motor_D = 1023;
     }
 
}
 
//Funcao Principal
void main(void)
{
     __inicializa;
     __SmartSensorCmd(1, 1, 8, 0);
     bitset(__TRIS_PWM, __DIGITAL1);
     __PWM1 = 180;
     Garra = 180;
     while(TRUE) {
         LeRF();
         ControleJoy();
     }
 
     while(TRUE) { }
}