Diff: main.cpp

Revision:

1:b11f1623539d

Parent:

0:b59cb9ef5b5b

--- a/main.cpp	Sat Apr 13 01:00:26 2013 +0000
+++ b/main.cpp	Mon Apr 15 03:59:49 2013 +0000
@@ -6,6 +6,7 @@
 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(){
@@ -13,6 +14,7 @@
             radio.Update();
             saber1.write((radio.chan1+1)/2+trim);
             saber2.write((radio.chan2+1)/2+trim);
+            return;
 }
 
 
@@ -30,6 +32,162 @@
             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()
@@ -38,6 +196,16 @@
 saber2.period_us(5);
 radio.Init();
 while(1){
-         DriveMixedRC();
+        int started=0;
+        while(ReadRadio(3)>0)
+        { // Modo Autonomo
+              if(started==0){StartRoutine(); started=1;}
+              while(LineCheck())
+              {
+               Chase();
+               Search();
+              }
+        }
+             DriveMixedRC();
         }
 }
\ No newline at end of file