File content as of revision 0:b31e672064ea:

/*=================== 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
}