// UKMARSBOTDHlinefollowV3 in UKMARSBOT folder
// proportional  control
// 10:1 gearbox plus encoders
// Input pins:
const int rside = A0; // left side sensor input
const int rfront = A1; //front left line sensor input
const int lfront = A2; //front right left sensor input
const int lside = A3; // right side sensor input
const int sens1 = A4; // unassigned sensor input 1
const int sens2 = A5; // unassigned sensor input 2
const int fourwayswitch = A6; // input from function switch
const int battery = A7; // input for battery measurement
const int Receive = 0; //Receive pin
const int Transmit = 1; //Transmit pin
const int m1encoder1 = 2; // motor 1 encoder 1 input interrupt pin
const int m1encoder2 = 4; // motor 1 encoder 2 input
const int m2encoder1 = 3; // motor 2 encoder 1 input interrupt pin
const int m2encoder2 = 5; // motor 2 encoder 2 input
// Output pins:
const int sensorLED1 = 6; // 1st diagnostic LED on sensor board
const int lmotorDIR = 7; //Left motor dirn input 1
const int rmotorDIR = 8; //Right motor dirn input 3
const int lmotorPWM = 9; //Left motor PWN pin
const int rmotorPWM = 10; //Right motor PWN pin
const int sensorLED2 = 11; // 2nd diagnostic LED on sensor board
const int trigger = 12; // trigger for sensor LEDs
const int LED13 = 13; // ext LED Red
//Inputs
int rfrontsens = 0; //Right front sensor value
int lfrontsens = 0; //Left front sensor value
int rsidesens = 0; //Right side sensor value
int lsidesens = 0; //Left side sensor value
int sensdiff = 0; //Difference between front sensors
int error = 0;
int batteryvolts = 0; // battery voltage reading
int batterycalc = 0; // working field
int switchvoltage = 0; // analogue value coming back from reading function or 4 way switch
int fnswvalue = 0; // value (in range 0 to 16) of 4 way function switch
int posn = 0; // if on line or off it and which side
//Motor variables
int basespeed = 60; //Base speed (constant)
int rightspeed = 70; //Right motor speed
int leftspeed = 70; //Left motor speed
int turnspeed = 10; // calibration spin speed
int i=0; // loop counter
int adjustamount = 0; // amount to adjust the motors by
int threshold = 800; // mid poiint between back and white front sensor values
int rfhigh = 0;
int rflow = 1023;
int lfhigh = 0;
int lflow = 1023;
int seeblack = 900; // level at which pretty much black seen
int volts = 0; //battery voltage 
int switchvalue = 0; // value back from 4 way switch
int speedvalue = 10; //
long starttime;
long currtime;
long howlongtime;

 
void setup() { // this code will run once on startup
  pinMode(lmotorDIR, OUTPUT);
  pinMode(rmotorDIR, OUTPUT);
  pinMode(lmotorPWM, OUTPUT);
  pinMode(rmotorPWM, OUTPUT);
  pinMode(sensorLED1, OUTPUT);
  pinMode(sensorLED2, OUTPUT);
  pinMode(trigger, OUTPUT);
  pinMode(LED13, OUTPUT);
  pinMode(m1encoder1, INPUT);
  pinMode(m1encoder2, INPUT);
  pinMode(m2encoder1, INPUT);
  pinMode(m2encoder2, INPUT);
  Serial.begin(9600); // set up serial monitor comms on USB
}
 
 
void loop()
{
  buttonwait(); // wait for function button to be pressed
  delay (200); // debounce switch
  functionswitch(); // read function switch value after button released
  Serial.print (" switch ");
  Serial.print (switchvalue);
  Serial.print (" ");
  Serial.println(fnswvalue);
  if (fnswvalue == 0) linefollow(); // bang bang line follower routine
  if (fnswvalue == 1) phototest();
  if (fnswvalue == 2) dragrace();
  if (fnswvalue == 4) bluetooth();  
  if (fnswvalue == 8) motortest();
  
}

void calibrate() // this sets high and low values for front 2 sensors
{
  // turn clockwise until left side sees white then black
  turnspeed = 15;
  digitalWrite(rmotorDIR, LOW);  // set right motor forward
  digitalWrite(lmotorDIR, HIGH);  // set left motor forward
  analogWrite(rmotorPWM, turnspeed); // set right motor speed
  analogWrite(lmotorPWM, turnspeed); // set left motor speed
  photoread();
  while (lsidesens > threshold) testhighlow();
  while (lsidesens < threshold) testhighlow();
  // straighten up

  // turn anti-clockwise  until right side sees white then black
  digitalWrite(rmotorDIR, HIGH);  // set right motor forward
  digitalWrite(lmotorDIR, LOW);  // set left motor forward
  while (rsidesens > threshold) testhighlow();
  while (rsidesens < threshold) testhighlow();
  // straighten up
  digitalWrite(rmotorDIR, LOW);  // set right motor forward
  digitalWrite(lmotorDIR, HIGH);  // set left motor forward
  while (lfrontsens > threshold) 
    {photoread();
    }
return;
}
void testhighlow()
{  photoread();
  if (rfrontsens > rfhigh)rfhigh = rfrontsens;
  if (rfrontsens < rflow)rflow = rfrontsens;
  if (lfrontsens > lfhigh)lfhigh = lfrontsens;
  if (lfrontsens < lflow)lflow = lfrontsens;
}

void linefollow() //proportional line following plus extra correctin if off the line
{
  calibrate();
  basespeed = 25; // straight line speed value for this line follower
  adjustamount = 15; // amount to increase and decrease motors for tracking
  rightspeed = basespeed;
  leftspeed = basespeed;
  threshold = 700;
  seeblack = 850;
  digitalWrite(rmotorDIR, HIGH);  // set right motor forward
  digitalWrite(lmotorDIR, HIGH);  // set left motor forward
  analogWrite(rmotorPWM, rightspeed); // set right motor speed
  analogWrite(lmotorPWM, leftspeed); // set left motor speed
 
while(true)
  {
  photoread(); // read photosensors
  sensdiff = (rfrontsens - lfrontsens) / 20;
  if ((rfrontsens > seeblack) and (lfrontsens > seeblack)) // gone off the line code
      {if (posn == 1)
        {rightspeed = basespeed + adjustamount;  //
        leftspeed = 0;  //
        analogWrite(rmotorPWM, rightspeed); // set right motor speed
        analogWrite(lmotorPWM, leftspeed); // set left motor speed == 1)
        }
        if (posn == 2)
        {rightspeed = 0;  //
        leftspeed = basespeed + adjustamount;  //
        analogWrite(rmotorPWM, rightspeed); // set right motor speed
        analogWrite(lmotorPWM, leftspeed); // set left motor speed == 1)
        }
      }  // end of gone off the line code
  else // normal line tracking code here
  {   
  if (rfrontsens > lfrontsens) //
      {posn = 1; // indicates which side of line we are seeing most of
       rightspeed = basespeed + sensdiff;  //
       leftspeed = basespeed - sensdiff;  //
       checkspeed();
       analogWrite(rmotorPWM, rightspeed); // set right motor speed
       analogWrite(lmotorPWM, leftspeed); // set left motor speed
       digitalWrite(sensorLED1, HIGH);
       digitalWrite(sensorLED2, LOW);
      }
  if (rfrontsens < lfrontsens) //
      {posn = 2; // indicates which side of line we are seeing most of
       rightspeed = basespeed + sensdiff;  //
       leftspeed = basespeed - sensdiff;  //
       checkspeed();
       analogWrite(rmotorPWM, rightspeed); // set right motor speed
       analogWrite(lmotorPWM, leftspeed); // set left motor speed
       digitalWrite(sensorLED1, LOW);
       digitalWrite(sensorLED2, HIGH);
      }
  if (rfrontsens == lfrontsens) //
      {rightspeed = basespeed ;  //
       leftspeed = basespeed ;  //
       analogWrite(rmotorPWM, rightspeed); // set right motor speed
       analogWrite(lmotorPWM, leftspeed); // set left motor speed
       digitalWrite(sensorLED1, HIGH);
       digitalWrite(sensorLED2, HIGH);
      }
  } 
} // end of while loop
}

void checkspeed()
  { if (rightspeed > 250)rightspeed = 250;
    if (rightspeed < 1)rightspeed = 1;
    if (leftspeed > 250)leftspeed = 250;
    if (leftspeed < 1)leftspeed = 1;
  }
 
void dragrace() //proportional line following plus extra correctin if off the line
{
  basespeed = 80; // straight line speed value for this line follower
  adjustamount = 10; // amount to increase and decrease motors for tracking
  rightspeed = basespeed;
  leftspeed = basespeed;
  threshold = 800;
  seeblack = 900;
  starttime = millis();
  digitalWrite(rmotorDIR, HIGH);  // set right motor forward
  digitalWrite(lmotorDIR, HIGH);  // set left motor forward
  analogWrite(rmotorPWM, rightspeed); // set right motor speed
  analogWrite(lmotorPWM, leftspeed); // set left motor speed
 
while(true)
  {
  photoread(); // read photosensors
  if ((rsidesens < 500) & (lsidesens < 500)) // look for white line across track
      {currtime = millis();
      howlongtime = currtime - starttime;
      if (howlongtime > 2000) slowstop();
      }
  sensdiff =  rsidesens - lsidesens; // proportional control
  if(sensdiff <0) sensdiff = sensdiff * (-1);
  adjustamount = sensdiff/5; 
  adjustamount = adjustamount - (error / 4);
  
  error = sensdiff/5;
  
  if ((rfrontsens > seeblack) and (lfrontsens > seeblack)) // gone off the line code
      {if (posn == 1)
        {rightspeed = basespeed + adjustamount;  //
        leftspeed = basespeed/2;  //
        analogWrite(rmotorPWM, rightspeed); // set right motor speed
        analogWrite(lmotorPWM, leftspeed); // set left motor speed == 1)
        }
        if (posn == 2)
        {rightspeed = basespeed/2;  //
        leftspeed = basespeed + adjustamount;  //
        analogWrite(rmotorPWM, rightspeed); // set right motor speed
        analogWrite(lmotorPWM, leftspeed); // set left motor speed == 1)
        }
      }  // end of gone off the line code
  else // normal line tracking code here
  {   
  if (rfrontsens > lfrontsens) //
      {posn = 1; // indicates which side of line we are seeing most of
       rightspeed = basespeed + adjustamount;  //
       leftspeed = basespeed - adjustamount;  //
       analogWrite(rmotorPWM, rightspeed); // set right motor speed
       analogWrite(lmotorPWM, leftspeed); // set left motor speed
       digitalWrite(sensorLED1, HIGH);
       digitalWrite(sensorLED2, LOW);
      }
  if (rfrontsens < lfrontsens) //
      {posn = 2; // indicates which side of line we are seeing most of
       rightspeed = basespeed - adjustamount;  //
       leftspeed = basespeed + adjustamount;  //
       analogWrite(rmotorPWM, rightspeed); // set right motor speed
       analogWrite(lmotorPWM, leftspeed); // set left motor speed
       digitalWrite(sensorLED1, LOW);
       digitalWrite(sensorLED2, HIGH);
      }
  if (rfrontsens == lfrontsens) //
      {rightspeed = basespeed ;  //
       leftspeed = basespeed ;  //
       analogWrite(rmotorPWM, rightspeed); // set right motor speed
       analogWrite(lmotorPWM, leftspeed); // set left motor speed
       digitalWrite(sensorLED1, HIGH);
       digitalWrite(sensorLED2, HIGH);
      }
  } 
} // end of while loop
}
 
 
void buttonwait(){ // waits until tactile button is pressed
digitalWrite(LED13, HIGH); // put LED on
switchvoltage = analogRead(fourwayswitch);
while (switchvoltage < 1000){ // while button not pressed
switchvoltage = analogRead(fourwayswitch);
}
while (switchvoltage > 1000){ // while button pressed
switchvoltage = analogRead(fourwayswitch);
}
digitalWrite(LED13, LOW);
}
 
 
void functionswitch(){
switchvoltage = analogRead(fourwayswitch);
fnswvalue = 16;
if (switchvoltage > 87) fnswvalue = fnswvalue - 1;
if (switchvoltage > 171) fnswvalue = fnswvalue - 1;
if (switchvoltage > 242) fnswvalue = fnswvalue - 1;
if (switchvoltage > 310) fnswvalue = fnswvalue - 1;
if (switchvoltage > 369) fnswvalue = fnswvalue - 1;
if (switchvoltage > 411) fnswvalue = fnswvalue - 1;
if (switchvoltage > 449) fnswvalue = fnswvalue - 1;
if (switchvoltage > 492) fnswvalue = fnswvalue - 1;
if (switchvoltage > 532) fnswvalue = fnswvalue - 1;
if (switchvoltage > 556) fnswvalue = fnswvalue - 1;
if (switchvoltage > 579) fnswvalue = fnswvalue - 1;
if (switchvoltage > 601) fnswvalue = fnswvalue - 1;
if (switchvoltage > 622) fnswvalue = fnswvalue - 1;
if (switchvoltage > 638) fnswvalue = fnswvalue - 1;
if (switchvoltage < 654) fnswvalue = fnswvalue - 1;
if (switchvoltage > 654) fnswvalue = 0;
if (switchvoltage > 999) fnswvalue = 16;
}
 
void phototest()
{
 while (1){ // wait loop
digitalWrite(trigger, HIGH);
delayMicroseconds(150); // wait for LED to light up fully
rfrontsens = analogRead (rfront); // read right front photo sensor input
lfrontsens = analogRead (lfront); // read left front photo sensor input
rsidesens = analogRead (rside); // read right side photo sensor input
lsidesens = analogRead (lside); // read left side photo sensor input
// digitalWrite(trigger, LOW);
Serial.print("LS ");
 Serial.print(lsidesens); // send photo sensor values to serial monitor
Serial.print(" LF ");
 Serial.print(lfrontsens);
 Serial.print(" RF ");
 Serial.print(rfrontsens);
 Serial.print(" RS ");
 Serial.print(rsidesens);
 Serial.println(" ");
delay (100); }
}

void motortest()
{  Serial.println(" Motortest ");
  digitalWrite(lmotorDIR, HIGH); // set left motor direction
  digitalWrite(rmotorDIR, HIGH); // set right motor direction
  rightspeed = 50; // set the required right motor speed in range 0 to 255
  leftspeed = 50; // set the required left motor speed in range 0 to 255
  analogWrite(rmotorPWM, rightspeed); // set right motor speed
  analogWrite(lmotorPWM, leftspeed); // set left motor speed
  delay(3000);
  stopmotors();
  while(true){
    delay(500); 
    digitalWrite(LED13,LOW);
    delay(500); 
    digitalWrite(LED13,HIGH);  
    }
  
}
 void motorfwd() // starts both motors in direction forward
  {if (rightspeed < 0) rightspeed = 0;
  if (rightspeed > 255) rightspeed = 255;
  if (leftspeed < 0) leftspeed = 0;
  if (leftspeed > 255) leftspeed = 255;
  digitalWrite(rmotorDIR, HIGH);  // set right motor forward
  digitalWrite(lmotorDIR, HIGH);  // set left motor forward
  analogWrite(rmotorPWM, rightspeed); // set right motor speed
  analogWrite(lmotorPWM, leftspeed); // set left motor speed
  return;
  }
 void motorback() // starts both motors in direction forward
  {if (rightspeed < 0) rightspeed = 0;
  if (rightspeed > 255) rightspeed = 255;
  if (leftspeed < 0) leftspeed = 0;
  if (leftspeed > 255) leftspeed = 255;
  digitalWrite(rmotorDIR, LOW);  // set right motor backward
  digitalWrite(lmotorDIR, LOW);  // set left motor backward
  analogWrite(rmotorPWM, rightspeed); // set right motor speed
  analogWrite(lmotorPWM, leftspeed); // set left motor speed
  return;
  }

void slowstop()
  {digitalWrite(lmotorDIR, HIGH); // set left motor direction
  digitalWrite(rmotorDIR, HIGH); // set right motor direction
  rightspeed = 40; // set the required right motor speed in range 0 to 255
  leftspeed = 40; // set the required left motor speed in range 0 to 255
  analogWrite(rmotorPWM, rightspeed); // set right motor speed
  analogWrite(lmotorPWM, leftspeed); // set left motor speed
  delay(500);
  stopmotors();
  while(true){
    delay(500); 
    digitalWrite(LED13,HIGH);
    delay(500); 
    digitalWrite(LED13,LOW);  
    }
  }
 
void stopmotors()
  {
  digitalWrite(lmotorDIR, HIGH); // set left motor direction
  digitalWrite(rmotorDIR, HIGH); // set right motor direction
  rightspeed = 0; // set the required right motor speed in range 0 to 255
  leftspeed = 0; // set the required left motor speed in range 0 to 255
  analogWrite(rmotorPWM, rightspeed); // set right motor speed
  analogWrite(lmotorPWM, leftspeed); // set left motor speed
  }
 
void batterycheck() { // function to check battery voltage is over 6 volts
  int batteryread = analogRead(battery); // read battery voltage
  if (batteryread > 614) return; // check if over 6 volts and return if it is
  analogWrite(rmotorPWM, 0); // set right motor speed to stop
  analogWrite(lmotorPWM, 0); // set left motor speed to stop
  while (1) { // endless loop
    digitalWrite(LED13, HIGH); // switch on LED
    delay (100); // wait 1/10 second
    digitalWrite(LED13, LOW); // switch off LED
    delay (500); // wait 1/2 second
  }
} // end of batterycheck function
 
void photoread(){
// read both line sensors then the two side sensors
digitalWrite(trigger, HIGH);
delayMicroseconds(150); // wait for LED to light up fully
lfrontsens = analogRead(lfront);
rfrontsens = analogRead(rfront);
rsidesens = analogRead (rside); // read right side photo sensor input
lsidesens = analogRead (lside); // read left side photo sensor input
 digitalWrite(trigger, LOW);
// light the right hand sensor LED if white line seen by left sensor
if (rfrontsens < 850)
{digitalWrite (sensorLED2, HIGH);
}
else
 {digitalWrite (sensorLED2, LOW);
}
// light the left hand sensor LED if white line seen by left sensor
if (lfrontsens < 850)
{digitalWrite (sensorLED1, HIGH);
}
else
 {digitalWrite (sensorLED1, LOW);
}
}
 
 
void switchread()
{
  //switchvalue = analogRead(fourwayswitch);
  functionswitch();
  Serial.print(" switchvalue");
  Serial.print(" ");
  Serial.print(switchvalue);
  Serial.println(" ");
  delay (500);
  if (fnswvalue == 1) {
    linefollow();
  }
}
void bluetooth()
{ digitalWrite(LED13, HIGH);
  //Serial.println("bluetooth called");
 
  while (true)
    {delay (30); 
    String t; // empty string to store bluetooth messages from phone
    while (Serial.available() > 0) {
      t = t + (char)Serial.read();
      digitalWrite(sensorLED1, HIGH);
      }
   
    if (t.length() > 0)  {
        //Serial.print("Received ");
        //Serial.println(t); 
        digitalWrite(LED13, LOW);
        digitalWrite(sensorLED1, HIGH);
        digitalWrite(sensorLED2, HIGH);  
         
     if (t == "fstr") {  
        rightspeed = rightspeed + 10;
        leftspeed = leftspeed + 10;
        digitalWrite(LED13, HIGH);
        digitalWrite(sensorLED1, HIGH);
        digitalWrite(sensorLED2, HIGH);
        motorfwd(); 
        } 
      if (t == "frwd") {
        rightspeed = 30;
        leftspeed = 30;
        digitalWrite(LED13, LOW);
        digitalWrite(sensorLED1, HIGH);
        digitalWrite(sensorLED2, HIGH);
        motorfwd();
        }
      if (t == "left") {
        rightspeed = rightspeed +5;
        leftspeed = leftspeed - 5;
        digitalWrite(LED13, LOW);
        digitalWrite(sensorLED1, LOW);
        digitalWrite(sensorLED2, HIGH);
        motorfwd();
        } 
      if (t == "stop") {
        digitalWrite(LED13, LOW);
        digitalWrite(sensorLED1, LOW);
        digitalWrite(sensorLED2, LOW);
        stopmotors(); 
        }
      if (t == "rght") {
        rightspeed = rightspeed - 5;
        leftspeed = leftspeed + 5;
        digitalWrite(LED13, LOW);
        digitalWrite(sensorLED1, HIGH);
        digitalWrite(sensorLED2, LOW);
        motorfwd();
        }
      if (t == "back") {
        rightspeed = 20;
        leftspeed = 20;
        digitalWrite(LED13, LOW);
        digitalWrite(sensorLED1, HIGH);
        digitalWrite(sensorLED2, HIGH);
        motorback(); 
        }
      if (t == "slow") {
        rightspeed = rightspeed + 5;
        leftspeed = leftspeed + 5;
        digitalWrite(LED13, HIGH);
        digitalWrite(sensorLED1, HIGH);
        digitalWrite(sensorLED2, HIGH);
       motorback();
        }

        
      }
    }
}