Diff: main.cpp

Revision:

0:12479c9c5c72

Child:

1:14997ad30b4e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 15 06:59:16 2017 +0000
@@ -0,0 +1,44 @@
+#include "mbed.h"
+#include "Ping.h"
+#include "Motor.h"
+
+#define PIN PC_15
+
+Ping ping(PIN);
+Motor powerScrew(PB_10, PB_14, PB_13);
+Serial pc(USBTX,USBRX);
+
+int jarak;
+double target = 90;
+double a,b,c;
+double current_error, previous_error1 = 0;
+double Kp = -0.003753, Ki = -1.001e-06, Ts=30;
+double pwm, previous_pwm = 0;
+
+int main(){
+    pc.baud(115200);
+    powerScrew.speed(0.5);
+    while(1){
+        int jarak;
+        ping.Send();
+        wait_ms(10);
+        jarak = ping.Read_cm();
+        
+        pc.printf("%d\n", jarak);
+        /*
+        current_error =  (double) (target-jarak);
+        a = Kp + Ki*Ts/2;
+        b = -Kp + Ki*Ts/2;
+     
+        pwm = previous_pwm + a*current_error + b*previous_error1;   
+        
+        if (pwm>1) pwm=1;
+        if (pwm<-1) pwm=-1;
+        
+        powerScrew.speed(pwm);
+        
+        previous_pwm = pwm;
+        previous_error1 = current_error;
+        */
+    }
+}
\ No newline at end of file