uji coba PID dengan sensor garis
Revision 0:b31e672064ea, committed 2015-11-08
- Comitter:
- rizqicahyo
- Date:
- Sun Nov 08 08:26:43 2015 +0000
- Commit message:
- versi 1 untuk arduino
Changed in this revision
pid.txt | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r b31e672064ea pid.txt --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid.txt Sun Nov 08 08:26:43 2015 +0000 @@ -0,0 +1,486 @@ +/*=================== Konfigurasi PIN di arduino =========================*/ +const int selA = A2; +const int selB = A3; +const int selC = A4; + +const int m1a=2; // pin motor1 untuk atur arah +const int m1b=3; // pin motor1 untuk atur arah +const int pwm1=5; // pin output pwm motor1 ke motor driver +int motor_ki; // variabel nilai pwm motor1 + +const int m2a=4; // pin motor2 untuk atur arah +const int m2b=7; // pin motor2 untuk atur arah +const int pwm2=6; // pin output pwm motor2 ke motor driver +int motor_ka; // variabel nilai pwm motor2 + +/*============ Konfigurasi variabel dan konstanta PID + Sensor ===============*/ +int batas = 860; // nilai batas sensor +int pid,pv,error,error0; // varibel-variabel untuk PID +int sp=0; // set point, sensor robot ditengah , sp=0 +int ts=10; // time sampling, untuk waktu sampling pada konstanta I dan D +int kode; // variabel keadaan ketika robot keluar dari garis + +int kp=50; // konstanta Proporsional +int ki=20; // konstanta Integral +int kd=70; // konstanta Deferensial + + +/*=========================== Program Utama =============================*/ +void setup() { + //Serial.begin(9600); + + pinMode(selA,OUTPUT); + pinMode(selB,OUTPUT); + pinMode(selC,OUTPUT); + + pinMode(m1a,OUTPUT); + pinMode(m1b,OUTPUT); + pinMode(m2a,OUTPUT); + pinMode(m2b,OUTPUT); +} + +void loop() { + + + linefollower(); + + //show_serial_sensor(); + //delay(1); +} +/*======================= Akhri Program Utama =============================*/ +// +// DEKLARASI PROSEDUR DAN FUNGSI +// +/*========================= Pengendalian Motor =============================*/ +void ff(void) //prosedur untuk bergerak maju +{ + digitalWrite(m1a,HIGH); + digitalWrite(m1b,LOW); + + digitalWrite(m2a,HIGH); + digitalWrite(m2b,LOW); +} + +void brake_left(void) //prosedur untuk mengerem motor kiri +{ + digitalWrite(m1a,LOW); + digitalWrite(m1b,HIGH); + + digitalWrite(m2a,HIGH); + digitalWrite(m2b,LOW); +} + +void brake_right(void) //prosedur untuk mengerem motor kanan +{ + digitalWrite(m1a,HIGH); + digitalWrite(m1b,LOW); + + digitalWrite(m2a,LOW); + digitalWrite(m2b,HIGH); +} + +/*===================== Pengendalian Sensor Garis ============================*/ +void selektor(int value) +{ + if( value == 0) + { + digitalWrite(selA,LOW); + digitalWrite(selB,LOW); + digitalWrite(selC,LOW); + } + else if( value == 1) + { + digitalWrite(selA,HIGH); + digitalWrite(selB,LOW); + digitalWrite(selC,LOW); + } + else if( value == 2) + { + digitalWrite(selA,LOW); + digitalWrite(selB,HIGH); + digitalWrite(selC,LOW); + } + else if( value == 3) + { + digitalWrite(selA,HIGH); + digitalWrite(selB,HIGH); + digitalWrite(selC,LOW); + } + else if( value == 4) + { + digitalWrite(selA,LOW); + digitalWrite(selB,LOW); + digitalWrite(selC,HIGH); + } + else if( value == 5) + { + digitalWrite(selA,HIGH); + digitalWrite(selB,LOW); + digitalWrite(selC,HIGH); + } + else if( value == 6) + { + digitalWrite(selA,LOW); + digitalWrite(selB,HIGH); + digitalWrite(selC,HIGH); + } + else + { + digitalWrite(selA,HIGH); + digitalWrite(selB,HIGH); + digitalWrite(selC,HIGH); + } +} + +bool SensorL(int x) +{ + bool nilai; + selektor(x); + if (analogRead(A0) < batas) + { + nilai = LOW; + } + else + { + nilai = HIGH; + } + + return nilai; +} + +bool SensorR(int x) +{ + bool nilai; + selektor(x); + if (analogRead(A1) < batas) + { + nilai = LOW; + } + else + { + nilai = HIGH; + } + + return nilai; +} + +void show_serial_sensor() //prosedur untuk menampilkan sensor dan PWM ke serial monitor +{ + Serial.print(SensorL(7)); + Serial.print(" "); + + Serial.print(SensorL(6)); + Serial.print(" "); + + Serial.print(SensorL(5)); + Serial.print(" "); + + Serial.print(SensorL(4)); + Serial.print(" "); + + Serial.print(SensorL(3)); + Serial.print(" "); + + Serial.print(SensorL(2)); + Serial.print(" "); + + Serial.print(SensorL(1)); + Serial.print(" "); + + Serial.print(SensorL(0)); + Serial.print(" "); + + Serial.print(SensorR(7)); + Serial.print(" "); + + Serial.print(SensorR(6)); + Serial.print(" "); + + Serial.print(SensorR(5)); + Serial.print(" "); + + Serial.print(SensorR(4)); + Serial.print(" "); + + Serial.print(SensorR(3)); + Serial.print(" "); + + Serial.print(SensorR(2)); + Serial.print(" "); + + Serial.print(SensorR(1)); + Serial.print(" "); + + Serial.print(SensorR(0)); + //Serial.println(); + + Serial.print(" "); + Serial.print(pv); + Serial.print(" "); + Serial.print(motor_ki); + Serial.print(" "); + Serial.println(motor_ka); + + delay(1); +} + +/*=================== Line Follower dan pengendalian PID ===================*/ +void linefollower(void) +{ + if(SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S1"); + pv=(-15); + kode=(-20); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && SensorR(0)) + { + //Serial.println("S16"); + pv=15; + kode=20; + } + else if(SensorL(7) && SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=(-14); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && SensorR(1) && SensorR(0)) + { + pv=14; + kode=0; + ff(); + } + else if(!SensorL(7) && SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S2"); + pv=(-13); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && SensorR(1) && !SensorR(0)) + { + //Serial.println("S15"); + pv=13; + kode=0; + ff(); + } + else if(!SensorL(7) && SensorL(6) && SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=(-12); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && SensorR(2) && SensorR(1) && !SensorR(0)) + { + pv=12; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S3"); + pv=(-11); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S14"); + pv=11; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && SensorL(5) && SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=(-10); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && SensorR(3) && SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=10; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S4"); + pv=(-9); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S13"); + pv=9; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && SensorL(4) && SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=(-8); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && SensorR(4) && SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=8; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S5"); + pv=(-7); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S12"); + pv=7; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && SensorL(3) && SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=(-6); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && SensorR(5) && SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=6; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S6"); + pv=(-5); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S11"); + pv=5; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && SensorL(2) && SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=(-4); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && SensorR(6) && SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=4; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && SensorL(1) && !SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S7"); + pv=(-3); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && !SensorR(7) && SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S10"); + pv=3; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && SensorL(1) && SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=(-2); + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && SensorR(7) && SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + pv=2; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && SensorL(0) && !SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S8"); + pv=(-1); + kode=0; + ff(); + }else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && !SensorL(0) && SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //Serial.println("S9"); + pv=1; + kode=0; + ff(); + } + else if(!SensorL(7) && !SensorL(6) && !SensorL(5) && !SensorL(4) && !SensorL(3) && !SensorL(2) && !SensorL(1) && SensorL(0) && SensorR(7) && !SensorR(6) && !SensorR(5) && !SensorR(4) && !SensorR(3) && !SensorR(2) && !SensorR(1) && !SensorR(0)) + { + //CENTER + pv=0; + kode=0; + ff(); + } + + + if(kode == (-20)) // keadaan sensor keluar garis dari sebelah kiri + { + pv=(0); + brake_left(); + //delay(10); + } + else if(kode == 20) //keadaan sensor keluar garis dari sebelah kanan + { + pv=(0); + brake_right(); + // delay(10); + } + + error = sp + pv; + + pid = (kp*error)+(ki/10*(error+error0)*ts)+(kd/ts*(error-error0)); + error0=error; //error pada keadaan lampau + + + //melakukan perhitungan PWM dari nilai PID yang didapat + motor_ki = 255 + pid; + if (motor_ki > 255) + { + motor_ki=255; + } + else if (motor_ki < 0) + { + motor_ki=0; + } + else + { + motor_ki = 255+pid; + } + + motor_ka = 255 - pid; + if (motor_ka > 255) + { + motor_ka=255; + } + else if(motor_ka < 0) + { + motor_ka = 0; + } + else + { + motor_ka = 255-pid; + } + + + //output berupa sinyal PWM ke motor driver + analogWrite(pwm1,motor_ki); + analogWrite(pwm2,motor_ka); + + delay(ts); //delay untuk time sampling +} \ No newline at end of file