// modulo2e3.c - Criado pelo studio UNO 2.3
// 24 November 2015 6:45:22 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 curva_esq_90(void);
void le_segui(void);
void pantilt(void);
void freio(void);
void joystick(void);
void gira_esq_180(void);
void le_enconder(void);
void inicia(void);
void resgate(void);
void le_joy(void);
void seguelinha(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();
}
// 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 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);
}
// 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 freio(void)
{
__motor_curso(__REVERSE);
__delay(10);
__motor_curso(__STOP);
}
// 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();
}
}
}
// 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_enconder(void)
{
enc_esq = __EncoderGetValue(1);
__delay(5);
enc_dir = __EncoderGetValue(2);
__delay(5);
}
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;
}
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();
}
void le_joy(void)
{
ace_x = __ControllerGetAccelerometer_X(1);
__delay(5);
ace_y = __ControllerGetAccelerometer_Y(1);
__delay(5);
ace_z = __ControllerGetAccelerometer_Z(1);
__delay(5);
joy_x = __ControllerGetJoystick_X(1);
__delay(5);
joy_y = __ControllerGetJoystick_Y(1);
__delay(5);
joy_c = __ControllerGetButton_C(1);
__delay(5);
joy_z = __ControllerGetButton_Z(1);
__delay(5);
}
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, 800);
__motor(2, 800);
}
if (((ir_2 == linha) && ((ir_3 == linha) && (ir_4 == pista)))) {
__motor(1, 700);
__motor(2, 800);
lado = lado_direito;
}
if (((ir_2 == pista) && ((ir_3 == linha) && (ir_4 == linha)))) {
__motor(1, 800);
__motor(2, 700);
lado = lado_esquerdo;
}
if (((ir_2 == linha) && ((ir_3 == pista) && (ir_4 == pista)))) {
__motor(1, 600);
__motor(2, 800);
lado = lado_direito;
}
if (((ir_2 == pista) && ((ir_3 == pista) && (ir_4 == linha)))) {
__motor(1, 800);
__motor(2, 600);
lado = lado_esquerdo;
}
}
//Funcao Principal
void main(void)
{
__inicializa;
inicia();
while(TRUE) {
le_segui();
seguelinha();
}
while(TRUE) { }
}