KRAI 2016
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 |
--- /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