Diff: main.cpp

Revision:

0:baab7b3370d5

Child:

1:bdfcf96dc8b1

diff -r 000000000000 -r baab7b3370d5 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 22 17:16:46 2015 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "HCSR04.h"
+
+#define velocidade(a, b)    MDPWM = a; MEPWM = b 
+#define sentido(a, b)       MDdirect = a; MEdirect = b
+
+//DigitalOut myled(LED1);
+//Serial pc(USBTX,USBRX);     
+
+HCSR04 sonarF(D13, D12);
+
+DigitalOut MEdirect(D4);    //Motor 2 Direction control
+DigitalOut MDdirect(D7);    //Motor 1 Direction control
+PwmOut MEPWM(D5);           //Motor 2 PWM control
+PwmOut MDPWM(D6);           //Motor 1 PWM control
+
+float SP = 25.0;
+float VM;
+float Kp = 0.06;
+float e;
+float v;
+
+int main() {
+    velocidade(0.4, 0.4);
+    while(1){
+        VM = sonarF.getCm();
+        e = SP - VM;
+        v = Kp * e;
+        if(e <= 0){
+            velocidade(v*(-1), v*(-1));
+            sentido(1, 1);
+        }
+        else{
+            velocidade(v, v);
+            sentido(0, 0);
+        }
+        wait(0.2);
+    }
+    
+    /*while(1){
+        if(sonarF.getCm() < 25){
+            sentido(0, 0);
+        }
+        else{
+            sentido(1, 1);
+        }
+        wait(0.2);
+    }*/
+}