Codigo do miniSumo de 4 rodas

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
rperoba
Date:
Tue Aug 11 20:54:54 2020 +0000
Commit message:
Codigo inicial do minisumo com 4 rodas

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 0321883eb43b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 11 20:54:54 2020 +0000
@@ -0,0 +1,211 @@
+#include "mbed.h"
+#include "PwmOut.h"
+
+#define PpmSuperMaximo 2000 //AjustarValores
+#define PpmMaximo 1800
+#define PpmPositivo2 1700
+#define PpmPositivo 1600
+#define PpmMedio  1500
+#define PpmNegativo 1400
+#define PpmNegativo2 1300
+#define PpmMinimo 1200
+
+#define debug false
+
+//Sensores de distância
+DigitalIn SensorEsquerdo (p16);
+DigitalIn SensorEsquerdoFrontal (p15);
+DigitalIn SensorFrontal (p14);
+DigitalIn SensorDireitoFrontal (p13);
+DigitalIn SensorDireito (p12);
+
+//Sensores de linha
+InterruptIn LinhaEsquerda(p19);
+InterruptIn LinhaDireita(p20);
+
+//Motores
+PwmOut MotoresEsquerdos(p21);
+PwmOut MotoresDireitos(p23);
+
+//MicroStart
+InterruptIn MicroStart(p18);
+
+//Bluetooth
+DigitalOut bluetoothState(p26);
+DigitalOut bluetoothEnable (p29);
+Serial bluetoothSerial (p28,p27);
+
+//Variáveis
+int leituraDeDistancia = 0;
+
+int comando=0;
+int ultimoPpmDireito;
+int ultimoPpmEsquerdo;
+
+void Drive(int ppmEsquerdo,int ppmDireito);
+void SetarMotores ();
+void LerSensoresDeDistancia();
+void SensorDeLinhaLido();
+void MicroStartInterruptRise();
+void MicroStartInterruptFall();
+void Estrategia();
+
+int main()
+{
+#if debug
+
+    bluetoothSerial.printf("Iniciando...");
+
+#endif
+    MicroStart.rise(&MicroStartInterruptRise);
+    MicroStart.fall(&MicroStartInterruptFall);
+    SetarMotores(); // Ajusta os motores
+    LinhaDireita.rise(&SensorDeLinhaLido);//Interrupção para os sensores de linha
+    LinhaEsquerda.rise(&SensorDeLinhaLido);
+    
+
+    while (comando == 0) { //Esperando a luta iniciar
+        Drive(PpmMedio,PpmMedio);//Para o miniSumo
+        LerSensoresDeDistancia();
+    }
+
+    while (comando == 1) { //É ligado pelo microStart
+        
+        Estrategia();//Ajusta o PPM dos motores
+        LerSensoresDeDistancia();
+        
+    }
+}
+
+void MicroStartInterruptRise()
+{
+    comando = 1;
+    return;
+}
+
+void MicroStartInterruptFall()
+{
+    comando = 0;
+    Drive(PpmMedio,PpmMedio);//Para o miniSumo
+    return;
+}
+
+
+void LerSensoresDeDistancia() // Faz a leitura dos sensores de distância e armazena em leitura de distância
+{
+    leituraDeDistancia = 0;
+    leituraDeDistancia += SensorDireito.read();
+    leituraDeDistancia += SensorDireitoFrontal.read() * 10 ;
+    leituraDeDistancia += SensorFrontal.read() * 100 ;
+    leituraDeDistancia += SensorEsquerdoFrontal.read() * 1000 ;
+    leituraDeDistancia += SensorEsquerdo.read() * 10000 ;
+
+#if debug
+
+    bluetoothSerial.printf("Leitura de distancia: %i",leituraDeDistancia);
+
+#endif
+
+    return;
+}
+
+void SensorDeLinhaLido() // rotina quando algum sensor de linha é lido
+{
+
+    Drive(PpmMinimo,PpmMinimo);//Ir para trás
+    wait_us(1000);
+    Drive(PpmNegativo2, PpmPositivo2);//Girar
+    wait_us(500);
+
+#if debug
+
+    bluetoothSerial.printf("Sensor de linha lido");
+
+#endif
+
+    return;
+
+}
+
+void Drive(int ppmEsquerdo,int ppmDireito)
+{
+    MotoresDireitos.pulsewidth_us(ppmDireito);
+    MotoresEsquerdos.pulsewidth_us(ppmEsquerdo);
+    ultimoPpmDireito = ppmDireito;
+    ultimoPpmEsquerdo = ppmEsquerdo;
+
+#if debug
+
+    bluetoothSerial.printf("Ppm direito: %i  Ppm esquerdo: %i",(ppmEsquerdo,ppmDireito));
+
+#endif
+
+    return;
+}
+
+void SetarMotores() // Ajusta o periodo do PWM dos motores,requerimento do Mbed
+{
+    MotoresEsquerdos.period_ms(16);
+    MotoresDireitos.period_ms(16);
+    Drive(PpmMedio,PpmMedio);
+    wait(3);
+
+#if debug
+
+    bluetoothSerial.printf("Setando motores");
+
+#endif
+
+    return;
+}
+
+void Estrategia() 
+{
+    switch(leituraDeDistancia) { //implementar todas as leituras
+    
+        case 0:
+            Drive(ultimoPpmEsquerdo,ultimoPpmDireito);
+            break;
+    
+        case 1: // Somente sensor direito
+            Drive(PpmPositivo2,PpmNegativo2);
+            break;
+
+        case 10://Somente sensor DireitoFrontal
+            Drive(PpmPositivo2,PpmPositivo);
+            break;
+
+        case 100://Somente sensor Frontal
+            Drive(PpmMaximo,PpmMaximo);
+            break;
+
+        case 1000://Somente sensor EsquerdoFrontal
+            Drive(PpmPositivo,PpmPositivo2);
+            break;
+
+        case 10000://Somente sensor Esquerdo
+            Drive(PpmPositivo2,PpmNegativo2);
+            break;
+
+        case 110://Frontal e frontalDireito
+            Drive(PpmMaximo,PpmPositivo2);
+            break;
+
+        case 111://Todos os frontais
+            Drive(PpmSuperMaximo,PpmSuperMaximo);
+            break;
+
+        case 1100://FrontalEsquerdo e frontal
+            Drive(PpmPositivo2, PpmMaximo);
+            break;
+
+        default:
+            Drive(PpmPositivo,PpmPositivo2);
+            break;
+    }
+    return;
+
+}
+
+
+
diff -r 000000000000 -r 0321883eb43b mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Aug 11 20:54:54 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file