/*
Driving 2 motors, varying speed with PWM, using an L293D dual H bridge driver

NOTE - when uploading don't put power on Atmel until the "Binary Sketch size"
       message appears, otherwise you will get an upload(synchronisation)error
 
 The circuit:
 * LED attached from digital pin 7 to 5v so low lights the LED.
 * Right motor PWM 1/2 enable drive on Arduino pin 9 with
 *   1A and 2A direction control outputs on pins 4 and 8
  * Left motor PWM 3/4 enable drive on Arduino pin 10 with
 *   3A and 4A direction control outputs on pins 2 and 3
 * Analogue inputs from front sensors on analogue inputs 2,3,4,&5
 * Analogue input from right motor encoder on analogue input 0
 
 */

// set up pins for right motor
int rightMotorPWM = 9;    // motor PWM
int rightMotorForward = 4;
int rightMotorReverse = 8;
// set up pins for left motor
int leftMotorPWM = 10;    // motor PWM
int leftMotorForward = 3;
int leftMotorReverse = 2;
// set initial PWM values - stopped
int pwmValue = 0;  // variable for base value PWM percentage
int pwmValRight = 0;  // variable for Right motor PWM percentage
int pwmValLeft = 0;  // variable for Left motor PWM percentage
int pwmPrev = 0;  // previous base PWM value
int pwmRightPrev = 0;  // previous Right motor PWM value
int pwmLeftPrev = 0;  // previous Left MotorPWM value

// set up encoder pins & fields
int sensorPin = A0;    // input from right motor encoder phototransistor
int sensorValue = 0;  // variable to store the value coming from the sensor
int maxSensorValue = 0;  // variable to store the highest value coming from the sensor
int minSensorValue = 1023;  // variable to store the lowest value coming from the sensor
int midSensorValue = 0;  // variable to store the mid point value for the sensor

int  newval = 0;    // new value of encoder ready to process (after state change)
int  cval = LOW;  // whether encoder is currently seeing low or high
int  highcount = 0; // count of no of times seen high on encoder
int  lowcount = 0; // count of no of times seen low on encoder
int  encount = 0; // count on encoder when state changes

// set up digital pin 7 for use by LED
int ledPin = 7;       // Green LED on Digital Pin 7 - low lights it


// set up pins for the 4 line sensors on A2 to A5
int sensorA5Pin = A5;    // input from left front sensor
int sensorLeft = 0;  // variable to store the value coming from the sensor
int sensorA4Pin = A4;    // input from left middle sensor
int sensorMidLeft = 0;  // variable to store the value coming from the sensor
int sensorA3Pin = A3;    // input from right middle sensor
int sensorMidRight = 0;  // variable to store the value coming from the sensor
int sensorA2Pin = A2;    // input from right sensor
int sensorRight = 0;  // variable to store the value coming from the sensor
int lastseen = 0;  // variable for which middle sensor last saw white


void setup()  {         // do this routine just once
  
  pinMode(ledPin, OUTPUT);              // define LED pin as an output 
  
  // define outputs for motor direction being sent to L293D 
  pinMode(rightMotorForward, OUTPUT);    
  pinMode(rightMotorReverse, OUTPUT);
  pinMode(leftMotorForward, OUTPUT);
  pinMode(leftMotorReverse, OUTPUT);

  // set initial values of motor directions  
  digitalWrite(rightMotorForward, HIGH);
  digitalWrite(rightMotorReverse, LOW);
  digitalWrite(leftMotorForward, HIGH);
  digitalWrite(leftMotorReverse, LOW);
  
  // set PWM value and send it to the motors
  pwmValue = 005;
  analogWrite(rightMotorPWM, pwmValue);         
  analogWrite(leftMotorPWM, pwmValue);  

  // set up timer for pwm
  TCCR2B |=0x07; //set prescaler to divide clock by 1024 for timer 2 table 17-9 p163
  TCNT2 = 0;     // TCNT2 contains timer 2 counter value 
  
  // set serial port speed for "printing" values back to PC serial monitor
  Serial.begin(19200);
} 

// main loop that runs forever
void loop()  {
  
  
  // This code monitors the output from the right moor encoder and counts the number of times
  //      we go round the main loop before it changes segment colour 
 
  // read the value from the right motor encoder sensor:
  sensorValue = analogRead(sensorPin);
  // auto calibrate mid point value
  if (sensorValue > maxSensorValue) { maxSensorValue = sensorValue; }
  if (sensorValue < minSensorValue) { minSensorValue = sensorValue; }
  midSensorValue = (minSensorValue / 2) + (maxSensorValue / 2);
 
  // if sensor is seing white add 1 to highcount unless just changed from seeing black
  if (sensorValue >= midSensorValue) 
     {   // digitalWrite(ledPin, LOW); show on LED if seeing black or white - commented out
      if (cval == HIGH) {highcount = highcount + 1; newval = LOW; }
      if (cval == LOW) {newval = HIGH; encount = lowcount; lowcount = 0; cval = HIGH; }}
      
  // if sensor is seing black add 1 to lowcount unless just changed from seeing white    
  if (sensorValue < midSensorValue) 
      {  //  digitalWrite(ledPin, HIGH); show on LED if seeing black or white - commented out
      if (cval == LOW) {lowcount = lowcount + 1; newval = LOW; }
      if (cval == HIGH) {newval = HIGH; encount = highcount; highcount = 0; cval = LOW; }}
  
    // if changed from seeing black to white or vice versa use encoder count to set PWM value
  if (newval == HIGH) // set PWM based on encoder count value
      // simple 2 speed control 
      {if (encount > 10) {pwmValue = 120; } ;  // going too slow so speed up
      if (encount <= 10) {pwmValue = 80;}  }  // going too fast so slow down  
      
  // these 2 lines cater for going very slowly and not generating a state change, so set PWM to fast
  if (highcount > 80) {pwmValue = 255;}  // count of 
  if (lowcount > 80) {pwmValue = 255;}
  
  // adjust speed of motors to follow line
  pwmValRight = pwmValue;  // set right motor speed from base speed
  pwmValLeft = pwmValue;  // set left motor speed from base speed
  
  //read line sensors
  sensorLeft = analogRead(sensorA5Pin);
  sensorMidLeft = analogRead(sensorA4Pin);
  sensorMidRight = analogRead(sensorA3Pin); 
  sensorRight = analogRead(sensorA2Pin);
  
  // code to light LEd if mid left sensor is seeing the line
  if (sensorMidLeft > 500) {
     digitalWrite(ledPin, HIGH);}  // not seen line so extinguish LED
  if (sensorMidLeft <= 500) {
     digitalWrite(ledPin, LOW);}   // seen white line so light LED
     
  // if both middle sensors seeing white go straight ahead   
  if (sensorMidLeft <= 500) {       // if it sees white
     if (sensorMidRight <= 500) {   // if it sees white
       pwmValRight = pwmValue;  // set right motor speed from base speed
       pwmValLeft = pwmValue; }} // set left motor speed from base speed  
 
 // if left middle sensor sees white but right middle sees black, need to swing left   
  if (sensorMidLeft <= 500) {    // if it sees white
     if (sensorMidRight > 500) { // if it sees black
       pwmValRight = pwmValue + 20 ;  // set right motor speed faster than base speed
       pwmValLeft = pwmValue - 20;    // set left motor speed slower than base speed 
       lastseen = 0; }}               // lastseen = 0 means heading right
 
// if right middle sensor sees white but left middle sees black, need to swing right   
  if (sensorMidRight <= 500) {    // if it sees white
     if (sensorMidLeft > 500) {   // if it sees black
       pwmValRight = pwmValue - 20 ;  // set right motor speed slower than base speed
       pwmValLeft = pwmValue + 20;    // set left motor speed faster than base speed
       lastseen = 1; }}               // lastseen = 1 means heading right      
  
// If both sensors see black use value of lastseen to know whether to go right or left  
  if (sensorMidRight > 500) {    // if it sees black
     if (sensorMidLeft > 500) {   // if it sees black
        if (lastseen == 0) { 
        pwmValRight = pwmValue + 40 ;  // set right motor speed faster than base speed
        pwmValLeft = pwmValue - 40; } // set left motor speed slower than base speed
        if (lastseen == 1) { 
        pwmValRight = pwmValue - 40 ;  // set right motor speed slower than base speed
        pwmValLeft = pwmValue + 40; }}} // set left motor speed faster than base speed
 
 // Check that adjustment has not taken value over 255
  if (pwmValRight > 255) { pwmValRight = 255;}
  if (pwmValLeft > 255) { pwmValLeft = 255;}  
       
 // *********** code to go here to test if value gone less than 0      
  
// ************ code for seeing start & stop & corner markers to go here  
       
 
 // if pwmvalue for right motor has changed, send new value to PWM controller
  if (pwmRightPrev != pwmValRight) {
      analogWrite(rightMotorPWM, pwmValRight);         
       pwmRightPrev = pwmValRight;}
 // if pwmvalue for left motor has changed, send new value to PWM controller      
  if (pwmLeftPrev != pwmValLeft) {
      analogWrite(leftMotorPWM, pwmValLeft);         
       pwmLeftPrev = pwmValLeft;} 
  
  
//   sensorPrint();   display values of sensor on serial link as debugging aid
  delay(1);
}

void sensorPrint () {     
  Serial.print(sensorValue, DEC);
  Serial.print(" ");
  Serial.print(midSensorValue, DEC);
  Serial.print(" ");
  Serial.print(minSensorValue, DEC);
  Serial.print(" ");
  Serial.println(maxSensorValue, DEC);
  Serial.print(encount, DEC);
  delay(100);
}