Luiz Manoel Sales
/
jorge
bumba
Diff: main.cpp
- Revision:
- 0:9d16fef10591
- Child:
- 1:8aece97d3c78
--- /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