Master Function - UnbDroid/sekksi2016 GitHub Wiki

#include "CableCommunication.h"
#include "protocol.h"

#define CONNECTION 1
#define MAOZINHAS OUT_BC

#define SENSOR_LUZ_ESQ IN_1
#define SENSOR_LUZ_DIR IN_2
#define SENSOR_DIST_FRONT IN_3
#define SENSOR_COMUNICACAO IN_4

#define LUZ_ESQ SensorValue(SENSOR_LUZ_ESQ)
#define LUZ_DIR SensorValue(SENSOR_LUZ_DIR)
#define DIST_FRONT SensorUS(SENSOR_DIST_FRONT)
#define COMUNICACAO SENSOR_4

#define JOGAR RotateMotor(MAOZINHAS,100,-90)
#define DESCER OnFwd(MAOZINHAS,20);Wait(1000);Off(MAOZINHAS)

string ok = "0";

string andarFrente = "a";
string andarTras = "b";
string pararMotores = "c";
string andarAteLinhaQuadradoRe = "d";
string andarAteLinhaPretaRe = "e";
string viraNorteEsquerdoHorario = "f";
string viraNorteEsquerdoAntiHorario = "g";
string viraLesteEsquerdoHorario =  "h";
string viraLesteEsquerdoAntiHorario = "i";
string viraOesteEsquerdoHorario = "j";
string viraOesteEsquerdoAntiHorario = "k";
string viraSulEsquerdoHorario = "l";
string viraSulEsquerdoAntiHorario = "m";
string viraNorteDireitoHorario = "n";
string viraNorteDireitoAntiHorario = "o";
string viraLesteDireitoHorario = "p";
string viraLesteDireitoAntiHorario ="q";
string viraOesteDireitoHorario = "r";
string viraOesteDireitoAntiHorario ="s";
string viraSulDireitoHorario = "t";
string viraSulDireitoAntiHorario = "u";
string viraEsq = "v";
string viraDir = "w";
string pararRotacao = "x";
string pararTudo = "y";

#define ANDAR_FRENTE SendCableMessage(andarFrente)//RemoteStartProgram(CONNECTION, "AndarFrente.rxe");//OnRev(RODAS,50)//OnRevRegPID(RODAS,50,OUT_REGMODE_SYNC,P,I,D)
#define ANDAR_TRAS SendCableMessage(andarTras)//RemoteStartProgram(CONNECTION, "AndarTras.rxe");//OnFwd(RODAS,50)//OnFwdRegPID(RODAS,50,OUT_REGMODE_SYNC,P,I,D)
#define VIRAR_ESQ SendCableMessage(viraEsq)
#define VIRAR_DIR SendCableMessage(viraDir)
#define PARAR SendCableMessage(pararMotores)//RemoteStopProgram(CONNECTION)
#define PARAR_ROTACAO SendCableMessage(pararRotacao)

#define ESQUERDA 0
#define DIREITA 1

int TEMPO_ERRO =  7000;

int limiar = 15;

bool esperandoMensagem = false;

int contFunction=0;

int lado;

int ladoInicial;

task mexerPa(){
     while(true){
     if(esperandoMensagem ){
      OnRev(MAOZINHAS,40);
      Wait(100);
      OnFwd(MAOZINHAS,20);
      Wait(100);
      Off(MAOZINHAS);
     }
     }
}

void AndarAteParede(){
     do{
     ANDAR_FRENTE;
     }until(DIST_FRONT<12);
     PARAR;
     contFunction++;
}

void AndarVarrer(){
     do{
     ANDAR_FRENTE;
     }until(DIST_FRONT<20);
     PARAR;
     contFunction++;
}
void AndarAteLinhaQuadradoRe(){
     SendCableMessage(andarAteLinhaQuadradoRe);
     string terminou;
     do{
                 TextOut(0,LCD_LINE2,"JOAO");
       terminou = ReceiveCableMessage();
       TextOut(0,LCD_LINE2,"JOAO");
     }until (terminou == ok);
     contFunction++;
}
void AndarAteLinhaPretaRe(){
     SendCableMessage(andarAteLinhaPretaRe);
     string terminou;
     do{
       terminou= ReceiveCableMessage();
     }until (terminou == ok || DIST_FRONT>70);
     contFunction++;
}


void ReParaJogar(){
     ANDAR_TRAS;
     Wait(1650);
     contFunction++;
}

void ArremessarBolinhas(){
     PARAR;
     JOGAR;
     Wait(100);
     DESCER;
     contFunction++;
}


void ViraNorteEsquerdoHorario(){
     SendCableMessage(viraNorteEsquerdoHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraNorteEsquerdoAntiHorario(){
     SendCableMessage(viraNorteEsquerdoAntiHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraLesteEsquerdoHorario(){
     SendCableMessage(viraLesteEsquerdoHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraLesteEsquerdoAntiHorario(){
     SendCableMessage(viraLesteEsquerdoAntiHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraOesteEsquerdoHorario(){
     SendCableMessage(viraOesteEsquerdoHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraOesteEsquerdoAntiHorario(){
     SendCableMessage(viraOesteEsquerdoAntiHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraSulEsquerdoHorario(){
     SendCableMessage(viraSulEsquerdoHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraSulEsquerdoAntiHorario(){
     SendCableMessage(viraSulEsquerdoAntiHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraNorteDireitoHorario(){
     SendCableMessage(viraNorteDireitoHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraNorteDireitoAntiHorario(){
     SendCableMessage(viraNorteDireitoAntiHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraLesteDireitoHorario(){
     SendCableMessage(viraLesteDireitoHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraLesteDireitoAntiHorario(){
     SendCableMessage(viraLesteDireitoAntiHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraOesteDireitoHorario(){
     SendCableMessage(viraOesteDireitoHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraOesteDireitoAntiHorario(){
     SendCableMessage(viraOesteDireitoAntiHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraSulDireitoHorario(){
     SendCableMessage(viraSulDireitoHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void ViraSulDireitoAntiHorario(){
     SendCableMessage(viraSulDireitoAntiHorario);
     string terminou;
     do{
       terminou = ReceiveCableMessage();
     }until (terminou == ok);
     contFunction++;
}

void VirarAteLinhaEsq(){
     esperandoMensagem = true;
     VIRAR_ESQ;
     until(LUZ_ESQ<limiar);
     PARAR;
     contFunction++;
     esperandoMensagem = false;
}

void VirarAteLinhaDir(){
     esperandoMensagem = true;
     VIRAR_DIR;
     until(LUZ_DIR<limiar);
     PARAR;
     contFunction++;
     esperandoMensagem = false;
}


void SeguirLinhaEsq(){
     TEMPO_ERRO = 15000;
     //ANDAR_FRENTE;
     while(DIST_FRONT>12){
       if(LUZ_ESQ>limiar){
            //VIRAR_DIR;
            NumOut(0,LCD_LINE1,LUZ_ESQ);
               NumOut(0,LCD_LINE2,LUZ_DIR);
       }else{
            //VIRAR_ESQ;
            NumOut(0,LCD_LINE1,LUZ_ESQ);
               NumOut(0,LCD_LINE2,LUZ_DIR);
       }
     }
     PARAR;
     contFunction++;
     TEMPO_ERRO = 7000;
}

void SeguirLinhaDir(){
     TEMPO_ERRO = 15000;
     ANDAR_FRENTE;
     while(DIST_FRONT>12){
       if(LUZ_DIR>limiar){
            VIRAR_ESQ;
                        NumOut(0,LCD_LINE1,LUZ_ESQ);
               NumOut(0,LCD_LINE2,LUZ_DIR);
       }else{
            VIRAR_DIR;
                        NumOut(0,LCD_LINE1,LUZ_ESQ);
               NumOut(0,LCD_LINE2,LUZ_DIR);
       }
     }
     PARAR;
     contFunction++;
     TEMPO_ERRO = 7000;
}

void Re(int time){
     ANDAR_TRAS;
     Wait(time);
     PARAR;
     contFunction++;
}

bool estaConectado(){
     byte e = BluetoothStatus(0);
            if(e == 0){
                 return true;
                 }else{
                       return false;
                 }
}

void ChecarVarreVarre(){
     string msg = "T";
     slavecheck();
     sendtomaster(msg);
     string r;
     esperandoMensagem=true;
     r="";
     do{
        r = receivefrommaster();
     }until(r == "V");
     r="";
     esperandoMensagem=false;
     contFunction++;
}

void Andar(int time){
     ANDAR_FRENTE;
     Wait(time);
     PARAR;
     contFunction++;
}