Diff: main.cpp

Revision:

0:182a12372dd4

Child:

1:027b210e631d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jan 27 21:57:22 2015 +0000
@@ -0,0 +1,170 @@
+#include "mbed.h"
+#include "Servo.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
+
+//Sonar Frente
+//DigitalOut trig(PTB2,0);      //Configuração do pino de Trigger
+//InterruptIn echo(D12);       //Configuração da interrupção por pino de Echo 
+//Sonar Esquerdo
+//DigitalOut trig(A0,0);      //Configuração do pino de Trigger
+//InterruptIn echo(D11);       //Configuração da interrupção por pino de Echo 
+
+
+
+DigitalOut bip(D2);         //
+Servo myservo(D3);          //
+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
+//Sonar Frente
+DigitalOut trig(PTB2,0);      //Configuração do pino de Trigger
+InterruptIn echo(D12);       //Configuração da interrupção por pino de Echo 
+DigitalOut Led(LED1);
+
+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); 
+void lerSonar(void);
+
+float tdist=0, distcm=0, distin=0, dist0=0;
+float dist_esq = 0, dist_dir = 0, dist_fre = 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.38, 0.38);
+    frente();
+    
+    while(1){
+        lerSonar();
+        printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
+        dist_fre = distcm;
+    
+        if((dist_fre <= 30) && (dist_fre > 3)){
+            bip = 1;
+            printf("PARA\n");
+            re();
+            wait_ms(100);
+            velocidade(0.0, 0.0);
+            bip = 0;
+            
+            myservo.position(-33.0);
+            wait_ms(1000);
+            lerSonar();
+            printf("Distancia direita %.2f cm \n",distcm);
+            dist_dir = distcm;
+            
+            myservo.position(30.8);
+            wait_ms(1000);
+            lerSonar();
+            printf("Distancia esquerda %.2f cm \n",distcm);
+            dist_esq = distcm;
+            
+            if(dist_dir > dist_esq){
+                printf("VIRA ESQUERDA \n");
+                direita();
+                velocidade(0.38, 0.38);
+                wait_ms(250);
+                velocidade(0.0, 0.0);
+                myservo.position(-3.0);
+                wait_ms(500);
+                velocidade(0.38, 0.38);
+                frente();
+            }
+            else if(dist_dir < dist_esq){
+                printf("VIRA ESQUERDA \n");
+                esquerda();
+                velocidade(0.38, 0.38);
+                wait_ms(250);
+                velocidade(0.0, 0.0);
+                myservo.position(-3.0);
+                wait_ms(500);
+                velocidade(0.38, 0.38);
+                frente();   
+            }
+            //myservo.position(-3.0);
+            //wait_ms(100);
+        }
+    }
+    
+    
+    
+    /*while(1){
+        M1direct = 1;
+        M2direct = 1;
+        bip = 0;
+        
+        for(float p=0; p<=1.0; p += 0.1){
+            myservo = p;
+            trig=1;             //Inicio do trigger
+            wait_us(10);        // 10us de pulso
+            trig=0;             //Fim do trigger
+            echo.rise(&iniP);   //leitura do inicio do pulso de retorno
+            echo.fall(&finP);   //Leitura do final do pulso de retorno
+            printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
+            wait(0.05);
+        }
+        wait(0.13);
+        
+        M1direct = 1;
+        M2direct = 1;
+        bip = 0;
+        
+        for(float p=1.0; p>=0.0; p -= 0.1) {
+            myservo = p;
+            trig=1;             //Inicio do trigger
+            wait_us(10);        // 10us de pulso
+            trig=0;             //Fim do trigger
+            echo.rise(&iniP);   //leitura do inicio do pulso de retorno
+            echo.fall(&finP);   //Leitura do final do pulso de retorno
+            printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
+            wait(0.05);
+        }
+        wait(0.13);
+    }*/
+}
+
+//Rotina para receber o pulso inicial do pino Echo
+void iniP(void)
+{            
+    tempo.start();              //Rotina para iniciar o contador
+    return;
+}
+//Rotina para pegar o tempo final do pulso   
+void finP(void)
+{ 
+    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;
+}
+
+void lerSonar(void)
+{
+    trig=1;             //Inicio do trigger
+    wait_us(10);        //10us de pulso
+    trig=0;             //Fim do trigger
+    echo.rise(&iniP);   //leitura do inicio do pulso de retorno
+    echo.fall(&finP);   //Leitura do final do pulso de retorno
+    wait_ms(100);
+    return;
+}