roboLine 0.2

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,16,2);

int IR_SENSOR = A1;
int LED = 13;
int IR_VALUE = 0;
int DIRECTION;
int speed     = 40;
int speeDiff  = 0;

int enA       = 9;
int inA1      = 8;
int inA2      = 7;
int enB       = 3;
int inB3      = 5;
int inB4      = 4;
int buttonPin = 12;

float Pid       = 0.09;

void setup() {
  Serial.begin(9600);
  pinMode(IR_SENSOR, INPUT);
  pinMode(LED,  OUTPUT);
  pinMode(enA,  OUTPUT);
  pinMode(enB,  OUTPUT);
  pinMode(inA1, OUTPUT);
  pinMode(inA2, OUTPUT);
  pinMode(inB3, OUTPUT);
  pinMode(inB4, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);   //Configuration push button
  digitalWrite(inA1, LOW);
  digitalWrite(inA2, LOW);
  digitalWrite(inB3, LOW);
  digitalWrite(inB4, LOW);
  lcd.init();
  lcd.clear();
  lcd.backlight();

  writeToScreen("RoboGold Init...", "STARTING!");
  delay(1000);
  roboRun();
}

void roboRun () {
  int mid = 377;
  writeToScreen("To recalibrate", "");
  delay(500);
  writeToScreen("press now,", "...or just wait");
  unsigned long startTimer = millis();
  for(;startTimer + 2000 > millis();){
    if(!digitalRead(buttonPin)) {
      mid = calibrate();
    }
  }

  writeToScreen("I'm Ready!", "Hit me!");
  while(digitalRead(buttonPin)) {delay(1);}
  delay(100);

  while (true){
    drive(mid);
    /*if(!digitalRead(buttonPin)){
      writeToScreen("stopping", "");
      digitalWrite(inA2, LOW);
      digitalWrite(inB4, LOW);
      return;
    }*/
  }
  writeToScreen("I'm done", ";-)");
}

void drive (int mid) {
  int correction = (mid - analogRead(IR_SENSOR))*Pid;
  delay(5);
  analogWrite(enA, speed - speeDiff + correction);
  analogWrite(enB, speed + speeDiff - correction);
  digitalWrite(inA2, HIGH);
  digitalWrite(inB4, HIGH);
}

int calibrate(){
  writeToScreen("Starting to", "calibrate");
  unsigned long startTimer = millis();
  int black = analogRead(IR_SENSOR), white = analogRead(IR_SENSOR), IR_Value;
  for(;startTimer + 4000 > millis();){
    IR_Value = analogRead(IR_SENSOR);
    if(IR_Value > black) { black = IR_Value; }
    if(IR_Value < white) { white = IR_Value; }
    writeToScreen("Calibratint...", "b: "+(String)(black)+", w: "+(String)(white));
    delay(100);
  }
  return (black - white)/2;
}

void writeToScreen(String line1, String line2){
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(line1);
  lcd.setCursor(0,1);
  lcd.print(line2);
}


int calibrate2 () {
  writeToScreen("Place sensor","over black");
  delay(200);
  while(digitalRead(buttonPin)) {delay(1);}

  int black = analogRead(IR_SENSOR);
  writeToScreen("Comleted!", " ");
  delay(1000);

  writeToScreen("Place sensor", "over white");
  while(digitalRead(buttonPin)) {delay(1);}

  int white = analogRead(IR_SENSOR);
  writeToScreen("Comleted!", " ");
  delay(500);
  lcd.clear();

  return (black - white)/2;
}
void loop() {}