PIMP Cable - 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 = 50;

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 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(3000);
     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(){
     VIRAR_ESQ;
     until(LUZ_ESQ<limiar);
     PARAR;
     contFunction++;
}

void VirarAteLinhaDir(){
     VIRAR_DIR;
     until(LUZ_DIR<limiar);
     PARAR;
     contFunction++;
}


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++;
}

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

task comecarEsquerda(){
     ladoInicial = ESQUERDA;
     lado = ESQUERDA;
     AndarAteLinhaQuadradoRe();
     
     AndarAteLinhaPretaRe();

     ChecarVarreVarre();

     AndarAteParede();

     while(true){

     ReParaJogar();

     ArremessarBolinhas();

     AndarAteLinhaPretaRe();

     VirarAteLinhaEsq();

     SeguirLinhaEsq();


     lado = DIREITA;
     Re(500);

     ViraSulDireitoHorario();

     ArremessarBolinhas();

     AndarAteLinhaPretaRe();

     ViraOesteDireitoHorario();

     Re(1000);

     ViraSulDireitoAntiHorario();

     ChecarVarreVarre();

     AndarAteParede();

     ReParaJogar();

     ArremessarBolinhas();

     AndarAteLinhaPretaRe();

     VirarAteLinhaDir();

     SeguirLinhaDir();

     lado = ESQUERDA;
     Re(500);

     ViraSulEsquerdoAntiHorario();

     ArremessarBolinhas();

     AndarAteLinhaPretaRe();

     ViraLesteDireitoAntiHorario();

     Re(1000);

     ViraSulDireitoHorario();

     ChecarVarreVarre();

     AndarAteParede();

     }
}

task comecarDireita(){
     ladoInicial = DIREITA;
     lado = DIREITA;
     AndarAteLinhaQuadradoRe();

     AndarAteLinhaPretaRe();

     slavecheck();
     sendtomaster("T");
     ChecarVarreVarre();

     AndarAteParede();

     while(true){

     ReParaJogar();

     ArremessarBolinhas();

     AndarAteLinhaPretaRe();

     VirarAteLinhaDir();

     SeguirLinhaDir();

     lado = ESQUERDA;
     Re(500);

     ViraSulEsquerdoAntiHorario();

     ArremessarBolinhas();

     AndarAteLinhaPretaRe();

     ChecarVarreVarre();

     ViraLesteDireitoAntiHorario();

     Re(1000);

     ViraSulDireitoHorario();

     AndarAteParede();

     ReParaJogar();

     ArremessarBolinhas();

     AndarAteLinhaPretaRe();

     VirarAteLinhaEsq();

     SeguirLinhaEsq();
     
     lado = DIREITA;
     Re(500);

     ViraSulDireitoHorario();

     ArremessarBolinhas();

     AndarAteLinhaPretaRe();

     ChecarVarreVarre();

     ViraOesteDireitoHorario();

     Re(1000);

     ViraSulDireitoAntiHorario();

     AndarAteParede();
   }
}

task verificarErro(){
     int oldValue;
     long initialTime = CurrentTick();
     long currentTime = CurrentTick();
     oldValue = contFunction;
     while(true){
         currentTime = CurrentTick();
         if(oldValue==contFunction){
               if((currentTime - initialTime)>TEMPO_ERRO && !esperandoMensagem ){
                   PlayTone(1100,1000);
                   if(ladoInicial == DIREITA){
                       stop comecarDireita;
                   }
                   if(ladoInicial == ESQUERDA){
                       stop comecarEsquerda;
                   }
                   SendCableMessage(pararTudo);
                   string msg;
                   if(lado == ESQUERDA){
                           msg =  "E";
                   }if(lado == DIREITA){
                           msg = "D";
                           PlayFileEx("Direita.wav", 4, 0);
                   }
                        slavecheck();
                        sendtomaster(msg);
               }if(esperandoMensagem){
                   initialTime=CurrentTick();
               }
         }else{
               oldValue=contFunction;
               initialTime = CurrentTick();
         }
     }
}


task main(){
     ConfigureRS485();
     SetSensorLowspeed(SENSOR_DIST_FRONT);
     SetSensorLight(SENSOR_LUZ_ESQ);
     SetSensorLight(SENSOR_LUZ_DIR);
     PlayFileEx("Direita.wav", 4, 1);
     Precedes(comecarDireita,mexerPa);
     }