KRAI 2016 / Mbed 2 deprecated PIDsensorPING

Prototype PID - PING Feedback

Dependencies:   Ping Servo mbed

main.cpp@1:c87ba5ee0b6a, 2015-11-13 (annotated)

Committer:
Najib_irvani
Date:
Fri Nov 13 09:59:58 2015 +0000
Revision:
1:c87ba5ee0b6a
Parent:
0:8777031777bf 
Versi 1.3 (clearence 20)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Najib_irvani 0:8777031777bf 1 #include "mbed.h"
Najib_irvani 0:8777031777bf 2 #include "Servo.h"
Najib_irvani 0:8777031777bf 3 #include "Ping.h"
Najib_irvani 0:8777031777bf 4
Najib_irvani 0:8777031777bf 5 Ping distance(A0); //sensor ping pin 8
Najib_irvani 0:8777031777bf 6
Najib_irvani 0:8777031777bf 7 float pwm = 0; //pwm awal;
Najib_irvani 0:8777031777bf 8 float pwm1 = 50;
Najib_irvani 0:8777031777bf 9 float kp = 25; //konstanta proportional
Najib_irvani 0:8777031777bf 10 float ki = 21.429; //konstanta integral
Najib_irvani 0:8777031777bf 11 float kd = 2.625; //konstanta derivative
Najib_irvani 0:8777031777bf 12
Najib_irvani 1:c87ba5ee0b6a 13 float clearence = 20; //wors case jarak lengan dengan robot eco
Najib_irvani 0:8777031777bf 14 float errors;
Najib_irvani 0:8777031777bf 15 float last_error;
Najib_irvani 0:8777031777bf 16 float sum_error = 0;
Najib_irvani 0:8777031777bf 17 float p,d,i; //deklarasi PID
Najib_irvani 0:8777031777bf 18 float value; //nilai sensor ping
Najib_irvani 0:8777031777bf 19
Najib_irvani 0:8777031777bf 20 unsigned int min_pwm = 30;
Najib_irvani 0:8777031777bf 21 unsigned int max_pwm = 80;
Najib_irvani 0:8777031777bf 22
Najib_irvani 0:8777031777bf 23
Najib_irvani 0:8777031777bf 24 void init_pwm() //pembatasan nilai pwm
Najib_irvani 0:8777031777bf 25 {
Najib_irvani 0:8777031777bf 26 if (pwm<min_pwm)
Najib_irvani 0:8777031777bf 27 {
Najib_irvani 0:8777031777bf 28 pwm = min_pwm;
Najib_irvani 0:8777031777bf 29 } if (pwm>max_pwm)
Najib_irvani 0:8777031777bf 30 {
Najib_irvani 0:8777031777bf 31 pwm = max_pwm;
Najib_irvani 0:8777031777bf 32 }
Najib_irvani 0:8777031777bf 33 }
Najib_irvani 0:8777031777bf 34
Najib_irvani 0:8777031777bf 35 float read_jarak () {
Najib_irvani 0:8777031777bf 36 float jarak;
Najib_irvani 0:8777031777bf 37
Najib_irvani 0:8777031777bf 38 distance.Send();
Najib_irvani 0:8777031777bf 39 wait_ms(45);
Najib_irvani 0:8777031777bf 40 jarak = distance.Read_cm()/2;
Najib_irvani 0:8777031777bf 41 return jarak;
Najib_irvani 0:8777031777bf 42 }
Najib_irvani 0:8777031777bf 43 int main() {
Najib_irvani 0:8777031777bf 44
Najib_irvani 0:8777031777bf 45 Servo edf(PC_7);
Najib_irvani 0:8777031777bf 46 Serial pc(USBTX, USBRX);
Najib_irvani 0:8777031777bf 47
Najib_irvani 0:8777031777bf 48 while(1) {
Najib_irvani 0:8777031777bf 49 value = read_jarak(); //baca jarak
Najib_irvani 0:8777031777bf 50 errors = clearence - value; //menghitung nilai error
Najib_irvani 0:8777031777bf 51
Najib_irvani 0:8777031777bf 52 p = kp*errors; //menghitung nilai proportional
Najib_irvani 0:8777031777bf 53 d = kd*(errors-last_error); //menghitung nilai derifativ
Najib_irvani 0:8777031777bf 54 i = ki*(sum_error); //menghitung nilai integral
Najib_irvani 0:8777031777bf 55
Najib_irvani 0:8777031777bf 56 pwm = p; //nilai pid
Najib_irvani 0:8777031777bf 57 init_pwm();
Najib_irvani 0:8777031777bf 58 edf = pwm; //input nilai pwm
Najib_irvani 0:8777031777bf 59
Najib_irvani 0:8777031777bf 60 last_error = errors; //menyimpan nilai error
Najib_irvani 0:8777031777bf 61 sum_error = sum_error+errors; //menyimpan jumlah nilai error
Najib_irvani 0:8777031777bf 62
Najib_irvani 0:8777031777bf 63 printf("Jarak = ");printf("%f\t",value);
Najib_irvani 0:8777031777bf 64 printf("PWM = ");printf("%f\t",pwm);
Najib_irvani 0:8777031777bf 65 printf("ERROR = ");printf("%f\t",errors);
Najib_irvani 0:8777031777bf 66 printf("P = ");printf("%f\t",p);
Najib_irvani 0:8777031777bf 67 printf("I = ");printf("%f\t",i);
Najib_irvani 0:8777031777bf 68 printf("D= ");printf("%f\t",d);
Najib_irvani 0:8777031777bf 69 printf("PID = ");printf("%f\n",p+i+d);;//display
Najib_irvani 0:8777031777bf 70
Najib_irvani 0:8777031777bf 71 wait_ms(10);
Najib_irvani 0:8777031777bf 72
Najib_irvani 0:8777031777bf 73
Najib_irvani 0:8777031777bf 74 }
Najib_irvani 0:8777031777bf 75 }
Najib_irvani 0:8777031777bf 76