Luiz Manoel Sales
bumba
Diff: main.cpp
- Revision:
- 0:9d16fef10591
- Child:
- 1:8aece97d3c78
diff -r 000000000000 -r 9d16fef10591 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Aug 11 00:57:52 2019 +0000
@@ -0,0 +1,238 @@
+#include "mbed.h"
+PwmOut Left(p25); //Definição do motor da esquerda
+PwmOut Right(p23); //Definicao do motor da direita
+DigitalIn Sensor1 (p17);// Sensor digital localizado na esquerda do robo
+DigitalIn Sensor2 (p15);//Sensor digital localizado na diagonal esquerda do robo
+DigitalIn Sensor3 (p13);//Sensor digital localizado na frente do robo
+DigitalIn Sensor4 (p11); //Sensor digital localizado na diagonal direita do robo
+DigitalIn Sensor5 (p9); //Sensor digital localizado na direita do robo
+
+
+#define MIN 1060 //velocidade maxima para tras
+#define MID ((MAX+MIN)/2) //valor para parar
+#define BUS 12 //velocidade de busca
+#define MAX 1860 //velocidade maxima para frente
+#define r 2
+#define e 3
+#define d 4
+#define p 9
+#define b 5
+
+
+int i=MID;
+int k=MID;
+int total; //{right, right_diagonal, front, left_diagonal, left} valor atual da leitura
+int measure[5]={0,0,0,0,0}; // vetor que grava a leitura atual
+int tot=0;
+
+
+//***************************** DEFINICOES - FINAL *****************************
+//################################################################################
+//################################################################################
+//******************************* FUNCOES - INICIO ******************************
+//Funcao que inicializa os motores
+void Motors_Setup()
+{
+ Right.period_ms(16);
+ Left.period_ms(16);
+ Right.pulsewidth_us(MID);
+ Left.pulsewidth_us(MID);
+ wait(0.1);
+ }
+//Função que desloca os motores de acordo com o sinal PPM recebido
+void Drive(int maximo,int minimo,int controle)
+{
+ switch(controle)
+ {
+ case 1:
+ Left.pulsewidth_us(minimo);
+ Right.pulsewidth_us(minimo);
+ break;
+ case 2:
+ for (;k<=maximo || i<=maximo;k+=50,i+=50)
+ {
+ Left.pulsewidth_us(k);
+ wait(0.05);
+ Right.pulsewidth_us(i);
+ wait(0.05);
+ if (k>MAX)
+ {
+ k=MAX;
+ }
+ if (i>MAX)
+ {
+ i=MAX;
+ }
+ if (k<MIN)
+ {
+ k=MIN;
+ }
+ if (i<MIN)
+ {
+ i=MIN;
+ }
+ }
+ break;
+ case 3:
+ for (;k>=minimo || i<=maximo;k-=50,i+=50)
+ {
+ Left.pulsewidth_us(k);
+ wait(0.05);
+ Right.pulsewidth_us(i);
+ wait(0.05);
+ if (k>MAX)
+ {
+ k=MAX;
+ }
+ if (i>MAX)
+ {
+ i=MAX;
+ }
+ if (k<MIN)
+ {
+ k=MIN;
+ }
+ if (i<MIN)
+ {
+ i=MIN;
+ }
+ }
+
+ break;
+ case 4:
+ for (;k<=maximo || i>=minimo;k+=50,i-=50)
+ {
+ Left.pulsewidth_us(k);
+ wait(0.05);
+ Right.pulsewidth_us(i);
+ wait(0.05);
+ if (k>MAX)
+ {
+ k=MAX;
+ }
+ if (i>MAX)
+ {
+ i=MAX;
+ }
+ if (k<MIN)
+ {
+ k=MIN;
+ }
+ if (i<MIN)
+ {
+ i=MIN;
+ }
+ }
+
+ break;
+ case 5:
+ for (;k>=minimo || i>=minimo;k-=50,i-=50)
+ {
+ Left.pulsewidth_us(k);
+ wait(0.05);
+ Right.pulsewidth_us(i);
+ wait(0.05);
+ if (k>MAX)
+ {
+ k=MAX;
+ }
+ if (i>MAX)
+ {
+ i=MAX;
+ }
+ if (k<MIN)
+ {
+ k=MIN;
+ }
+ if (i<MIN)
+ {
+ i=MIN;
+ }
+ }
+
+ break;
+ default:
+ Left.pulsewidth_us(MID);
+ Right.pulsewidth_us(MID);
+ break;
+ }
+}
+
+int Search ()
+{
+ measure[0] = Sensor1.read();
+ measure[1] = Sensor2.read();
+ measure[2] = Sensor3.read();
+ measure[3] = Sensor4.read();
+ measure[4] = Sensor5.read();
+
+ total = measure[0]*10000 + measure[1]*1000 + measure[2]*100 + measure[3]*10 + measure[4]*1;
+ return total;
+}
+
+int Action(int total)
+{
+ switch(total)
+ {
+ case 1: //vira para a direita
+ Drive(MAX,MIN,d);
+ break;
+
+ case 10: //vira para a direita
+ Drive(MAX,MIN,d);
+ break;
+
+ case 11: //vira para a direita
+ Drive(MAX,MIN,d);
+ break;
+
+ case 100: //Vai reto
+ Drive(MAX,MIN,r);
+ break;
+
+ case 110: //direita com leiruta do sensor da diagonal direita
+ Drive(MAX,MIN,d);
+ break;
+
+ case 1000: //vai para a esquerda
+ Drive(MAX,MIN,e);
+ break;
+
+ case 1100: //esquerda com leitura do sensor da diagonal esquerda
+ Drive(MAX,MIN,e);
+ break;
+
+ case 1110: //vai reto com a leitura dos sensores diagonais e o da frente
+ Drive(MAX,MIN,r);
+ break;
+
+ case 10000: //vai para a esquerda
+ Drive(MAX,MIN,e);
+ break;
+
+ case 11000: //vai para a esquerda
+ Drive(MAX,MIN,e);
+ break;
+
+ case 11111: //leu todos os sensores ao mesmo tempo
+ Drive(MAX,MIN,p);
+ break;
+
+ //todos os outros casos que são impossíveis
+ default:// Para
+ Drive(MAX,MIN,0);
+ break;
+ }
+ return 0;
+}
+
+int main()
+{
+ while(1)
+ {
+ tot=Search();
+ Action(tot);
+ }
+ return 0;
+ }
+
\ No newline at end of file