File content as of revision 1:c87ba5ee0b6a:

#include "mbed.h"
#include "Servo.h"
#include "Ping.h"

Ping distance(A0); //sensor ping pin 8

float pwm = 0; //pwm awal;
float pwm1 = 50;
float kp = 25; //konstanta proportional
float ki = 21.429; //konstanta integral
float kd = 2.625; //konstanta derivative

float clearence = 20; //wors case jarak lengan dengan robot eco
float errors;
float last_error;
float sum_error = 0;
float p,d,i; //deklarasi PID
float value; //nilai sensor ping

unsigned int min_pwm = 30;
unsigned int max_pwm = 80;


void init_pwm() //pembatasan nilai pwm
{
  if (pwm<min_pwm)
  {
    pwm = min_pwm;
  } if (pwm>max_pwm)
  {
    pwm = max_pwm;
  }
}

float read_jarak () {
    float jarak;
    
    distance.Send();
    wait_ms(45);
    jarak = distance.Read_cm()/2;
    return jarak;
    }
int main() {
    
    Servo edf(PC_7);
    Serial pc(USBTX, USBRX);
    
    while(1) {
  value = read_jarak(); //baca jarak
  errors = clearence - value; //menghitung nilai error
  
  p = kp*errors; //menghitung nilai proportional
  d = kd*(errors-last_error);  //menghitung nilai derifativ
  i = ki*(sum_error); //menghitung nilai integral
  
  pwm = p; //nilai pid
  init_pwm();
  edf = pwm; //input nilai pwm
  
  last_error = errors; //menyimpan nilai error
  sum_error = sum_error+errors; //menyimpan jumlah nilai error
  
  printf("Jarak = ");printf("%f\t",value);
  printf("PWM = ");printf("%f\t",pwm);
  printf("ERROR = ");printf("%f\t",errors);
  printf("P = ");printf("%f\t",p);
  printf("I = ");printf("%f\t",i);
  printf("D= ");printf("%f\t",d);
  printf("PID = ");printf("%f\n",p+i+d);;//display
  
  wait_ms(10);
  

    }
}