File content as of revision 2:59c557d1230d:

#include "Servo.h"
#include "mbed.h"
#include "RadioIn.h"
#include "Config.h"
int result=0;

//Saber2=> Motor esquerda Saber1=> Motor Direita
void Drive(float m_esq, float m_dir){ // Valores de -1 a 1 em cada motor
float gain=0.95;
float trime=-0.03;
float trimd=0;
saber2.write((-m_esq*gain/2)+0.5+trime); 
saber1.write((-m_dir*gain/2)+0.5+trimd);
return;
}

void Monitor(){
int a=sd1;
int b=sd2;
int c=sd3;
int d=sd4;
int e=sd5;
int f=!slFR;
int g=!slRR;
int h=!slRL;
int i=!slFL;
int linha=!slFL*1000+100*!slRL+10*!slRR+!slFR;
float j=-(saber1.read()-0.5)*2;
float k=-(saber2.read()-0.5)*2;
float l=radio.chan3;

//pc.printf("sd1= %d|sd2=%d|sd3=%d|sd4=%d|sd5=%d",a,b,c,d,e);
pc.printf("Linha=%d|",linha);
pc.printf("|FR=%d|RR=%d|RL=%d|FL=%d|",f,g,h,i);
pc.printf("ME=%.3f|MD=%.3f|",k,j);
pc.printf("Ch3=%.1f\r",l);
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(){
    
    result=!slFL*1000+!slFR;
    led1=!slFR; 
    led2=!slRR;
    led3=!slRL;
    led4=!slFL;
    
    if (result==0 && anterior==0){return 0;}
    if (result==0 && anterior==1){anterior=0; wait(0); return 0;}
    switch (result){

    case 1:{ // Somente FR
             Drive(-1,-1);
             wait(0.15);
             Drive(-1,1);
             wait(0.15);
             anterior=1;
             break;
    }
    case 10:{ // Somente RR
             //Drive(-1,1);
             anterior=1;
             //wait(0.05);
             break;
    }
    case 100:{ // Somente RL
             //Drive(1,-1);
             anterior=1;
             //wait(0.05);
             break;
    }
    case 1000:{ // Somente FL
             Drive(-1,-1);
             wait(0.15);
             Drive(1,-1);
             wait(0.15);
             anterior=1;
             break;
    }
    case 1001:{ // Os dois da frente
             Drive(-1,-1);
             wait(0.15);
             Drive(-1,1);
             wait(0.15); //Mesmo valor do wait final
             anterior=1;
             break;
    }
    case 0110:{ // Os dois de tras
             Drive(1,1);
             anterior=1;
             wait(0.15);
             break;
    }
    case 1100:{ // Os dois da direita
             Drive(-1,-1);
             wait(0.15);
             Drive(-1,1);
             anterior=1;
             break;
    }
    case 0011:{ // Os dois da esquerda
             Drive(-1,-1);
             wait(0.15);
             Drive(1,-1);
             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(sd5){Drive(-1,1);}
          else if(sd4){Drive(0.25,1);}
          else if(sd3){Drive(1,1);}
          else if(sd2){Drive(1,0.25);}
          else if(sd1){Drive(1,-1);};
          break;
     }
     case 2:{
          if(sd5&&sd2){Drive(-0.7,-0.3);}
          else if(sd4&&sd3){Drive(1,0.75);}
          else if(sd3&&sd2){Drive(0.75,1);}
          else if(sd1&&sd4){Drive(-0.3,-0.7);}
          break;
     }
     case 3:{
          if(sd1&&sd4&&sd3){Drive(-0.3,-0.7);}
          else if(sd2&&sd3&&sd4){Drive(1,1);}
          else if(sd3&&sd2&&sd5){Drive(-0.7,-0.3);}
          break;
          }
     default: {break;}
     }
     detectados=sd1+sd2+sd3+sd4+sd5;
    }

    return 1;
}

void Search(){
     
     Drive(0.8,0.8);  //Anda reto até bater na linha
    return;
}

void StartRoutine(){
int y=0;    //Parametro incremental para quebrar os wait times em tempos pequenos
int z=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 2 (Inicia com o robo apontando 45graus (definir direito o angulo depois) pra direita e ele faz um L terminando no centro)
    wait(5);
    Drive(1,1);
    wait(0.03);
    /*
    while(y<3){
        stop=LineCheck();
        stop+=Chase();
        if(stop){return;}
        Drive(0.7,0.7);
        wait(0.005);
        y++;
        Monitor();
    }
        while(z<3){
        stop=LineCheck();
        stop+=Chase();
        if(stop){return;}
        Drive(-0.7,0.7);
        wait(0.005);
        z++;
        */
        Monitor();
        
    
  //Fim da Estrategia 1
}
else
{
    //Inicio da Estrategia 1 (Inicia com o robo na mesma posicao da estrategia 1 e ele vira de cara e vai reto pro centro
    wait(5);
    while(y<3){
        stop=LineCheck();
        stop+=Chase();
        if(stop){return;}
        Drive(-1,1);
        wait(0.005);
        y++;
        Monitor();
    }
    // Fim da estrategia 2
}


 return;
}

int main()
{
saber1.period_us(5);
saber2.period_us(5);
radio.Init();
int started=0;

while(1){
        Monitor();
        started=0;
        while(ReadRadio(3)>0)
        { // Modo Autonomo
              Monitor();
              if(started==0){StartRoutine(); started=1;}
              LineCheck();
              Chase();
              Search();
        }
             DriveMixedRC();
         
        }
        
}