/*
  New control of Explorer based robot

  Author Tony Wills
  Licensed CC-BY-SA

  20220411 version 3.00

  see http://www.projects.scorchingbay.nz/dokuwiki//robotic/tracked (or an archive.org copy) for more information
*/

#define Lmotordirpin        4 // HIGH=forward LOW=reverse
#define Lmotorpwmpin       10 // PWM speed control
#define Lmotorcurpin       A0 // Current drawn by motor

#define Rmotordirpin        2 // HIGH=forward LOW=reverse
#define Rmotorpwmpin        9 // PWM speed control
#define Rmotorcurpin       A5 // Current drawn by motor

#define LeftfrontIRledpin  11 // IR 2 - HIGH turns on front left IR LED
#define Leftsidesenpin     A2 // IR 2 - High values when object in close proximity

#define RightfrontIRledpin  8 // IR 1 - HIGH turns on front right IR LED
#define Rightsidesenpin    A1 // IR 1 - High values when object in close proximity

#define LeftrearIRledpin   12 // IR 3 - HIGH turns on rear left IR LED
#define Leftrearsenpin     A3 // IR 3 - High values when object in close proximity

#define RightrearIRledpin   7 // IR 4 - HIGH turns on rear right IR LED
#define Rightrearsenpin    A4 // IR 4 - High values when object in close proximity

#define Ledpin             13 // arduino LED

#define Speedcontrol      200 // Motor creep speed - Global value for left/right motor
#define IRdelay           100 // Time in uS needed by IRphototransistor to give good range reading

int lastAction;                       // last manoeuvre performed
const byte actionForward =  0;        //    traveling forward
const byte actionLeft =     1;        //    avoided object on left by turning right
const byte actionRight =    2;        //    avoided object on right by turning left
const byte actionSpin =     3;        //    spun (one wheel forward, other back) left or right
const byte actionBack =     4;        //    backed up and chose another direction
const byte actionWheelly =  5;        //    stop then full speed ahead
const byte actionChange =   6;        //    random change after long forward
unsigned long actionStart;            // time last manoeuvre began
unsigned long actionEnd;              // time last manoeuvre finished
unsigned long runStart;               // time the last actionFoward manoeuvre began
const byte LEFT =  0;                 // left side
const byte RIGHT = 1;                 // right side

int frontLeftSide;                    // value of front left IR sensor           analog input
int frontRightSide;                   // value of front right IR sensor          analog input
int rearLeftSide;                     // value of rear left IR sensor            analog input
int rearRightSide;                    // value of rear right IR sensor           analog input

int leftSpeed;                        // speed of left motor                    -255 to 255
int rightSpeed;                       // speed of right motor                   -255 to 255
int leftScale[2];                     // used to correct left  motor speed in forward and reverse
int rightScale[2];                    // used to correct right motor speed in forward and reverse

const int followDistance =   100;     // distance at which to follow a wall
const int avoidDistance =    800;     // distance at which to take evasive action
const int reverseDistance =  700;     // distance at which to back out and try a different path

byte runSpeed;                        // speed for actionForward travel

/*****************************************************************************************/

void setup() {
  Serial.begin(57600);

  pinMode(Lmotordirpin, OUTPUT);                             // left motor direction pin, low=reverse : high=forward
  pinMode(Lmotorpwmpin, OUTPUT);
  pinMode(Rmotordirpin, OUTPUT);                             // right motor direction pin, low=reverse : high=forward
  pinMode(Rmotorpwmpin, OUTPUT);
  pinMode(LeftfrontIRledpin, OUTPUT);                        // left front object sensor LED pin
  pinMode(RightfrontIRledpin, OUTPUT);                       // right front object sensor LED pin
  pinMode(LeftrearIRledpin, OUTPUT);                         // left rear object sensor LED pin
  pinMode(RightrearIRledpin, OUTPUT);                        // right rear object sensor LED pin
  pinMode(Ledpin, OUTPUT);
  
  leftScale[0] = 100;
  leftScale[1] = 100;
  rightScale[0] = 100;
  rightScale[1] = 100;

  lastAction = 255;
  actionStart = millis();
  actionEnd = millis();
  runStart = millis();

  frontLeftSide = 1023;
  frontRightSide = 1023;

  runSpeed = 255;

  flashAntenna(100, 10);
  leftSpeed = Speedcontrol;
  rightSpeed = Speedcontrol;
  motorUpdate();
}

/*****************************************************************************************/

void loop() {

  sensorCheck();

  if ((millis() - actionEnd) > 7000) {
    // seem to have been running forward for a long time without manoeuvre, try something else
    // all stop
    driveForward(0);
    digitalWrite(LeftfrontIRledpin, HIGH);
    digitalWrite(RightfrontIRledpin, HIGH);
    digitalWrite(LeftrearIRledpin, HIGH);
    digitalWrite(RightrearIRledpin, HIGH);
    myDelay(2000);
    flashAntenna(100, 5);
    driveBack(1000);
    driveSpin( LEFT, random(100, 2500));
    myDelay(500);
    lastAction = actionChange;
    runSpeed = 255;
    actionEnd = millis();
  }

  if (((frontLeftSide < reverseDistance) && (frontRightSide < reverseDistance)) ) {
    // front on to wall so stop and turn around
    actionStart = millis();
    digitalWrite(LeftfrontIRledpin, HIGH);
    digitalWrite(RightfrontIRledpin, HIGH);
    lastAction = actionSpin;
    // stop and pause
    driveForward( 0 );
    myDelay(1000);
    //digitalWrite(Ledpin, LOW);
    // backup
    driveBack( 1000 );
    // rotate rotate 180 degrees
    driveSpin(LEFT, 4500);
    actionEnd = millis();
  } else {
    if (frontLeftSide < avoidDistance) {
      // close to something on left so turn right
      actionStart = millis();
      digitalWrite(LeftfrontIRledpin, HIGH);
      if ((lastAction == actionRight) && ((millis() - actionEnd) < 1000) ) {
        // last recent action was a turn the other way, so keep turning that way rather than
        // getting into a turn left, turn right, turn left endless loop
        driveBack(1000);
        //driveRight(2500);
        driveLeft(2500);
      } else {
        driveBack(200);
        if ((lastAction == actionLeft) && ((millis() - actionEnd) < 1000) ) {
          // just turned this way and still in contact so pull back a little further
          driveBack(200);
        }
        driveRight(random(500, 1500));
        lastAction = actionLeft;
      }

      actionEnd = millis();
    } else if (frontRightSide < avoidDistance) {
      actionStart = millis();
      digitalWrite(RightfrontIRledpin, HIGH);
      if ((lastAction == actionLeft) && ((millis() - actionEnd) < 1000) ) {
        // last recent action was a turn the other way, so keep turning that way rather than
        // getting into a turn left, turn right, turn left endless loop
        driveBack(1000);
        //driveLeft(2500);
        driveRight(2500);
      } else {
        if ((lastAction == actionRight) && ((millis() - actionEnd) < 1000) ) {
          // just turned this way and still in contact so pull back a little further
          driveBack(200);
        }
        driveBack(200);
        driveLeft(random(500, 1500));
        lastAction = actionRight;
      }
      actionEnd = millis();
    }
  }
  if (((frontLeftSide > avoidDistance) && (frontRightSide > avoidDistance)) ) {
    actionStart = millis();
    digitalWrite(LeftfrontIRledpin, LOW);
    digitalWrite(RightfrontIRledpin, LOW);
    //lastAction = actionForward;
    runSpeed = 255;
    driveForward( runSpeed );
    runStart = millis();
  }

  driveForward( runSpeed );   // forward both
  myDelay(100);
  //lastAction = actionForward;
}
/*****************************************************************************************/

void sensorCheck() {
  // check for obstructions in range
  digitalWrite(LeftfrontIRledpin, HIGH);
  digitalWrite(RightfrontIRledpin, HIGH);
  digitalWrite(LeftrearIRledpin, HIGH);
  digitalWrite(RightrearIRledpin, HIGH);
  delayMicroseconds(IRdelay);

  // read sensors
  frontLeftSide = analogRead(Leftsidesenpin);
  frontRightSide = analogRead(Rightsidesenpin);
  rearLeftSide = analogRead(Leftrearsenpin);
  rearRightSide = analogRead(Rightrearsenpin);

  // turn off object detection LEDs
  digitalWrite(LeftfrontIRledpin, LOW);
  digitalWrite(RightfrontIRledpin, LOW);
  digitalWrite(LeftrearIRledpin, LOW);
  digitalWrite(RightrearIRledpin, LOW);
  delayMicroseconds(IRdelay);

  // subtract ambient IR from total IR to give reflected IR values
  // subtract from max intensity to turn the value into a distance instead (inverse of intensity)
  frontLeftSide = 1023 - (frontLeftSide - analogRead(Leftsidesenpin));
  frontRightSide = 1023 - (frontRightSide - analogRead(Rightsidesenpin));
  rearLeftSide = 1023 - (rearLeftSide - analogRead(Leftrearsenpin));
  rearRightSide = 1023 - (rearRightSide - analogRead(Rightrearsenpin));
  Serial.print(frontRightSide / 8); Serial.print("\t");
  Serial.print(frontLeftSide / 8); Serial.print("\t");
  Serial.print(rearLeftSide / 8); Serial.print("\t");
  Serial.println(rearRightSide / 8);
}

void myDelay(unsigned long period ) {
  delay(period);
}

void driveBack(unsigned long howLong) {
  leftSpeed = -Speedcontrol * 0.5;
  rightSpeed = -Speedcontrol * 0.5;
  motorUpdate();
  myDelay( howLong );
}

void driveRight(unsigned long howLong) {
  leftSpeed = Speedcontrol * 0.5;
  //rightSpeed = 0;
  rightSpeed = -Speedcontrol * 0.5;
  motorUpdate();
  myDelay(howLong);
}

void driveLeft(unsigned long howLong) {
  //leftSpeed = 0;
  leftSpeed = -Speedcontrol * 0.5;
  rightSpeed = Speedcontrol * 0.5;
  motorUpdate();
  myDelay(howLong);
}

void driveForward(int howFast) {
  leftSpeed = howFast;
  rightSpeed = howFast;
  motorUpdate();
}

void driveSpin(byte direction, unsigned long howLong) {
  if (direction == RIGHT) {
    //spin right
    leftSpeed = Speedcontrol * 0.5;
    rightSpeed = -Speedcontrol * 0.5;
    motorUpdate();
    myDelay(howLong);
  } else {
    // spin other way
    leftSpeed = -Speedcontrol * 0.5;
    rightSpeed = Speedcontrol * 0.5;
    motorUpdate();
    myDelay(howLong);
  }
}

void flashAntenna(int period, int cycles) {
  for (int i = 0; i < cycles; i++) {
    digitalWrite(Ledpin, LOW);
    delay(period);
    digitalWrite(Ledpin, HIGH);
    delay(2*period);
  }
}

//================ Motor speed direction management and update =============
void motorUpdate()
{
  leftSpeed = leftSpeed * leftScale[(leftSpeed > 0)] / 100;  // calibrate left motor speed
  if (leftSpeed > 255) leftSpeed = 255;
  if (leftSpeed < -255) leftSpeed = -255;

  digitalWrite(Lmotordirpin, (leftSpeed < 0));               // set Left Motor Direction to forward if speed>0
  analogWrite(Lmotorpwmpin, abs(leftSpeed));                 // set Left Motor Speed

  rightSpeed = rightSpeed*rightScale[(rightSpeed > 0)]/100;  // calibrate right motor speed
  if (rightSpeed > 255) rightSpeed = 255;
  if (rightSpeed < -255) rightSpeed = -255;

  digitalWrite(Rmotordirpin, (rightSpeed < 0));              // set Right Motor Direction to forward if speed>0
  analogWrite(Rmotorpwmpin, abs(rightSpeed));                // set Right Motor Speed

  //Serial.print("L Speed: ");
  //Serial.println(leftSpeed);
  //Serial.print("R Speed: ");
  //Serial.println(rightSpeed);
}