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