File content as of revision 1:14997ad30b4e:

#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 a,b,c;
double current_error, previous_error1 = 0;
double pwm, previous_pwm = 0;

// Atur Variable Tuning
double target = 90;
double Kp = -0.003753, Ki = -1.001e-06, Ts=10;
double batas_pwm_atas = 0.35;
double batas_pwm_bawah = 0.05;

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\t%d\n", target, 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;
        
        if (pwm<batas_pwm_atas && pwm>batas_pwm_bawah){
            pwm = batas_pwm_atas;
        }
        if (pwm>-1*batas_pwm_atas && pwm<-1*batas_pwm_bawah){
            pwm = -1*batas_pwm_atas;
        }
        
        powerScrew.speed(pwm);
        
        previous_pwm = pwm;
        previous_error1 = current_error;
        
    }
}