Luiz Fernando Nobre / Mbed 2 deprecated Sumo_v2_RG13

Versao que rodou durante a competicao

Dependencies:   RadioIn Servo mbed

Fork of Sumo_v2_Unstable by Luiz Fernando Nobre

main.cpp@2:59c557d1230d, 2013-04-30 (annotated)

Committer:
lfsantarelli
Date:
Tue Apr 30 19:50:05 2013 +0000
Revision:
2:59c557d1230d
Parent:
1:b11f1623539d 
versao que competiu;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lfsantarelli 0:b59cb9ef5b5b 1 #include "Servo.h"
lfsantarelli 0:b59cb9ef5b5b 2 #include "mbed.h"
lfsantarelli 0:b59cb9ef5b5b 3 #include "RadioIn.h"
lfsantarelli 0:b59cb9ef5b5b 4 #include "Config.h"
lfsantarelli 2:59c557d1230d 5 int result=0;
lfsantarelli 0:b59cb9ef5b5b 6
lfsantarelli 2:59c557d1230d 7 //Saber2=> Motor esquerda Saber1=> Motor Direita
lfsantarelli 0:b59cb9ef5b5b 8 void Drive(float m_esq, float m_dir){ // Valores de -1 a 1 em cada motor
lfsantarelli 2:59c557d1230d 9 float gain=0.95;
lfsantarelli 2:59c557d1230d 10 float trime=-0.03;
lfsantarelli 2:59c557d1230d 11 float trimd=0;
lfsantarelli 2:59c557d1230d 12 saber2.write((-m_esq*gain/2)+0.5+trime);
lfsantarelli 2:59c557d1230d 13 saber1.write((-m_dir*gain/2)+0.5+trimd);
lfsantarelli 2:59c557d1230d 14 return;
lfsantarelli 2:59c557d1230d 15 }
lfsantarelli 2:59c557d1230d 16
lfsantarelli 2:59c557d1230d 17 void Monitor(){
lfsantarelli 2:59c557d1230d 18 int a=sd1;
lfsantarelli 2:59c557d1230d 19 int b=sd2;
lfsantarelli 2:59c557d1230d 20 int c=sd3;
lfsantarelli 2:59c557d1230d 21 int d=sd4;
lfsantarelli 2:59c557d1230d 22 int e=sd5;
lfsantarelli 2:59c557d1230d 23 int f=!slFR;
lfsantarelli 2:59c557d1230d 24 int g=!slRR;
lfsantarelli 2:59c557d1230d 25 int h=!slRL;
lfsantarelli 2:59c557d1230d 26 int i=!slFL;
lfsantarelli 2:59c557d1230d 27 int linha=!slFL*1000+100*!slRL+10*!slRR+!slFR;
lfsantarelli 2:59c557d1230d 28 float j=-(saber1.read()-0.5)*2;
lfsantarelli 2:59c557d1230d 29 float k=-(saber2.read()-0.5)*2;
lfsantarelli 2:59c557d1230d 30 float l=radio.chan3;
lfsantarelli 2:59c557d1230d 31
lfsantarelli 2:59c557d1230d 32 //pc.printf("sd1= %d|sd2=%d|sd3=%d|sd4=%d|sd5=%d",a,b,c,d,e);
lfsantarelli 2:59c557d1230d 33 pc.printf("Linha=%d|",linha);
lfsantarelli 2:59c557d1230d 34 pc.printf("|FR=%d|RR=%d|RL=%d|FL=%d|",f,g,h,i);
lfsantarelli 2:59c557d1230d 35 pc.printf("ME=%.3f|MD=%.3f|",k,j);
lfsantarelli 2:59c557d1230d 36 pc.printf("Ch3=%.1f\r",l);
lfsantarelli 1:b11f1623539d 37 return;
lfsantarelli 0:b59cb9ef5b5b 38 }
lfsantarelli 0:b59cb9ef5b5b 39
lfsantarelli 0:b59cb9ef5b5b 40 void DriveRC(){
lfsantarelli 0:b59cb9ef5b5b 41 float trim=0; // Trim para casar o 0 da sabertooth com o PWM do Mbed
lfsantarelli 0:b59cb9ef5b5b 42 radio.Update();
lfsantarelli 0:b59cb9ef5b5b 43 saber1.write((radio.chan1+1)/2+trim);
lfsantarelli 0:b59cb9ef5b5b 44 saber2.write((radio.chan2+1)/2+trim);
lfsantarelli 1:b11f1623539d 45 return;
lfsantarelli 0:b59cb9ef5b5b 46 }
lfsantarelli 0:b59cb9ef5b5b 47
lfsantarelli 0:b59cb9ef5b5b 48
lfsantarelli 0:b59cb9ef5b5b 49 float ReadRadio(int chan){ // Retorna o valor do canal entre -1 e 1
lfsantarelli 0:b59cb9ef5b5b 50 radio.Update();
lfsantarelli 0:b59cb9ef5b5b 51 if (chan==1){return radio.chan1;}
lfsantarelli 0:b59cb9ef5b5b 52 if (chan==2){return radio.chan2;}
lfsantarelli 0:b59cb9ef5b5b 53 if (chan==3){return radio.chan3;}
lfsantarelli 0:b59cb9ef5b5b 54 else return 0;
lfsantarelli 0:b59cb9ef5b5b 55 }
lfsantarelli 0:b59cb9ef5b5b 56
lfsantarelli 0:b59cb9ef5b5b 57
lfsantarelli 0:b59cb9ef5b5b 58 // Melhor usar o V-Tail Mixer e fazer o mix via hardware
lfsantarelli 0:b59cb9ef5b5b 59 void DriveMixedRC(){
lfsantarelli 0:b59cb9ef5b5b 60 radio.Update();
lfsantarelli 0:b59cb9ef5b5b 61 saber1.write(((radio.chan1+radio.chan2)+1)/2);
lfsantarelli 0:b59cb9ef5b5b 62 saber2.write(((radio.chan1-radio.chan2)+1)/2);
lfsantarelli 1:b11f1623539d 63 return;
lfsantarelli 1:b11f1623539d 64 }
lfsantarelli 1:b11f1623539d 65
lfsantarelli 1:b11f1623539d 66 int LineCheck(){
lfsantarelli 2:59c557d1230d 67
lfsantarelli 2:59c557d1230d 68 result=!slFL*1000+!slFR;
lfsantarelli 2:59c557d1230d 69 led1=!slFR;
lfsantarelli 2:59c557d1230d 70 led2=!slRR;
lfsantarelli 2:59c557d1230d 71 led3=!slRL;
lfsantarelli 2:59c557d1230d 72 led4=!slFL;
lfsantarelli 2:59c557d1230d 73
lfsantarelli 1:b11f1623539d 74 if (result==0 && anterior==0){return 0;}
lfsantarelli 2:59c557d1230d 75 if (result==0 && anterior==1){anterior=0; wait(0); return 0;}
lfsantarelli 1:b11f1623539d 76 switch (result){
lfsantarelli 1:b11f1623539d 77
lfsantarelli 1:b11f1623539d 78 case 1:{ // Somente FR
lfsantarelli 2:59c557d1230d 79 Drive(-1,-1);
lfsantarelli 2:59c557d1230d 80 wait(0.15);
lfsantarelli 2:59c557d1230d 81 Drive(-1,1);
lfsantarelli 2:59c557d1230d 82 wait(0.15);
lfsantarelli 1:b11f1623539d 83 anterior=1;
lfsantarelli 1:b11f1623539d 84 break;
lfsantarelli 1:b11f1623539d 85 }
lfsantarelli 1:b11f1623539d 86 case 10:{ // Somente RR
lfsantarelli 2:59c557d1230d 87 //Drive(-1,1);
lfsantarelli 1:b11f1623539d 88 anterior=1;
lfsantarelli 2:59c557d1230d 89 //wait(0.05);
lfsantarelli 1:b11f1623539d 90 break;
lfsantarelli 1:b11f1623539d 91 }
lfsantarelli 1:b11f1623539d 92 case 100:{ // Somente RL
lfsantarelli 2:59c557d1230d 93 //Drive(1,-1);
lfsantarelli 1:b11f1623539d 94 anterior=1;
lfsantarelli 2:59c557d1230d 95 //wait(0.05);
lfsantarelli 1:b11f1623539d 96 break;
lfsantarelli 1:b11f1623539d 97 }
lfsantarelli 1:b11f1623539d 98 case 1000:{ // Somente FL
lfsantarelli 2:59c557d1230d 99 Drive(-1,-1);
lfsantarelli 2:59c557d1230d 100 wait(0.15);
lfsantarelli 2:59c557d1230d 101 Drive(1,-1);
lfsantarelli 2:59c557d1230d 102 wait(0.15);
lfsantarelli 1:b11f1623539d 103 anterior=1;
lfsantarelli 1:b11f1623539d 104 break;
lfsantarelli 1:b11f1623539d 105 }
lfsantarelli 1:b11f1623539d 106 case 1001:{ // Os dois da frente
lfsantarelli 1:b11f1623539d 107 Drive(-1,-1);
lfsantarelli 2:59c557d1230d 108 wait(0.15);
lfsantarelli 1:b11f1623539d 109 Drive(-1,1);
lfsantarelli 2:59c557d1230d 110 wait(0.15); //Mesmo valor do wait final
lfsantarelli 1:b11f1623539d 111 anterior=1;
lfsantarelli 1:b11f1623539d 112 break;
lfsantarelli 1:b11f1623539d 113 }
lfsantarelli 1:b11f1623539d 114 case 0110:{ // Os dois de tras
lfsantarelli 1:b11f1623539d 115 Drive(1,1);
lfsantarelli 1:b11f1623539d 116 anterior=1;
lfsantarelli 2:59c557d1230d 117 wait(0.15);
lfsantarelli 1:b11f1623539d 118 break;
lfsantarelli 1:b11f1623539d 119 }
lfsantarelli 1:b11f1623539d 120 case 1100:{ // Os dois da direita
lfsantarelli 2:59c557d1230d 121 Drive(-1,-1);
lfsantarelli 2:59c557d1230d 122 wait(0.15);
lfsantarelli 2:59c557d1230d 123 Drive(-1,1);
lfsantarelli 1:b11f1623539d 124 anterior=1;
lfsantarelli 1:b11f1623539d 125 break;
lfsantarelli 1:b11f1623539d 126 }
lfsantarelli 1:b11f1623539d 127 case 0011:{ // Os dois da esquerda
lfsantarelli 2:59c557d1230d 128 Drive(-1,-1);
lfsantarelli 2:59c557d1230d 129 wait(0.15);
lfsantarelli 2:59c557d1230d 130 Drive(1,-1);
lfsantarelli 1:b11f1623539d 131 anterior=1;
lfsantarelli 1:b11f1623539d 132 break;
lfsantarelli 1:b11f1623539d 133 }
lfsantarelli 1:b11f1623539d 134 default: {anterior=1; break;}
lfsantarelli 1:b11f1623539d 135 }
lfsantarelli 1:b11f1623539d 136 return 1;
lfsantarelli 1:b11f1623539d 137 }
lfsantarelli 1:b11f1623539d 138
lfsantarelli 1:b11f1623539d 139 int Chase(){
lfsantarelli 1:b11f1623539d 140 int detectados=0;
lfsantarelli 1:b11f1623539d 141 detectados=sd1+sd2+sd3+sd4+sd5;
lfsantarelli 1:b11f1623539d 142 if (detectados==0){return 0;}
lfsantarelli 1:b11f1623539d 143
lfsantarelli 1:b11f1623539d 144 while(detectados)
lfsantarelli 1:b11f1623539d 145 {
lfsantarelli 1:b11f1623539d 146 LineCheck();
lfsantarelli 1:b11f1623539d 147 switch(detectados){
lfsantarelli 1:b11f1623539d 148
lfsantarelli 1:b11f1623539d 149 case 1:{
lfsantarelli 2:59c557d1230d 150 if(sd5){Drive(-1,1);}
lfsantarelli 2:59c557d1230d 151 else if(sd4){Drive(0.25,1);}
lfsantarelli 1:b11f1623539d 152 else if(sd3){Drive(1,1);}
lfsantarelli 2:59c557d1230d 153 else if(sd2){Drive(1,0.25);}
lfsantarelli 2:59c557d1230d 154 else if(sd1){Drive(1,-1);};
lfsantarelli 1:b11f1623539d 155 break;
lfsantarelli 1:b11f1623539d 156 }
lfsantarelli 1:b11f1623539d 157 case 2:{
lfsantarelli 2:59c557d1230d 158 if(sd5&&sd2){Drive(-0.7,-0.3);}
lfsantarelli 2:59c557d1230d 159 else if(sd4&&sd3){Drive(1,0.75);}
lfsantarelli 2:59c557d1230d 160 else if(sd3&&sd2){Drive(0.75,1);}
lfsantarelli 2:59c557d1230d 161 else if(sd1&&sd4){Drive(-0.3,-0.7);}
lfsantarelli 1:b11f1623539d 162 break;
lfsantarelli 1:b11f1623539d 163 }
lfsantarelli 1:b11f1623539d 164 case 3:{
lfsantarelli 2:59c557d1230d 165 if(sd1&&sd4&&sd3){Drive(-0.3,-0.7);}
lfsantarelli 1:b11f1623539d 166 else if(sd2&&sd3&&sd4){Drive(1,1);}
lfsantarelli 2:59c557d1230d 167 else if(sd3&&sd2&&sd5){Drive(-0.7,-0.3);}
lfsantarelli 1:b11f1623539d 168 break;
lfsantarelli 1:b11f1623539d 169 }
lfsantarelli 1:b11f1623539d 170 default: {break;}
lfsantarelli 1:b11f1623539d 171 }
lfsantarelli 1:b11f1623539d 172 detectados=sd1+sd2+sd3+sd4+sd5;
lfsantarelli 1:b11f1623539d 173 }
lfsantarelli 1:b11f1623539d 174
lfsantarelli 1:b11f1623539d 175 return 1;
lfsantarelli 1:b11f1623539d 176 }
lfsantarelli 1:b11f1623539d 177
lfsantarelli 1:b11f1623539d 178 void Search(){
lfsantarelli 2:59c557d1230d 179
lfsantarelli 2:59c557d1230d 180 Drive(0.8,0.8); //Anda reto até bater na linha
lfsantarelli 1:b11f1623539d 181 return;
lfsantarelli 1:b11f1623539d 182 }
lfsantarelli 1:b11f1623539d 183
lfsantarelli 1:b11f1623539d 184 void StartRoutine(){
lfsantarelli 2:59c557d1230d 185 int y=0; //Parametro incremental para quebrar os wait times em tempos pequenos
lfsantarelli 2:59c557d1230d 186 int z=0; //Parametro incremental para quebrar os wait times em tempos pequenos
lfsantarelli 1:b11f1623539d 187 int stop=0; //Quebra a rotina caso ache o oponente ou a linha
lfsantarelli 1:b11f1623539d 188 //Rotina de inicio de partida
lfsantarelli 1:b11f1623539d 189 if (sSwitch==1){
lfsantarelli 2:59c557d1230d 190 // 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)
lfsantarelli 1:b11f1623539d 191 wait(5);
lfsantarelli 2:59c557d1230d 192 Drive(1,1);
lfsantarelli 2:59c557d1230d 193 wait(0.03);
lfsantarelli 2:59c557d1230d 194 /*
lfsantarelli 2:59c557d1230d 195 while(y<3){
lfsantarelli 1:b11f1623539d 196 stop=LineCheck();
lfsantarelli 1:b11f1623539d 197 stop+=Chase();
lfsantarelli 1:b11f1623539d 198 if(stop){return;}
lfsantarelli 1:b11f1623539d 199 Drive(0.7,0.7);
lfsantarelli 2:59c557d1230d 200 wait(0.005);
lfsantarelli 2:59c557d1230d 201 y++;
lfsantarelli 2:59c557d1230d 202 Monitor();
lfsantarelli 1:b11f1623539d 203 }
lfsantarelli 2:59c557d1230d 204 while(z<3){
lfsantarelli 1:b11f1623539d 205 stop=LineCheck();
lfsantarelli 1:b11f1623539d 206 stop+=Chase();
lfsantarelli 1:b11f1623539d 207 if(stop){return;}
lfsantarelli 2:59c557d1230d 208 Drive(-0.7,0.7);
lfsantarelli 2:59c557d1230d 209 wait(0.005);
lfsantarelli 2:59c557d1230d 210 z++;
lfsantarelli 2:59c557d1230d 211 */
lfsantarelli 2:59c557d1230d 212 Monitor();
lfsantarelli 2:59c557d1230d 213
lfsantarelli 2:59c557d1230d 214
lfsantarelli 1:b11f1623539d 215 //Fim da Estrategia 1
lfsantarelli 1:b11f1623539d 216 }
lfsantarelli 1:b11f1623539d 217 else
lfsantarelli 1:b11f1623539d 218 {
lfsantarelli 2:59c557d1230d 219 //Inicio da Estrategia 1 (Inicia com o robo na mesma posicao da estrategia 1 e ele vira de cara e vai reto pro centro
lfsantarelli 1:b11f1623539d 220 wait(5);
lfsantarelli 2:59c557d1230d 221 while(y<3){
lfsantarelli 1:b11f1623539d 222 stop=LineCheck();
lfsantarelli 1:b11f1623539d 223 stop+=Chase();
lfsantarelli 1:b11f1623539d 224 if(stop){return;}
lfsantarelli 1:b11f1623539d 225 Drive(-1,1);
lfsantarelli 2:59c557d1230d 226 wait(0.005);
lfsantarelli 2:59c557d1230d 227 y++;
lfsantarelli 2:59c557d1230d 228 Monitor();
lfsantarelli 1:b11f1623539d 229 }
lfsantarelli 1:b11f1623539d 230 // Fim da estrategia 2
lfsantarelli 1:b11f1623539d 231 }
lfsantarelli 1:b11f1623539d 232
lfsantarelli 1:b11f1623539d 233
lfsantarelli 1:b11f1623539d 234 return;
lfsantarelli 0:b59cb9ef5b5b 235 }
lfsantarelli 0:b59cb9ef5b5b 236
lfsantarelli 0:b59cb9ef5b5b 237 int main()
lfsantarelli 0:b59cb9ef5b5b 238 {
lfsantarelli 0:b59cb9ef5b5b 239 saber1.period_us(5);
lfsantarelli 0:b59cb9ef5b5b 240 saber2.period_us(5);
lfsantarelli 0:b59cb9ef5b5b 241 radio.Init();
lfsantarelli 2:59c557d1230d 242 int started=0;
lfsantarelli 2:59c557d1230d 243
lfsantarelli 0:b59cb9ef5b5b 244 while(1){
lfsantarelli 2:59c557d1230d 245 Monitor();
lfsantarelli 2:59c557d1230d 246 started=0;
lfsantarelli 1:b11f1623539d 247 while(ReadRadio(3)>0)
lfsantarelli 1:b11f1623539d 248 { // Modo Autonomo
lfsantarelli 2:59c557d1230d 249 Monitor();
lfsantarelli 1:b11f1623539d 250 if(started==0){StartRoutine(); started=1;}
lfsantarelli 2:59c557d1230d 251 LineCheck();
lfsantarelli 2:59c557d1230d 252 Chase();
lfsantarelli 2:59c557d1230d 253 Search();
lfsantarelli 1:b11f1623539d 254 }
lfsantarelli 1:b11f1623539d 255 DriveMixedRC();
lfsantarelli 2:59c557d1230d 256
lfsantarelli 0:b59cb9ef5b5b 257 }
lfsantarelli 2:59c557d1230d 258
lfsantarelli 0:b59cb9ef5b5b 259 }