RioBotz / Mbed 2 deprecated MiniSumo4Rodas

Codigo do miniSumo de 4 rodas

Dependencies:   mbed

main.cpp@0:0321883eb43b, 2020-08-11 (annotated)

Committer:
rperoba
Date:
Tue Aug 11 20:54:54 2020 +0000
Revision:
0:0321883eb43b
Codigo inicial do minisumo com 4 rodas

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rperoba 0:0321883eb43b 1 #include "mbed.h"
rperoba 0:0321883eb43b 2 #include "PwmOut.h"
rperoba 0:0321883eb43b 3
rperoba 0:0321883eb43b 4 #define PpmSuperMaximo 2000 //AjustarValores
rperoba 0:0321883eb43b 5 #define PpmMaximo 1800
rperoba 0:0321883eb43b 6 #define PpmPositivo2 1700
rperoba 0:0321883eb43b 7 #define PpmPositivo 1600
rperoba 0:0321883eb43b 8 #define PpmMedio 1500
rperoba 0:0321883eb43b 9 #define PpmNegativo 1400
rperoba 0:0321883eb43b 10 #define PpmNegativo2 1300
rperoba 0:0321883eb43b 11 #define PpmMinimo 1200
rperoba 0:0321883eb43b 12
rperoba 0:0321883eb43b 13 #define debug false
rperoba 0:0321883eb43b 14
rperoba 0:0321883eb43b 15 //Sensores de distância
rperoba 0:0321883eb43b 16 DigitalIn SensorEsquerdo (p16);
rperoba 0:0321883eb43b 17 DigitalIn SensorEsquerdoFrontal (p15);
rperoba 0:0321883eb43b 18 DigitalIn SensorFrontal (p14);
rperoba 0:0321883eb43b 19 DigitalIn SensorDireitoFrontal (p13);
rperoba 0:0321883eb43b 20 DigitalIn SensorDireito (p12);
rperoba 0:0321883eb43b 21
rperoba 0:0321883eb43b 22 //Sensores de linha
rperoba 0:0321883eb43b 23 InterruptIn LinhaEsquerda(p19);
rperoba 0:0321883eb43b 24 InterruptIn LinhaDireita(p20);
rperoba 0:0321883eb43b 25
rperoba 0:0321883eb43b 26 //Motores
rperoba 0:0321883eb43b 27 PwmOut MotoresEsquerdos(p21);
rperoba 0:0321883eb43b 28 PwmOut MotoresDireitos(p23);
rperoba 0:0321883eb43b 29
rperoba 0:0321883eb43b 30 //MicroStart
rperoba 0:0321883eb43b 31 InterruptIn MicroStart(p18);
rperoba 0:0321883eb43b 32
rperoba 0:0321883eb43b 33 //Bluetooth
rperoba 0:0321883eb43b 34 DigitalOut bluetoothState(p26);
rperoba 0:0321883eb43b 35 DigitalOut bluetoothEnable (p29);
rperoba 0:0321883eb43b 36 Serial bluetoothSerial (p28,p27);
rperoba 0:0321883eb43b 37
rperoba 0:0321883eb43b 38 //Variáveis
rperoba 0:0321883eb43b 39 int leituraDeDistancia = 0;
rperoba 0:0321883eb43b 40
rperoba 0:0321883eb43b 41 int comando=0;
rperoba 0:0321883eb43b 42 int ultimoPpmDireito;
rperoba 0:0321883eb43b 43 int ultimoPpmEsquerdo;
rperoba 0:0321883eb43b 44
rperoba 0:0321883eb43b 45 void Drive(int ppmEsquerdo,int ppmDireito);
rperoba 0:0321883eb43b 46 void SetarMotores ();
rperoba 0:0321883eb43b 47 void LerSensoresDeDistancia();
rperoba 0:0321883eb43b 48 void SensorDeLinhaLido();
rperoba 0:0321883eb43b 49 void MicroStartInterruptRise();
rperoba 0:0321883eb43b 50 void MicroStartInterruptFall();
rperoba 0:0321883eb43b 51 void Estrategia();
rperoba 0:0321883eb43b 52
rperoba 0:0321883eb43b 53 int main()
rperoba 0:0321883eb43b 54 {
rperoba 0:0321883eb43b 55 #if debug
rperoba 0:0321883eb43b 56
rperoba 0:0321883eb43b 57 bluetoothSerial.printf("Iniciando...");
rperoba 0:0321883eb43b 58
rperoba 0:0321883eb43b 59 #endif
rperoba 0:0321883eb43b 60 MicroStart.rise(&MicroStartInterruptRise);
rperoba 0:0321883eb43b 61 MicroStart.fall(&MicroStartInterruptFall);
rperoba 0:0321883eb43b 62 SetarMotores(); // Ajusta os motores
rperoba 0:0321883eb43b 63 LinhaDireita.rise(&SensorDeLinhaLido);//Interrupção para os sensores de linha
rperoba 0:0321883eb43b 64 LinhaEsquerda.rise(&SensorDeLinhaLido);
rperoba 0:0321883eb43b 65
rperoba 0:0321883eb43b 66
rperoba 0:0321883eb43b 67 while (comando == 0) { //Esperando a luta iniciar
rperoba 0:0321883eb43b 68 Drive(PpmMedio,PpmMedio);//Para o miniSumo
rperoba 0:0321883eb43b 69 LerSensoresDeDistancia();
rperoba 0:0321883eb43b 70 }
rperoba 0:0321883eb43b 71
rperoba 0:0321883eb43b 72 while (comando == 1) { //É ligado pelo microStart
rperoba 0:0321883eb43b 73
rperoba 0:0321883eb43b 74 Estrategia();//Ajusta o PPM dos motores
rperoba 0:0321883eb43b 75 LerSensoresDeDistancia();
rperoba 0:0321883eb43b 76
rperoba 0:0321883eb43b 77 }
rperoba 0:0321883eb43b 78 }
rperoba 0:0321883eb43b 79
rperoba 0:0321883eb43b 80 void MicroStartInterruptRise()
rperoba 0:0321883eb43b 81 {
rperoba 0:0321883eb43b 82 comando = 1;
rperoba 0:0321883eb43b 83 return;
rperoba 0:0321883eb43b 84 }
rperoba 0:0321883eb43b 85
rperoba 0:0321883eb43b 86 void MicroStartInterruptFall()
rperoba 0:0321883eb43b 87 {
rperoba 0:0321883eb43b 88 comando = 0;
rperoba 0:0321883eb43b 89 Drive(PpmMedio,PpmMedio);//Para o miniSumo
rperoba 0:0321883eb43b 90 return;
rperoba 0:0321883eb43b 91 }
rperoba 0:0321883eb43b 92
rperoba 0:0321883eb43b 93
rperoba 0:0321883eb43b 94 void LerSensoresDeDistancia() // Faz a leitura dos sensores de distância e armazena em leitura de distância
rperoba 0:0321883eb43b 95 {
rperoba 0:0321883eb43b 96 leituraDeDistancia = 0;
rperoba 0:0321883eb43b 97 leituraDeDistancia += SensorDireito.read();
rperoba 0:0321883eb43b 98 leituraDeDistancia += SensorDireitoFrontal.read() * 10 ;
rperoba 0:0321883eb43b 99 leituraDeDistancia += SensorFrontal.read() * 100 ;
rperoba 0:0321883eb43b 100 leituraDeDistancia += SensorEsquerdoFrontal.read() * 1000 ;
rperoba 0:0321883eb43b 101 leituraDeDistancia += SensorEsquerdo.read() * 10000 ;
rperoba 0:0321883eb43b 102
rperoba 0:0321883eb43b 103 #if debug
rperoba 0:0321883eb43b 104
rperoba 0:0321883eb43b 105 bluetoothSerial.printf("Leitura de distancia: %i",leituraDeDistancia);
rperoba 0:0321883eb43b 106
rperoba 0:0321883eb43b 107 #endif
rperoba 0:0321883eb43b 108
rperoba 0:0321883eb43b 109 return;
rperoba 0:0321883eb43b 110 }
rperoba 0:0321883eb43b 111
rperoba 0:0321883eb43b 112 void SensorDeLinhaLido() // rotina quando algum sensor de linha é lido
rperoba 0:0321883eb43b 113 {
rperoba 0:0321883eb43b 114
rperoba 0:0321883eb43b 115 Drive(PpmMinimo,PpmMinimo);//Ir para trás
rperoba 0:0321883eb43b 116 wait_us(1000);
rperoba 0:0321883eb43b 117 Drive(PpmNegativo2, PpmPositivo2);//Girar
rperoba 0:0321883eb43b 118 wait_us(500);
rperoba 0:0321883eb43b 119
rperoba 0:0321883eb43b 120 #if debug
rperoba 0:0321883eb43b 121
rperoba 0:0321883eb43b 122 bluetoothSerial.printf("Sensor de linha lido");
rperoba 0:0321883eb43b 123
rperoba 0:0321883eb43b 124 #endif
rperoba 0:0321883eb43b 125
rperoba 0:0321883eb43b 126 return;
rperoba 0:0321883eb43b 127
rperoba 0:0321883eb43b 128 }
rperoba 0:0321883eb43b 129
rperoba 0:0321883eb43b 130 void Drive(int ppmEsquerdo,int ppmDireito)
rperoba 0:0321883eb43b 131 {
rperoba 0:0321883eb43b 132 MotoresDireitos.pulsewidth_us(ppmDireito);
rperoba 0:0321883eb43b 133 MotoresEsquerdos.pulsewidth_us(ppmEsquerdo);
rperoba 0:0321883eb43b 134 ultimoPpmDireito = ppmDireito;
rperoba 0:0321883eb43b 135 ultimoPpmEsquerdo = ppmEsquerdo;
rperoba 0:0321883eb43b 136
rperoba 0:0321883eb43b 137 #if debug
rperoba 0:0321883eb43b 138
rperoba 0:0321883eb43b 139 bluetoothSerial.printf("Ppm direito: %i Ppm esquerdo: %i",(ppmEsquerdo,ppmDireito));
rperoba 0:0321883eb43b 140
rperoba 0:0321883eb43b 141 #endif
rperoba 0:0321883eb43b 142
rperoba 0:0321883eb43b 143 return;
rperoba 0:0321883eb43b 144 }
rperoba 0:0321883eb43b 145
rperoba 0:0321883eb43b 146 void SetarMotores() // Ajusta o periodo do PWM dos motores,requerimento do Mbed
rperoba 0:0321883eb43b 147 {
rperoba 0:0321883eb43b 148 MotoresEsquerdos.period_ms(16);
rperoba 0:0321883eb43b 149 MotoresDireitos.period_ms(16);
rperoba 0:0321883eb43b 150 Drive(PpmMedio,PpmMedio);
rperoba 0:0321883eb43b 151 wait(3);
rperoba 0:0321883eb43b 152
rperoba 0:0321883eb43b 153 #if debug
rperoba 0:0321883eb43b 154
rperoba 0:0321883eb43b 155 bluetoothSerial.printf("Setando motores");
rperoba 0:0321883eb43b 156
rperoba 0:0321883eb43b 157 #endif
rperoba 0:0321883eb43b 158
rperoba 0:0321883eb43b 159 return;
rperoba 0:0321883eb43b 160 }
rperoba 0:0321883eb43b 161
rperoba 0:0321883eb43b 162 void Estrategia()
rperoba 0:0321883eb43b 163 {
rperoba 0:0321883eb43b 164 switch(leituraDeDistancia) { //implementar todas as leituras
rperoba 0:0321883eb43b 165
rperoba 0:0321883eb43b 166 case 0:
rperoba 0:0321883eb43b 167 Drive(ultimoPpmEsquerdo,ultimoPpmDireito);
rperoba 0:0321883eb43b 168 break;
rperoba 0:0321883eb43b 169
rperoba 0:0321883eb43b 170 case 1: // Somente sensor direito
rperoba 0:0321883eb43b 171 Drive(PpmPositivo2,PpmNegativo2);
rperoba 0:0321883eb43b 172 break;
rperoba 0:0321883eb43b 173
rperoba 0:0321883eb43b 174 case 10://Somente sensor DireitoFrontal
rperoba 0:0321883eb43b 175 Drive(PpmPositivo2,PpmPositivo);
rperoba 0:0321883eb43b 176 break;
rperoba 0:0321883eb43b 177
rperoba 0:0321883eb43b 178 case 100://Somente sensor Frontal
rperoba 0:0321883eb43b 179 Drive(PpmMaximo,PpmMaximo);
rperoba 0:0321883eb43b 180 break;
rperoba 0:0321883eb43b 181
rperoba 0:0321883eb43b 182 case 1000://Somente sensor EsquerdoFrontal
rperoba 0:0321883eb43b 183 Drive(PpmPositivo,PpmPositivo2);
rperoba 0:0321883eb43b 184 break;
rperoba 0:0321883eb43b 185
rperoba 0:0321883eb43b 186 case 10000://Somente sensor Esquerdo
rperoba 0:0321883eb43b 187 Drive(PpmPositivo2,PpmNegativo2);
rperoba 0:0321883eb43b 188 break;
rperoba 0:0321883eb43b 189
rperoba 0:0321883eb43b 190 case 110://Frontal e frontalDireito
rperoba 0:0321883eb43b 191 Drive(PpmMaximo,PpmPositivo2);
rperoba 0:0321883eb43b 192 break;
rperoba 0:0321883eb43b 193
rperoba 0:0321883eb43b 194 case 111://Todos os frontais
rperoba 0:0321883eb43b 195 Drive(PpmSuperMaximo,PpmSuperMaximo);
rperoba 0:0321883eb43b 196 break;
rperoba 0:0321883eb43b 197
rperoba 0:0321883eb43b 198 case 1100://FrontalEsquerdo e frontal
rperoba 0:0321883eb43b 199 Drive(PpmPositivo2, PpmMaximo);
rperoba 0:0321883eb43b 200 break;
rperoba 0:0321883eb43b 201
rperoba 0:0321883eb43b 202 default:
rperoba 0:0321883eb43b 203 Drive(PpmPositivo,PpmPositivo2);
rperoba 0:0321883eb43b 204 break;
rperoba 0:0321883eb43b 205 }
rperoba 0:0321883eb43b 206 return;
rperoba 0:0321883eb43b 207
rperoba 0:0321883eb43b 208 }
rperoba 0:0321883eb43b 209
rperoba 0:0321883eb43b 210
rperoba 0:0321883eb43b 211