Luiz Fernando Nobre
Versao que rodou durante a competicao
Dependencies: RadioIn Servo mbed
Fork of
Sumo_v2_Unstable
by 
Luiz Fernando Nobre
Revision 2:59c557d1230d, committed 2013-04-30
- Comitter:
- lfsantarelli
- Date:
- Tue Apr 30 19:50:05 2013 +0000
- Parent:
- 1:b11f1623539d
- Commit message:
- versao que competiu;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Apr 15 03:59:49 2013 +0000
+++ b/main.cpp Tue Apr 30 19:50:05 2013 +0000
@@ -2,10 +2,38 @@
#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
-saber1.write((m_esq/2)+0.5);
-saber2.write((m_esq/2)+0.5);
+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;
}
@@ -36,53 +64,70 @@
}
int LineCheck(){
- int result=0;
- result=!slFL*1000+100*!slRL+10*!slRR+!slFR;
+
+ 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(1); return 0;}
-
+ if (result==0 && anterior==1){anterior=0; wait(0); return 0;}
switch (result){
case 1:{ // Somente FR
- Drive(0.7,1);
+ Drive(-1,-1);
+ wait(0.15);
+ Drive(-1,1);
+ wait(0.15);
anterior=1;
break;
}
case 10:{ // Somente RR
- Drive(0.8,1);
+ //Drive(-1,1);
anterior=1;
+ //wait(0.05);
break;
}
case 100:{ // Somente RL
- Drive(1,0.8);
+ //Drive(1,-1);
anterior=1;
+ //wait(0.05);
break;
}
case 1000:{ // Somente FL
- Drive(1,0.7);
+ 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(1);
+ wait(0.15);
Drive(-1,1);
- wait(1); //Mesmo valor do wait final
+ 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(0,1);
+ Drive(-1,-1);
+ wait(0.15);
+ Drive(-1,1);
anterior=1;
break;
}
case 0011:{ // Os dois da esquerda
- Drive(1,0);
+ Drive(-1,-1);
+ wait(0.15);
+ Drive(1,-1);
anterior=1;
break;
}
@@ -99,28 +144,27 @@
while(detectados)
{
LineCheck();
-
switch(detectados){
case 1:{
- if(sd1){Drive(-1,1);}
- else if(sd2){Drive(0,1);}
+ if(sd5){Drive(-1,1);}
+ else if(sd4){Drive(0.25,1);}
else if(sd3){Drive(1,1);}
- else if(sd4){Drive(1,0);}
- else if(sd5){Drive(1,-1);};
+ else if(sd2){Drive(1,0.25);}
+ else if(sd1){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);}
+ 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&&sd2&&sd3){Drive(-1,1);}
+ if(sd1&&sd4&&sd3){Drive(-0.3,-0.7);}
else if(sd2&&sd3&&sd4){Drive(1,1);}
- else if(sd3&&sd4&&sd5){Drive(1,-1);}
+ else if(sd3&&sd2&&sd5){Drive(-0.7,-0.3);}
break;
}
default: {break;}
@@ -132,56 +176,56 @@
}
void Search(){
-
- Drive(0.7,0.7); //Anda reto até bater na linha
+
+ Drive(0.8,0.8); //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 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 1 (Inicia com o robo apontando 45graus (definir direito o angulo depois) pra direita e ele faz um L terminando no centro)
+ // 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);
- while(i<10){
+ Drive(1,1);
+ wait(0.03);
+ /*
+ while(y<3){
stop=LineCheck();
stop+=Chase();
if(stop){return;}
Drive(0.7,0.7);
- wait(0.1);
- i++;
+ wait(0.005);
+ y++;
+ Monitor();
}
- while(j<5){
+ while(z<3){
stop=LineCheck();
stop+=Chase();
if(stop){return;}
- Drive(-1,1);
- wait(0.1);
- j++;
- }
+ Drive(-0.7,0.7);
+ wait(0.005);
+ z++;
+ */
+ Monitor();
+
+
//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
+ //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(i<5){
+ while(y<3){
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++;
+ wait(0.005);
+ y++;
+ Monitor();
}
// Fim da estrategia 2
}
@@ -195,17 +239,21 @@
saber1.period_us(5);
saber2.period_us(5);
radio.Init();
+int started=0;
+
while(1){
- int started=0;
+ Monitor();
+ started=0;
while(ReadRadio(3)>0)
{ // Modo Autonomo
+ Monitor();
if(started==0){StartRoutine(); started=1;}
- while(LineCheck())
- {
- Chase();
- Search();
- }
+ LineCheck();
+ Chase();
+ Search();
}
DriveMixedRC();
+
}
+
}
\ No newline at end of file