File content as of revision 0:12479c9c5c72:

#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;
        */
    }
}