Diff: main.cpp

Revision:

2:2e759e231fb6

Parent:

1:027b210e631d

Child:

3:e5d8eb3e3951

--- a/main.cpp	Thu Jan 29 00:35:29 2015 +0000
+++ b/main.cpp	Thu Feb 05 20:06:06 2015 +0000
@@ -42,62 +42,79 @@
 
 void iniP(void);     
 void finP(void); 
-void lerSonarF(void);
-void lerSonarD(void);
-void lerSonarE(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;
+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;
+  
+int main() { 
+    //printf("INICIA\n");
+    //myservo.calibrate(0.0013, 45.0);
     
-int main() { 
-    printf("INICIA\n");
-    myservo.calibrate(0.0013, 45.0);
+            //
+    
     wait_ms(3000);
-    //myservo.position(-33.0); todo pra direita
-    //myservo.position(30.8); todo pra esquerda
     myservo.position(-3.0);
-    Led = 1;
-    printf("COMECA O PROGRAMA\n");
-    velocidade(0.48, 0.48);
+    Led = 1; 
+    
+    //printf("COMECA O PROGRAMA\n");
     frente();
+    velocidade(Vd, Ve);
+    wait_ms(500);
     
     while(1){
-        lerSonarF();
-        dist_fre = distcm;
-        printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre);
+        dist_fre = lerSonarF(); 
+        if (dist_fre <= 20){
+            velocidade(0.0, 0.0);
+        }      
+        //sonarFrente();
+        //printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre);        
         //wait_ms(1000);
         
-        lerSonarE();
-        dist_esq = distcm;
-        printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq);
+        dist_esq = lerSonarE();
+        //printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq);
         //wait_ms(1000);
-        lerSonarD();
-        dist_dir = distcm;
-        printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir);
+        
+        dist_dir = lerSonarD();
+        //printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir);
         //wait_ms(1000);
         
-        razDE = dist_dir / dist_esq;
-    
-        if((dist_fre <= 30) && (dist_fre > 3)){
-            sonarFrente();
-        }
+        Xmeio = (dist_dir + dist_esq) / 2;  //Caucula o SetPoint
+        
+        ed = Xmeio - dist_dir;              //
+        ee = Xmeio - dist_esq;              //
+        
+        Ve = Vg + (ee * Kp);                //
+        Vd = Vg + (ed * Kp);                //
+        
+        //Ve =(ee * Kp);                //
+        //Vd =(ed * Kp);
         
-        if(razDE > 1){
-            velocidade(0.22, 0.48);
+        if(Ve > 0.38){
+            Ve = 0.38;
+        }
+        if(Vd > 0.38){
+            Vd = 0.38;
         }
-        else if(razDE < 1){
-            velocidade(0.48, 0.22);  
-        }   
+        if(Ve < 0.18){
+            Ve = 0.18;
+        }
+        if(Vd < 0.18){
+            Vd = 0.18;
+        }
+        velocidade(Vd, Ve);
     }
 }
 
-void lerSonarD(void)
+float lerSonarD(void)
 {
-    trigD=1;             //Inicio do trigger
-    wait_us(10);        //10us de pulso
-    trigD=0;             //Fim do trigger
+    trigD = 1;                  //Inicio do trigger
+    wait_us(10);                //10us de pulso
+    trigD = 0;                  //Fim do trigger
     while(!echoD);
     tempo.start();
     while(echoD);
@@ -106,13 +123,14 @@
     //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 lerSonarF(void)
+float lerSonarF(void)
 {
-    trigF=1;             //Inicio do trigger
-    wait_us(10);        //10us de pulso
-    trigF=0;             //Fim do trigger
+    trigF=1;                    //Inicio do trigger
+    wait_us(10);                //10us de pulso
+    trigF=0;                    //Fim do trigger
     while(!echoF);
     tempo.start();
     while(echoF);
@@ -121,13 +139,14 @@
     //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 lerSonarE(void)
+float lerSonarE(void)
 {
-    trigE = 1;             //Inicio do trigger
-    wait_us(10);        //10us de pulso
-    trigE = 0;             //Fim do trigger
+    trigE = 1;                  //Inicio do trigger
+    wait_us(10);                //10us de pulso
+    trigE = 0;                  //Fim do trigger
     while(!echoE);
     tempo.start();
     while(echoE);
@@ -136,6 +155,7 @@
     //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;
 }