Luiz Fernando Nobre / Mbed 2 deprecated Sumo_v2_Unstable

Unstable experimental version of the Mbed interface

Fork of Sumo_v2 by Luiz Fernando Nobre

main.cpp@1:b11f1623539d, 2013-04-15 (annotated)

Committer:
lfsantarelli
Date:
Mon Apr 15 03:59:49 2013 +0000
Revision:
1:b11f1623539d
Parent:
0:b59cb9ef5b5b 
0.2

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 0:b59cb9ef5b5b 5
lfsantarelli 0:b59cb9ef5b5b 6 void Drive(float m_esq, float m_dir){ // Valores de -1 a 1 em cada motor
lfsantarelli 0:b59cb9ef5b5b 7 saber1.write((m_esq/2)+0.5);
lfsantarelli 0:b59cb9ef5b5b 8 saber2.write((m_esq/2)+0.5);
lfsantarelli 1:b11f1623539d 9 return;
lfsantarelli 0:b59cb9ef5b5b 10 }
lfsantarelli 0:b59cb9ef5b5b 11
lfsantarelli 0:b59cb9ef5b5b 12 void DriveRC(){
lfsantarelli 0:b59cb9ef5b5b 13 float trim=0; // Trim para casar o 0 da sabertooth com o PWM do Mbed
lfsantarelli 0:b59cb9ef5b5b 14 radio.Update();
lfsantarelli 0:b59cb9ef5b5b 15 saber1.write((radio.chan1+1)/2+trim);
lfsantarelli 0:b59cb9ef5b5b 16 saber2.write((radio.chan2+1)/2+trim);
lfsantarelli 1:b11f1623539d 17 return;
lfsantarelli 0:b59cb9ef5b5b 18 }
lfsantarelli 0:b59cb9ef5b5b 19
lfsantarelli 0:b59cb9ef5b5b 20
lfsantarelli 0:b59cb9ef5b5b 21 float ReadRadio(int chan){ // Retorna o valor do canal entre -1 e 1
lfsantarelli 0:b59cb9ef5b5b 22 radio.Update();
lfsantarelli 0:b59cb9ef5b5b 23 if (chan==1){return radio.chan1;}
lfsantarelli 0:b59cb9ef5b5b 24 if (chan==2){return radio.chan2;}
lfsantarelli 0:b59cb9ef5b5b 25 if (chan==3){return radio.chan3;}
lfsantarelli 0:b59cb9ef5b5b 26 else return 0;
lfsantarelli 0:b59cb9ef5b5b 27 }
lfsantarelli 0:b59cb9ef5b5b 28
lfsantarelli 0:b59cb9ef5b5b 29
lfsantarelli 0:b59cb9ef5b5b 30 // Melhor usar o V-Tail Mixer e fazer o mix via hardware
lfsantarelli 0:b59cb9ef5b5b 31 void DriveMixedRC(){
lfsantarelli 0:b59cb9ef5b5b 32 radio.Update();
lfsantarelli 0:b59cb9ef5b5b 33 saber1.write(((radio.chan1+radio.chan2)+1)/2);
lfsantarelli 0:b59cb9ef5b5b 34 saber2.write(((radio.chan1-radio.chan2)+1)/2);
lfsantarelli 1:b11f1623539d 35 return;
lfsantarelli 1:b11f1623539d 36 }
lfsantarelli 1:b11f1623539d 37
lfsantarelli 1:b11f1623539d 38 int LineCheck(){
lfsantarelli 1:b11f1623539d 39 int result=0;
lfsantarelli 1:b11f1623539d 40 result=!slFL*1000+100*!slRL+10*!slRR+!slFR;
lfsantarelli 1:b11f1623539d 41 if (result==0 && anterior==0){return 0;}
lfsantarelli 1:b11f1623539d 42 if (result==0 && anterior==1){anterior=0; wait(1); return 0;}
lfsantarelli 1:b11f1623539d 43
lfsantarelli 1:b11f1623539d 44 switch (result){
lfsantarelli 1:b11f1623539d 45
lfsantarelli 1:b11f1623539d 46 case 1:{ // Somente FR
lfsantarelli 1:b11f1623539d 47 Drive(0.7,1);
lfsantarelli 1:b11f1623539d 48 anterior=1;
lfsantarelli 1:b11f1623539d 49 break;
lfsantarelli 1:b11f1623539d 50 }
lfsantarelli 1:b11f1623539d 51 case 10:{ // Somente RR
lfsantarelli 1:b11f1623539d 52 Drive(0.8,1);
lfsantarelli 1:b11f1623539d 53 anterior=1;
lfsantarelli 1:b11f1623539d 54 break;
lfsantarelli 1:b11f1623539d 55 }
lfsantarelli 1:b11f1623539d 56 case 100:{ // Somente RL
lfsantarelli 1:b11f1623539d 57 Drive(1,0.8);
lfsantarelli 1:b11f1623539d 58 anterior=1;
lfsantarelli 1:b11f1623539d 59 break;
lfsantarelli 1:b11f1623539d 60 }
lfsantarelli 1:b11f1623539d 61 case 1000:{ // Somente FL
lfsantarelli 1:b11f1623539d 62 Drive(1,0.7);
lfsantarelli 1:b11f1623539d 63 anterior=1;
lfsantarelli 1:b11f1623539d 64 break;
lfsantarelli 1:b11f1623539d 65 }
lfsantarelli 1:b11f1623539d 66 case 1001:{ // Os dois da frente
lfsantarelli 1:b11f1623539d 67 Drive(-1,-1);
lfsantarelli 1:b11f1623539d 68 wait(1);
lfsantarelli 1:b11f1623539d 69 Drive(-1,1);
lfsantarelli 1:b11f1623539d 70 wait(1); //Mesmo valor do wait final
lfsantarelli 1:b11f1623539d 71 anterior=1;
lfsantarelli 1:b11f1623539d 72 break;
lfsantarelli 1:b11f1623539d 73 }
lfsantarelli 1:b11f1623539d 74 case 0110:{ // Os dois de tras
lfsantarelli 1:b11f1623539d 75 Drive(1,1);
lfsantarelli 1:b11f1623539d 76 anterior=1;
lfsantarelli 1:b11f1623539d 77 break;
lfsantarelli 1:b11f1623539d 78 }
lfsantarelli 1:b11f1623539d 79 case 1100:{ // Os dois da direita
lfsantarelli 1:b11f1623539d 80 Drive(0,1);
lfsantarelli 1:b11f1623539d 81 anterior=1;
lfsantarelli 1:b11f1623539d 82 break;
lfsantarelli 1:b11f1623539d 83 }
lfsantarelli 1:b11f1623539d 84 case 0011:{ // Os dois da esquerda
lfsantarelli 1:b11f1623539d 85 Drive(1,0);
lfsantarelli 1:b11f1623539d 86 anterior=1;
lfsantarelli 1:b11f1623539d 87 break;
lfsantarelli 1:b11f1623539d 88 }
lfsantarelli 1:b11f1623539d 89 default: {anterior=1; break;}
lfsantarelli 1:b11f1623539d 90 }
lfsantarelli 1:b11f1623539d 91 return 1;
lfsantarelli 1:b11f1623539d 92 }
lfsantarelli 1:b11f1623539d 93
lfsantarelli 1:b11f1623539d 94 int Chase(){
lfsantarelli 1:b11f1623539d 95 int detectados=0;
lfsantarelli 1:b11f1623539d 96 detectados=sd1+sd2+sd3+sd4+sd5;
lfsantarelli 1:b11f1623539d 97 if (detectados==0){return 0;}
lfsantarelli 1:b11f1623539d 98
lfsantarelli 1:b11f1623539d 99 while(detectados)
lfsantarelli 1:b11f1623539d 100 {
lfsantarelli 1:b11f1623539d 101 LineCheck();
lfsantarelli 1:b11f1623539d 102
lfsantarelli 1:b11f1623539d 103 switch(detectados){
lfsantarelli 1:b11f1623539d 104
lfsantarelli 1:b11f1623539d 105 case 1:{
lfsantarelli 1:b11f1623539d 106 if(sd1){Drive(-1,1);}
lfsantarelli 1:b11f1623539d 107 else if(sd2){Drive(0,1);}
lfsantarelli 1:b11f1623539d 108 else if(sd3){Drive(1,1);}
lfsantarelli 1:b11f1623539d 109 else if(sd4){Drive(1,0);}
lfsantarelli 1:b11f1623539d 110 else if(sd5){Drive(1,-1);};
lfsantarelli 1:b11f1623539d 111 break;
lfsantarelli 1:b11f1623539d 112 }
lfsantarelli 1:b11f1623539d 113 case 2:{
lfsantarelli 1:b11f1623539d 114 if(sd1&&sd2){Drive(-0.5,1);}
lfsantarelli 1:b11f1623539d 115 else if(sd2&&sd3){Drive(0.5,1);}
lfsantarelli 1:b11f1623539d 116 else if(sd3&&sd4){Drive(1,0.5);}
lfsantarelli 1:b11f1623539d 117 else if(sd4&&sd5){Drive(1,-0.5);}
lfsantarelli 1:b11f1623539d 118 break;
lfsantarelli 1:b11f1623539d 119 }
lfsantarelli 1:b11f1623539d 120 case 3:{
lfsantarelli 1:b11f1623539d 121 if(sd1&&sd2&&sd3){Drive(-1,1);}
lfsantarelli 1:b11f1623539d 122 else if(sd2&&sd3&&sd4){Drive(1,1);}
lfsantarelli 1:b11f1623539d 123 else if(sd3&&sd4&&sd5){Drive(1,-1);}
lfsantarelli 1:b11f1623539d 124 break;
lfsantarelli 1:b11f1623539d 125 }
lfsantarelli 1:b11f1623539d 126 default: {break;}
lfsantarelli 1:b11f1623539d 127 }
lfsantarelli 1:b11f1623539d 128 detectados=sd1+sd2+sd3+sd4+sd5;
lfsantarelli 1:b11f1623539d 129 }
lfsantarelli 1:b11f1623539d 130
lfsantarelli 1:b11f1623539d 131 return 1;
lfsantarelli 1:b11f1623539d 132 }
lfsantarelli 1:b11f1623539d 133
lfsantarelli 1:b11f1623539d 134 void Search(){
lfsantarelli 1:b11f1623539d 135
lfsantarelli 1:b11f1623539d 136 Drive(0.7,0.7); //Anda reto até bater na linha
lfsantarelli 1:b11f1623539d 137 return;
lfsantarelli 1:b11f1623539d 138 }
lfsantarelli 1:b11f1623539d 139
lfsantarelli 1:b11f1623539d 140 void StartRoutine(){
lfsantarelli 1:b11f1623539d 141 int i=0; //Parametro incremental para quebrar os wait times em tempos pequenos
lfsantarelli 1:b11f1623539d 142 int j=0; //Parametro incremental para quebrar os wait times em tempos pequenos
lfsantarelli 1:b11f1623539d 143 int stop=0; //Quebra a rotina caso ache o oponente ou a linha
lfsantarelli 1:b11f1623539d 144 //Rotina de inicio de partida
lfsantarelli 1:b11f1623539d 145 if (sSwitch==1){
lfsantarelli 1:b11f1623539d 146 // 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)
lfsantarelli 1:b11f1623539d 147 wait(5);
lfsantarelli 1:b11f1623539d 148 while(i<10){
lfsantarelli 1:b11f1623539d 149 stop=LineCheck();
lfsantarelli 1:b11f1623539d 150 stop+=Chase();
lfsantarelli 1:b11f1623539d 151 if(stop){return;}
lfsantarelli 1:b11f1623539d 152 Drive(0.7,0.7);
lfsantarelli 1:b11f1623539d 153 wait(0.1);
lfsantarelli 1:b11f1623539d 154 i++;
lfsantarelli 1:b11f1623539d 155 }
lfsantarelli 1:b11f1623539d 156 while(j<5){
lfsantarelli 1:b11f1623539d 157 stop=LineCheck();
lfsantarelli 1:b11f1623539d 158 stop+=Chase();
lfsantarelli 1:b11f1623539d 159 if(stop){return;}
lfsantarelli 1:b11f1623539d 160 Drive(-1,1);
lfsantarelli 1:b11f1623539d 161 wait(0.1);
lfsantarelli 1:b11f1623539d 162 j++;
lfsantarelli 1:b11f1623539d 163 }
lfsantarelli 1:b11f1623539d 164 //Fim da Estrategia 1
lfsantarelli 1:b11f1623539d 165 }
lfsantarelli 1:b11f1623539d 166 else
lfsantarelli 1:b11f1623539d 167 {
lfsantarelli 1:b11f1623539d 168 //Inicio da Estrategia 2 (Inicia com o robo na mesma posicao da estrategia 1 e ele vira de cara e vai reto pro centro
lfsantarelli 1:b11f1623539d 169 wait(5);
lfsantarelli 1:b11f1623539d 170 while(i<5){
lfsantarelli 1:b11f1623539d 171 stop=LineCheck();
lfsantarelli 1:b11f1623539d 172 stop+=Chase();
lfsantarelli 1:b11f1623539d 173 if(stop){return;}
lfsantarelli 1:b11f1623539d 174 Drive(-1,1);
lfsantarelli 1:b11f1623539d 175 wait(0.1);
lfsantarelli 1:b11f1623539d 176 i++;
lfsantarelli 1:b11f1623539d 177 }
lfsantarelli 1:b11f1623539d 178 while(j<10){
lfsantarelli 1:b11f1623539d 179 stop=LineCheck();
lfsantarelli 1:b11f1623539d 180 stop+=Chase();
lfsantarelli 1:b11f1623539d 181 if(stop){return;}
lfsantarelli 1:b11f1623539d 182 Drive(0.7,0.7);
lfsantarelli 1:b11f1623539d 183 wait(0.1);
lfsantarelli 1:b11f1623539d 184 j++;
lfsantarelli 1:b11f1623539d 185 }
lfsantarelli 1:b11f1623539d 186 // Fim da estrategia 2
lfsantarelli 1:b11f1623539d 187 }
lfsantarelli 1:b11f1623539d 188
lfsantarelli 1:b11f1623539d 189
lfsantarelli 1:b11f1623539d 190 return;
lfsantarelli 0:b59cb9ef5b5b 191 }
lfsantarelli 0:b59cb9ef5b5b 192
lfsantarelli 0:b59cb9ef5b5b 193 int main()
lfsantarelli 0:b59cb9ef5b5b 194 {
lfsantarelli 0:b59cb9ef5b5b 195 saber1.period_us(5);
lfsantarelli 0:b59cb9ef5b5b 196 saber2.period_us(5);
lfsantarelli 0:b59cb9ef5b5b 197 radio.Init();
lfsantarelli 0:b59cb9ef5b5b 198 while(1){
lfsantarelli 1:b11f1623539d 199 int started=0;
lfsantarelli 1:b11f1623539d 200 while(ReadRadio(3)>0)
lfsantarelli 1:b11f1623539d 201 { // Modo Autonomo
lfsantarelli 1:b11f1623539d 202 if(started==0){StartRoutine(); started=1;}
lfsantarelli 1:b11f1623539d 203 while(LineCheck())
lfsantarelli 1:b11f1623539d 204 {
lfsantarelli 1:b11f1623539d 205 Chase();
lfsantarelli 1:b11f1623539d 206 Search();
lfsantarelli 1:b11f1623539d 207 }
lfsantarelli 1:b11f1623539d 208 }
lfsantarelli 1:b11f1623539d 209 DriveMixedRC();
lfsantarelli 0:b59cb9ef5b5b 210 }
lfsantarelli 0:b59cb9ef5b5b 211 }