![](/media/cache/profiles/603ec80e671c74a8641d332712c458e8.jpg.50x50_q85.jpg)
Robô móvel com FREDOM KL25Z
Dependencies: Servo mbed HC-SR04
Diff: main.cpp
- Revision:
- 3:e5d8eb3e3951
- Parent:
- 2:2e759e231fb6
--- a/main.cpp Thu Feb 05 20:06:06 2015 +0000 +++ b/main.cpp Thu Feb 05 23:38:30 2015 +0000 @@ -1,11 +1,10 @@ #include "mbed.h" #include "Servo.h" +#include "HCSR04.h" #define velocidade(a, b) MDPWM = a; MEPWM = b -#define frente() MDdirect = 1; MEdirect = 1 -#define re() MDdirect = 0; MEdirect = 0 -#define esquerda() MDdirect = 1; MEdirect = 0 -#define direita() MDdirect = 0; MEdirect = 1 +#define sentido(a, b) MDdirect = a; MEdirect = b + //Sonar Frente //DigitalOut trig(PTB2,0); //Configuração do pino de Trigger @@ -15,48 +14,30 @@ DigitalOut bip(D2); // -Servo myservo(D3); // +Servo myservo(PTB0); // + DigitalOut MEdirect(D4); //Motor 2 Direction control PwmOut MEPWM(D5); //Motor 2 PWM control PwmOut MDPWM(D6); //Motor 1 PWM control DigitalOut MDdirect(D7); //Motor 1 Direction control - -DigitalOut trigF(A2,0); //Configuração do pino de Trigger -DigitalIn echoF(D12); //Configuração da interrupção por pino de Echo +HCSR04 sonarD(D9, D8); +HCSR04 sonarE(D11, D10); +HCSR04 sonarF(D13, D12); -//Sonar Direito -DigitalOut trigD(A1, 0); //Configuração do pino de Trigger -DigitalIn echoD(D10); //Configuração da interrupção por pino de Echo - -//Sonar Esqerdo -DigitalOut trigE(A0,0); //Configuração do pino de Trigger -InterruptIn echoE(D11); //Configuração da interrupção por pino de Echo - -DigitalOut Led(LED1); +DigitalOut Led(LED2); Serial pc(USBTX,USBRX); //Configuração da comunicação serial para enviar o valor do sensor -Timer tempo; //Para usar rotinas de tempo - - -void iniP(void); -void finP(void); -float lerSonarF(void); -float lerSonarD(void); -float lerSonarE(void); void sonarFrente(void); -float tdist=0, distcm=0, distin=0, dist0=0; -float dist_esq = 0, dist_dir = 0, dist_fre = 0, razDE = 0, Xmeio = 0, Ve = 0.3, Vd = 0.3, Vg = 0.23, Kp = 0.080, ed = 0, ee = 0; -char flagTempo = 0; +float dist_esq = 0, dist_dir = 0, dist_fre = 0, razDE = 0, Xmeio = 0, Ve = 0.3, Vd = 0.3, Vg = 0.0, Kp = 0.050, ed = 0, ee = 0; + int main() { - //printf("INICIA\n"); - //myservo.calibrate(0.0013, 45.0); - - // - + printf("INICIA\n"); + myservo.calibrate(0.0013, 45.0); + wait_ms(3000); myservo.position(-3.0); Led = 1; @@ -67,19 +48,18 @@ wait_ms(500); while(1){ - dist_fre = lerSonarF(); - if (dist_fre <= 20){ + dist_fre = sonarF.getCm(); + /*if (dist_fre <= 20){ velocidade(0.0, 0.0); - } - //sonarFrente(); + }*/ //printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre); //wait_ms(1000); - dist_esq = lerSonarE(); + dist_esq = sonarE.getCm(); //printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq); //wait_ms(1000); - dist_dir = lerSonarD(); + dist_dir = sonarD.getCm(); //printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir); //wait_ms(1000); @@ -94,11 +74,13 @@ //Ve =(ee * Kp); // //Vd =(ed * Kp); - if(Ve > 0.38){ - Ve = 0.38; + + + if(Ve > 0.31){ + Ve = 0.31; } - if(Vd > 0.38){ - Vd = 0.38; + if(Vd > 0.31){ + Vd = 0.31; } if(Ve < 0.18){ Ve = 0.18; @@ -110,56 +92,10 @@ } } -float lerSonarD(void) -{ - trigD = 1; //Inicio do trigger - wait_us(10); //10us de pulso - trigD = 0; //Fim do trigger - while(!echoD); - tempo.start(); - while(echoD); - tdist = tempo.read_us(); //Leitura do tempo transcorrido - distcm = tdist/58; //Cálculo da distância detectada em "cm" - //distin = tdist/148; //Cálculo da distância detectada em "in" - tempo.stop(); //Paro o temporizador - tempo.reset(); //Reset para o próximo ciclo - return distcm; -} - -float lerSonarF(void) -{ - trigF=1; //Inicio do trigger - wait_us(10); //10us de pulso - trigF=0; //Fim do trigger - while(!echoF); - tempo.start(); - while(echoF); - tdist = tempo.read_us(); //Leitura do tempo transcorrido - distcm = tdist/58; //Cálculo da distância detectada em "cm" - //distin = tdist/148; //Cálculo da distância detectada em "in" - tempo.stop(); //Paro o temporizador - tempo.reset(); //Reset para o próximo ciclo - return distcm; -} - -float lerSonarE(void) -{ - trigE = 1; //Inicio do trigger - wait_us(10); //10us de pulso - trigE = 0; //Fim do trigger - while(!echoE); - tempo.start(); - while(echoE); - tdist = tempo.read_us(); //Leitura do tempo transcorrido - distcm = tdist/58; //Cálculo da distância detectada em "cm" - //distin = tdist/148; //Cálculo da distância detectada em "in" - tempo.stop(); //Paro o temporizador - tempo.reset(); //Reset para o próximo ciclo - return distcm; -} -void sonarFrente(void){ +//****************************************************************************** +/*void sonarFrente(void){ bip = 1; printf("PARA\n"); re(); @@ -201,4 +137,4 @@ velocidade(0.48, 0.48); frente(); } -} \ No newline at end of file +}*/ \ No newline at end of file