Cyclops 8

/*
Cyclops8: As Cyclops6, moves towards light. Due to laziness, Forward movement run time depends on angle of light, however left and right turns are reduced to simply 200 m/s rather than calculated.
At the start of the loop, it checks whether any of the front edge sensor pins are over the edge, and if so, backs one wheel up to bring it back on.
If both edge sensors are on the desk, then carries out the usual sweep, and movement, however while moving continues to monitor the edge sensors.
If either edge sensor pass over the edge, the motors are stopped for the remainder of the movement time, although the clock keeps going until the timer has expired.
.... Or least thats what i think it does. It certainly appears to work from this angle.
 */

#include <AFMotor.h>       // Adafruit MotorShield
#include <VarSpeedServo.h> // VarSpeedServo library gives more options, but variable speed isnt used yet

const int PR = A1; // connect PR to A1
int Array[125];    // array to store sweep values

VarSpeedServo Hori;
int HoriPos = 90; //  centre is 90. hardware modififed to prevent clashes. 30 (left) to 150 (right)
int CWHoriDir = 0;
int CCWHoriDir = 0;

VarSpeedServo Vert;
int VertPos = 90; // 130 = level, 0 is back on itself, 30 is straight up
int UpVertDir = 0;
int DownVertDir = 0;

AF_DCMotor motorL(3, MOTOR12_1KHZ);
AF_DCMotor motorR(2, MOTOR12_1KHZ);
const int RLED = 13; // indicator LED: 13 and 2 are the only properly unused digi pins on the motorshield.
const int LLED = 2;  // indicator LED

int VertDelay;    // integer used to calculate "distance" to travel. Shallow angle on Vert indicates distance to light is greater,and increases tim
int HoriDelay;    // Steep (vertical) angle indicates closer to light and therefore moves in small steps to help prevent over run

// front sensors and integers for calibration
const int LeftPR = A2;    // pin that the sensor is attached to
const int CentrePR = A3;    // pin that the sensor is attached to
const int RightPR = A4;    // pin that the sensor is attached to
int LeftValue = 0;         // the sensor value
int LeftMin = 1023;        // minimum sensor value
int LeftMax = 0;           // maximum sensor value
int CentreValue = 0;         // the sensor value
int CentreMin = 1023;        // minimum sensor value
int CentreMax = 0;           // maximum sensor value
int RightValue = 0;         // the sensor value
int RightMin = 1023;        // minimum sensor value
int RightMax = 0;           // maximum sensor value

unsigned long previous = 0;
unsigned long interval = 0;

void setup() {

  Serial.begin(9600);  // Serial at 9600 bps
  Serial.println("Hello!");
  Hori.attach(9);           // 9 is "Servo2"
  Vert.attach(10);          // 10 is "Servo1"
  motorL.setSpeed(250);     // 0 is stop, 255 is full speed:
  motorR.setSpeed(250);     //
  pinMode (RLED,OUTPUT);    // indicator LEDS. HIGH is on, LOW is off
  pinMode (LLED,OUTPUT);
  randomSeed(analogRead(0));

  Serial.println("Calibrate...");
  {
    while (millis() < random(3000, 7000)) { // For between 3 and 7 seconds, Calibrate (random time allows for random starting facing direction)

      //Spin on the spot
      motorL.run(FORWARD);
      motorR.run(BACKWARD);
      digitalWrite(LLED, LOW);
      digitalWrite(RLED, HIGH);

      //Calibrate
      LeftValue = analogRead(LeftPR);
      // record the maximum sensor value
      if (LeftValue > LeftMax) {
        LeftMax = LeftValue;
      }
      // record the minimum sensor value
      if (LeftValue < LeftMin) {
        LeftMin = LeftValue;
      }
      CentreValue = analogRead(CentrePR);
      // record the maximum sensor value
      if (CentreValue > CentreMax) {
        CentreMax = CentreValue;
      }
      // record the minimum sensor value
      if (CentreValue < CentreMin) {
        CentreMin = CentreValue;
      }
      RightValue = analogRead(RightPR);
      // record the maximum sensor value
      if (RightValue > RightMax) {
        RightMax = RightValue;
      }
      // record the minimum sensor value
      if (RightValue < RightMin) {
        RightMin = RightValue;
      }
    }
    motorL.run(RELEASE);  // End of Calibration
    motorR.run(RELEASE);
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);

    BlindSpot();         //BlindSpot Check
  }
}

void loop() {

  HoriPos = constrain (HoriPos, 30, 150); // constrain servos, Just in case.
  VertPos = constrain (VertPos, 30, 130);

  //LEFT SENSOR
  LeftValue = analogRead(LeftPR);
  LeftValue = map(LeftValue, LeftMin, LeftMax, 0, 100);
  LeftValue = constrain(LeftValue, 0, 200);
  // RIGHT SENSOR
  RightValue = analogRead(RightPR);
  RightValue = map(RightValue, RightMin, RightMax, 0, 100);
  RightValue = constrain(RightValue, 0, 200);

  unsigned long current = millis(); // timer for reversing at edge.
  previous = current; // timer for reversing at edge.
  interval = 500; // fixed half second of single wheel backing up.


  if (LeftValue > 100){ // if the Left edge sensor is over an edge, back up Right wheel
    while (millis() - previous < interval){
      motorL.run(RELEASE);
      motorR.run(BACKWARD);
      digitalWrite(LLED, HIGH);
      digitalWrite(RLED, LOW);
    }
    motorL.run(RELEASE);
    motorR.run(RELEASE);
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);
  }

  if (RightValue > 100){ // if the Right edge sensor is over an edge, back up Left wheel
    while (millis() - previous < interval){
      motorL.run(BACKWARD);
      motorR.run(RELEASE);
      digitalWrite(LLED, LOW);
      digitalWrite(RLED, HIGH);
    }
    motorL.run(RELEASE);
    motorR.run(RELEASE);
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);
  }

  else { // Assuming both edge sensors are on the table:

    HoriSweep();        //Horizontal Scan
    VertSweep();        // Vertical Scan
    VertDelay = map (VertPos, 30, 130, 500, 10000);

    if((HoriPos >= 70) && (HoriPos <= 110)){
      interval = VertDelay;
    }
    else{
      interval = 200;
    }
   unsigned long current = millis(); // timer for motors running
   previous = current;

    if(VertPos < 40){     //Stop if under light
      motorL.run(RELEASE);
      motorR.run(RELEASE);
      digitalWrite(LLED, LOW);
      digitalWrite(RLED, LOW);
      delay (99999999); // lazy mans ending
    }

    else {
      Serial.print("Interval: ");
      Serial. println (interval);

      while (millis() - previous < interval){   // while the movement timer allows:

        //LEFT SENSOR // monitor the Left Sensor
        LeftValue = analogRead(LeftPR);
        LeftValue = map(LeftValue, LeftMin, LeftMax, 0, 100);
        LeftValue = constrain(LeftValue, 0, 200);
        // RIGHT SENSOR // monitor the Right Sensor
        RightValue = analogRead(RightPR);
        RightValue = map(RightValue, RightMin, RightMax, 0, 100);
        RightValue = constrain(RightValue, 0, 200);

        // RIGHT // Spin right if servo sweep has detected light to the right
        if((HoriPos > 110) && (HoriPos <= 150)){
          motorL.run(FORWARD);
          motorR.run(BACKWARD);
          digitalWrite(LLED, LOW);
          digitalWrite(RLED, HIGH);
        }
        //LEFT // Spin left if servo sweep has detected light to the left
        if((HoriPos < 70) && (HoriPos >= 30)){
          motorL.run(BACKWARD);
          motorR.run(FORWARD);
          digitalWrite(LLED, HIGH);
          digitalWrite(RLED, LOW);
        }

        // FORWARD run forward: Window of 20 degrees in the centre results in running forward
          if((HoriPos >= 70) && (HoriPos <= 110)&& (RightValue < 100) && (LeftValue < 100)){ // Both edge sensors must be over desk
          motorL.run(FORWARD);
          motorR.run(FORWARD);
          digitalWrite(LLED, HIGH);
          digitalWrite(RLED, HIGH);
        }
        if((HoriPos >= 70) && (HoriPos <= 110) && ((RightValue > 100) || (LeftValue > 100))){ // One of edge sensore not over desk, stop for the remainder of the movement timers allowance
          motorL.run(RELEASE);
          motorR.run(RELEASE);
          digitalWrite(LLED, LOW);
          digitalWrite(RLED, LOW);
        }
      }
      {
        motorL.run(RELEASE);  // This loop has ended, stop the motors and go back to the begining.
        motorR.run(RELEASE);
        digitalWrite(LLED, LOW);
        digitalWrite(RLED, LOW);
      }
    }
  }
}