![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
bumba
main.cpp
- Committer:
- NeoLewis
- Date:
- 2019-08-11
- Revision:
- 0:9d16fef10591
- Child:
- 1:8aece97d3c78
File content as of revision 0:9d16fef10591:
#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; }