line follower

#include <LiquidCrystal_SR_LCD3.h>
const int PIN_LCD_STROBE  = 2;
const int PIN_LCD_DATA  = 3;
const int PIN_LCD_CLOCK = 4;
LiquidCrystal_SR_LCD3 lcd(PIN_LCD_DATA, PIN_LCD_CLOCK, PIN_LCD_STROBE);
/*
#include <BlynkSimpleStream.h>

char auth[] = "W9KxUhtSLkGGAyxYmiDImNjGC0DKiKmF";

WidgetLED led_obstacle(V1);
WidgetLED led_auto(V15);
WidgetLED led_engine(V13);

BlynkTimer timer; // Announcing the timer
*/
//#include <SoftwareSerial.h>

//SoftwareSerial mySerial(0, 1); // RX, TX


/* MOTOR */
int mt1=5, mt2=6, mt3=12, mt4=8;
#define enA NULL
#define in1 8
#define in2 6
#define enB NULL
#define in3 12
#define in4 5

int motorSpeedA = 0;
int motorSpeedB = 0;

// LS = Left Sensor , RS = Right Sensor
// LS3 - LS2 - LS1 -- RS1 - RS2 - RS3
int sensorPins[] = {A5 , A4 , A3 , A2 , A1, A0};

float sensorValue[6];
float sensorValueMin[6] = {1024, 1024, 1024, 1024, 1024, 1024};
float sensorValueMax[6] = {0, 0, 0, 0, 0, 0};
float threshold[6] = {10, 10, 10, 10, 10, 10};
float sensorValueAvg[6] = {512, 512, 512, 512, 512, 512};
boolean digitalValue[6];
short error = 0;

/* BUTTON SW3 SW4 SW5 SW6*/
int sw3=8, sw4=13, sw5=11, sw6=12;
/* END BUTTON */

/* Sensor Ping */
const int trigPin = 9;
const int echoPin = 10;

int push=0, ok=0;

int period = 300;
unsigned long time_now = 0;

int manual_mode = 0;
int engine_on = LOW;
/*
BLYNK_CONNECTED(){
  Blynk.syncVirtual(V0);

  Blynk.syncVirtual(V20);
  Blynk.syncVirtual(V21);
  Blynk.syncVirtual(V22);
  Blynk.syncVirtual(V23);
}

int forward = LOW,backward = LOW,left = LOW,right = LOW;

BLYNK_WRITE(V20){
  forward = param.asInt();
}

BLYNK_WRITE(V21){
  backward = param.asInt();
}

BLYNK_WRITE(V22){
  left = param.asInt();
}

BLYNK_WRITE(V23){
  right = param.asInt();
}

int motor_speed_r = 0,motor_speed_l = 0;
// LEFT MOTOR SPEED MANUAL
BLYNK_WRITE(V11){
  motor_speed_r = param.asInt();
}

// RIGHT MOTOR SPEED MANUAL
BLYNK_WRITE(V12){
  motor_speed_l = param.asInt();
}

BLYNK_WRITE(V0){
  int buttonState = param.asInt();
  engine_on = buttonState;
}

BLYNK_WRITE(V10) {
  switch(param.asInt()){
    case 1: // Manual
     manual_mode = 1;
     break;
    case 2: // AUTO
     manual_mode = 0;
     break;
     default:
     manual_mode = 1;
     break;
  }
}*/

void setup() {
  Serial.begin(9600);
  //Blynk.begin(auth, Serial);

  lcd.clear();
  //timer.setInterval(1000L, sensorDataSend);

  pinMode(7, OUTPUT);
  for (short i = 0; i < 6; i++)
  {
    pinMode(sensorPins[i], INPUT);
  }
  /* BUTTON */
  //pinMode(sw3, INPUT);
  pinMode(sw4, INPUT);
  pinMode(sw5, INPUT);
  //pinMode(sw6, INPUT);
  /* END BUTTON */

  /* MOTOR */
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  /* END MOTOR */


  /* SENSOR */

  /* END SENSOR */
  lcd.begin(16, 2);
  lcd.home();
  lcd.setCursor(4,0);
  lcd.print("*Welcome*"); delay(1000);
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Line  Follower");
  lcd.setCursor(6,1);
  lcd.print("v1.0");
  delay(3000);
  lcd.clear();
}

int SPD = 100;
float PID;
float kp = 10.0, ki = 5.0, kd = 0.5;
float last_error, integ, deriv;

unsigned long time_now2 = 0;

bool Prox = false;
int kecepatan = 100;

void loop() {

  updateSensor();
  updateMinMax();

  //calculateError();

  //ShowSensor();


  /*digitalWrite(in1, HIGH);
  digitalWrite(in3, HIGH);
  analogWrite(enA, motorSpeedA);
  analogWrite(enB, motorSpeedB);*/

  /* ERROR
   *  -4 : White
   *  -2 : Turn Left 100%
   *  -1 : Turn Left 50%
   *   0 : Forward
   *   1 : Turn Right 50%
   *   2 : Turn Right 100%
   */
  error = 0;
  short sum = 0;
  for (short i = 0; i < 6; i++)
  {
    sum += digitalValue[i];
    error += digitalValue[i] * (i + 1);
  }
  if(error < 0 || error > 0){
  error = error / sum - 3;
  }
  /*
  PID = error * kp + integ * ki + deriv * kd;
  integ += error;
  deriv = error - last_error;
  last_error = error;*/


 if(error == -4){
    smotor();
  }else if(error == -2){
    fmotor();
    fmotorpwm(200, 0);
    delay(50);
  }else if(error == -1){
    fmotor();
    fmotorpwm(150, 0);
  }else if(error == 0){
    fmotor();
    fmotorpwm(130, 130);
  }else if(error == 1){
    fmotor();
    fmotorpwm(0, 150);
  }else if(error == 2){
    fmotor();
    fmotorpwm(0, 200);
    delay(50);
  }

  //PROXIMITY SENSOR
  long duration, cm;

    pinMode(trigPin, OUTPUT);
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    pinMode(echoPin, INPUT);
    duration = pulseIn(echoPin, HIGH);
    cm = microsecondsToCentimeters(duration);
    if(cm<5){
      ok=1;
    }else{
      ok=0;
    }

  /*if(ok==1){
    bmotor();
    delay(200);
    smotor();
    fmotor();
    fmotorpwm(100, 0);
    delay(600);
    fmotorpwm(100, 100);
    delay(100);
    fmotorpwm(0, 100);
    delay(460);
    fmotorpwm(100, 100);
    delay(450);
    fmotorpwm(0, 100);
    delay(500);
    fmotorpwm(100, 100);
    delay(50);
    fmotorpwm(100, 0);
    delay(600);
    ok=0;
  }else{
    smotor();
  }*/
}

void fmotor(){
  analogWrite(in2, 255);
  analogWrite(in4, 255);
}

void bmotor(){
  digitalWrite(in1, HIGH);
  digitalWrite(in3, HIGH);
}

void smotor(){
  digitalWrite(in1, LOW);
  digitalWrite(in3, LOW);
  analogWrite(in2, 0);
  analogWrite(in4, 0);
}

void fmotorpwm(int motora, int motorb){
  analogWrite(in2, motora);
  analogWrite(in4, motorb);
}

void ShowPing(){
  if(millis() >= time_now + period){
    lcd.clear();
    time_now += period;
    long duration, cm;

    pinMode(trigPin, OUTPUT);
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    pinMode(echoPin, INPUT);
    duration = pulseIn(echoPin, HIGH);
    cm = microsecondsToCentimeters(duration);

    lcd.setCursor(0,0);
    lcd.print(cm);
    lcd.print(" CM");
  }
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}

void ShowSensor(){
  if(millis() >= time_now + period){
    lcd.clear();
    time_now += period;
  lcd.setCursor(0,0);
  lcd.print(analogRead(A5));
  lcd.setCursor(5,0);
  lcd.print(analogRead(A4));
  lcd.setCursor(10,0);
  lcd.print(analogRead(A3));

  lcd.setCursor(0,1);
  lcd.print(analogRead(A2));
  lcd.setCursor(5,1);
  lcd.print(analogRead(A1));
  lcd.setCursor(10,1);
  lcd.print(analogRead(A0));

  }
  //A5 , A4 , A3 , A2 , A1, A0
}

void ShowPID(){
  updateSensor();
  updateMinMax();
  calculateError();

  // Calculate PID
  PID = error * kp + integ * ki + deriv * kd;
  integ += error;
  deriv = error - last_error;
  last_error = error;
  lcd.print(SPD);
  lcd.print("  ");
  lcd.print(PID);

  //rotate Motors
 /* LeftMotor.rotate(SPD + PID);
  RightMotor.rotate(SPD - PID); */
  delay(100);
  lcd.clear();
}

void calculateError()
{
  error = 0;
  short sum = 0;
  for (short i = 0; i < 6; i++)
  {
    sum += digitalValue[i];
    error += digitalValue[i] * (i + 1);
  }
  error = error / sum - 3;
}


void updateSensor()
{
  for (short i = 0; i < 6; i++)
  {
    sensorValue[i] = analogRead(sensorPins[i]);
    digitalValue[i] = sensorValue[i] > (sensorValueAvg[i] + threshold[i]);
  }
}
void updateMinMax()
{
  for (short i = 0; i < 6; i++)
  {
    sensorValueMin[i] = sensorValue[i] < sensorValueMin[i] ? sensorValue[i] : sensorValueMin[i];
    sensorValueMax[i] = sensorValue[i] > sensorValueMax[i] ? sensorValue[i] : sensorValueMax[i];
    sensorValueAvg[i] = (sensorValueMin[i] + sensorValueMax[i]) / 2; // update the avarage value
    threshold[i] = (sensorValueMax[i] - sensorValueMax[i]) * 0.1; // Threshold is 10% of total sensor range
  }
}