File content as of revision 1:b11f1623539d:

#include "Servo.h"
#include "mbed.h"
#include "RadioIn.h"
#include "Config.h"

void Drive(float m_esq, float m_dir){ // Valores de -1 a 1 em cada motor
saber1.write((m_esq/2)+0.5);
saber2.write((m_esq/2)+0.5);
return;
}

void DriveRC(){
            float trim=0; // Trim para casar o 0 da sabertooth com o PWM do Mbed
            radio.Update();
            saber1.write((radio.chan1+1)/2+trim);
            saber2.write((radio.chan2+1)/2+trim);
            return;
}


float ReadRadio(int chan){  // Retorna o valor do canal entre -1 e 1
radio.Update();
if (chan==1){return radio.chan1;}
if (chan==2){return radio.chan2;}
if (chan==3){return radio.chan3;}
else return 0;
}


// Melhor usar o V-Tail Mixer e fazer o mix via hardware
void DriveMixedRC(){            
            radio.Update();
            saber1.write(((radio.chan1+radio.chan2)+1)/2);
            saber2.write(((radio.chan1-radio.chan2)+1)/2);
            return;
}

int LineCheck(){
    int result=0;
    result=!slFL*1000+100*!slRL+10*!slRR+!slFR;
    if (result==0 && anterior==0){return 0;}
    if (result==0 && anterior==1){anterior=0; wait(1); return 0;}

    switch (result){

    case 1:{ // Somente FR
             Drive(0.7,1);
             anterior=1;
             break;
    }
    case 10:{ // Somente RR
             Drive(0.8,1);
             anterior=1;
             break;
    }
    case 100:{ // Somente RL
             Drive(1,0.8);
             anterior=1;
             break;
    }
    case 1000:{ // Somente FL
             Drive(1,0.7);
             anterior=1;
             break;
    }
    case 1001:{ // Os dois da frente
             Drive(-1,-1);
             wait(1);
             Drive(-1,1);
             wait(1); //Mesmo valor do wait final
             anterior=1;
             break;
    }
    case 0110:{ // Os dois de tras
             Drive(1,1);
             anterior=1;
             break;
    }
    case 1100:{ // Os dois da direita
             Drive(0,1);
             anterior=1;
             break;
    }
    case 0011:{ // Os dois da esquerda
             Drive(1,0);
             anterior=1;
             break;
    }
     default: {anterior=1; break;}
    }
    return 1;
}

int Chase(){
    int detectados=0;
    detectados=sd1+sd2+sd3+sd4+sd5;
    if (detectados==0){return 0;}

    while(detectados)
    {
     LineCheck();

     switch(detectados){

     case 1:{
          if(sd1){Drive(-1,1);}
          else if(sd2){Drive(0,1);}
          else if(sd3){Drive(1,1);}
          else if(sd4){Drive(1,0);}
          else if(sd5){Drive(1,-1);};
          break;
     }
     case 2:{
          if(sd1&&sd2){Drive(-0.5,1);}
          else if(sd2&&sd3){Drive(0.5,1);}
          else if(sd3&&sd4){Drive(1,0.5);}
          else if(sd4&&sd5){Drive(1,-0.5);}
          break;
     }
     case 3:{
          if(sd1&&sd2&&sd3){Drive(-1,1);}
          else if(sd2&&sd3&&sd4){Drive(1,1);}
          else if(sd3&&sd4&&sd5){Drive(1,-1);}
          break;
          }
     default: {break;}
     }
     detectados=sd1+sd2+sd3+sd4+sd5;
    }

    return 1;
}

void Search(){

     Drive(0.7,0.7);  //Anda reto até bater na linha
    return;
}

void StartRoutine(){
int i=0;    //Parametro incremental para quebrar os wait times em tempos pequenos
int j=0;    //Parametro incremental para quebrar os wait times em tempos pequenos
int stop=0; //Quebra a rotina caso ache o oponente ou a linha
//Rotina de inicio de partida
if (sSwitch==1){
 // Inicio da Estrategia 1 (Inicia com o robo apontando 45graus (definir direito o angulo depois) pra direita e ele faz um L terminando no centro)
    wait(5);
    while(i<10){
        stop=LineCheck();
        stop+=Chase();
        if(stop){return;}
        Drive(0.7,0.7);
        wait(0.1);
        i++;
    }
        while(j<5){
        stop=LineCheck();
        stop+=Chase();
        if(stop){return;}
        Drive(-1,1);
        wait(0.1);
        j++;
    }
  //Fim da Estrategia 1
}
else
{
    //Inicio da Estrategia 2 (Inicia com o robo na mesma posicao da estrategia 1 e ele vira de cara e vai reto pro centro
    wait(5);
    while(i<5){
        stop=LineCheck();
        stop+=Chase();
        if(stop){return;}
        Drive(-1,1);
        wait(0.1);
        i++;
    }
        while(j<10){
        stop=LineCheck();
        stop+=Chase();
        if(stop){return;}
        Drive(0.7,0.7);
        wait(0.1);
        j++;
    }
    // Fim da estrategia 2
}


 return;
}

int main()
{
saber1.period_us(5);
saber2.period_us(5);
radio.Init();
while(1){
        int started=0;
        while(ReadRadio(3)>0)
        { // Modo Autonomo
              if(started==0){StartRoutine(); started=1;}
              while(LineCheck())
              {
               Chase();
               Search();
              }
        }
             DriveMixedRC();
        }
}