KRAI 2016
/
PIDsensorPING
Prototype PID - PING Feedback
main.cpp
- Committer:
- Najib_irvani
- Date:
- 2015-11-13
- Revision:
- 0:8777031777bf
- Child:
- 1:c87ba5ee0b6a
File content as of revision 0:8777031777bf:
#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 = 30; //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); } }