NV-DRD (Nearly) v2.0

/*
 *  NV:DRD v1.2 by Jason Adams <[email protected]>
 *
 *  DONE:
 *  + the bot now, every so often, spins around like a puppy for a few seconds and goes to sleep
 *  + created a function that alternately fades and flashes the LEDs
 *  + put blinking and backing-up in their own functions and added the ability to pass
 *    parameters to them to change the number of blinks and the duration of backing-up each time
 *  + created functions for turning left and right, with the ability to pass params (as ints)
 *    to determine how long to turn
 *  + implemented a way to detect that the bot is no longer moving forward and is therefore
 *    likely stuck on something outside its field of view
 *  + the bot takes a forward reading after turning but before moving again, to make sure there
 *    is a fair amount of distance to travel. It then continues to look around and turn
 *    until it detects enough room to move
 *  + found the bug that causes the bot to see a nonexistent obsticle as soon as loop() starts
 *
 *  TODO:
 *  - figure out the correct duration to turn in order to turn 90 degrees
 *  - add a default value to each of the functions that take perameters
 *  - find the max distance the sensor can return, convert it to cm, and add a test for that
 *    reading to the logic that determines when the bot is stuck, so it doesn't interpret a large,
 *    open room in which it can see to it's farthest ability for a length of time as being stuck
 *
 *  NEEDS TESTING:
 *
 */

char version[] = "NV:DRD v1.2";

#include <AFMotor.h>
#include <Servo.h>

// create the motor objects
AF_DCMotor motor1(1, MOTOR12_64KHZ); // create motor #1, 64KHz pwm
AF_DCMotor motor2(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
Servo headTurner;                    // create servo object for turning the Ping))) sensor

// the calibrated servo motor positions for forward, left, and right.
#define forward  99
#define left     180
#define right    20

// pin assignments
#define servoPin          9     // servo motor on pin 9
#define pingSensorPin     19    // Ping))) sensor on A5 (analog pins 0-5 can be used as digital pins 14-19)
#define leftAntennaeLED   6
#define rightAntennaeLED  5

// constantly changing variables
long pingSensorReading, forwardReading, leftReading, rightReading;
int directionToGo      = 0;     // 0 = forward, 1 = left, 2 = right
int defaultBrightness  = 50;    // default LED brightness using analogWrite(). the scale is 0-255
int minDistance        = 1500;  // the minimum distance to an object before the bot refuses to go forward anymore
int sleepTimer         = 0;     // a counter to increment until the next time it's time to stop and "sleep"
int logTimer           = 0;     // a counter to regulate the logging of forward Ping))) sensor readings
int counterToConfusion = 0;     // a counter to trigger he "confused" LED animation after multiple failed
                                // attempts to turn to a direction that offers enough room to move forward
boolean switchTurn     = false; // false = right, true = left - this bool is for alternating the direction
                                // the bot decides to turn when the right and left readings are the same.
                                // One time it'll go right, the next time left, etc.
boolean firstTimeRun   = true;  // this gets flipped to false only once, after the Ping))) sensor has
                                // recorded distances (converted to cm) in all four of the ints below
int forwardReading_CM = 0;
int forwardReading_2 = 0;
int forwardReading_3 = 0;
int forwardReading_4 = 0;


void setup() {
  // initiate the LEDs
  pinMode(leftAntennaeLED, OUTPUT);
  pinMode(rightAntennaeLED, OUTPUT);

  // set up Serial library at 9600 bps
  Serial.begin(9600);
  Serial.println(version);

  // set the speeds to 200/255
  motor1.setSpeed(255);
  motor2.setSpeed(255);

  // attach the servo object to the pin the servo motor is on
  headTurner.attach(servoPin);
  delay(100);

  // turn Ping))) sensor forward, just to orient it for show immediately upon starting
  headTurner.write(forward);

  // turn antennae lights on
  analogWrite(leftAntennaeLED, defaultBrightness);
  analogWrite(rightAntennaeLED, defaultBrightness);

  // choose which direction to go
  chooseDirection();

  // travel forward
  goForward();

  delay(1000);
}


void loop() {
  // see if we're nearing an object while moving forward
  forwardReading = readPingSensor();
  Serial.print("forward reading: ");
  Serial.println(forwardReading);

  // increment the timer that helps decide when to next stop and go to sleep
  sleepTimer++;

  // increment the timer that triggers storing of forwardReading values, to help determine we might be stuck
  logTimer++;
  if (logTimer == 5) {
    forwardReading_4 = forwardReading_3;
    forwardReading_3 = forwardReading_2;
    forwardReading_2 = forwardReading_CM;
    forwardReading_CM = (forwardReading * 0.03434);
    logTimer = 0;
    firstTimeRun = false;
  }

  // see if we're no longer making forward progress, indicating we may be stuck
  if (!firstTimeRun && forwardReading_CM == forwardReading_2 && forwardReading_2 == forwardReading_3 && forwardReading_3 == forwardReading_4) {
    allStop(); // stop moving
    blinkLEDs(2); // blink
    goReverse(2500); // back up

    // choose the direction with the farthest distance to move
    chooseDirection();

    // travel forward
    goForward();

    // reset the logging variables so a full examination is required again to determine if we're stuck
    forwardReading_4 = 0;
    forwardReading_3 = 0;
    forwardReading_2 = 0;

  // if we are nearing an object, stop moving...
  } else if (forwardReading <= minDistance) {
    allStop(); // stop moving
    blinkLEDs(2); // blink
    goReverse(400); // back up

    chooseDirection();

    goForward(); // travel forward

  } else if (sleepTimer >= 900) {
    // go to sleep every five minutes (900 seconds)
    /* (I put this inside the 'else' in order to avoid the bot sleeping immediately after detecting that it's stuck or
       too close to an object. It will catch the next time around that neither of the other two conditions are met) */
    stopAndSleep(); // stop and sleep
    sleepTimer = 0; // reset the sleep timer variable

    chooseDirection();

    goForward(); // travel forward
  }

  // wait 3/10 of a second so the Ping))) sensor gets polled 3x/sec when driving forward
  delay(333);
}


/*
  FUNCTIONS:
    chooseDirection()              // Tested
    stopAndSleep()                 // Tested
    readPingSensor()               // Tested
    alternatingLEDs(int)           // Tested
    snoreLEDs(int)                 // Tested
    blinkLEDs(int)                 // Tested
    goForward()                    // Tested
    goReverse(int)                 // Tested
    turnLeft(int)                  // Tested
    turnRight(int)                 // Tested
    allStop()                      // Tested
*/


void chooseDirection() {
  // turn Ping))) sensor to look forward and then take a reading
  headTurner.write(forward);
  delay(500);
  forwardReading = readPingSensor();
  Serial.print("forward reading: ");
  Serial.println(forwardReading);
  delay(250);

  // turn Ping))) sensor to look left and then take a reading
  headTurner.write(left);
  delay(500);
  leftReading = readPingSensor();
  Serial.print("left reading: ");
  Serial.println(leftReading);
  delay(250);

  // turn Ping))) sensor to look right and then take a reading
  headTurner.write(right);
  delay(650);
  rightReading = readPingSensor();
  Serial.print("right reading: ");
  Serial.println(rightReading);
  delay(250);

  if (forwardReading <= minDistance && leftReading <= minDistance && rightReading <= minDistance) {

    if (leftReading > rightReading) {
      turnLeft(450);
    } else if (leftReading < rightReading) {
      turnRight(450);
    } else if (leftReading == rightReading) {
      if (switchTurn == false) {
        Serial.println("  right and left readings are the same; going right this time");
        turnRight(450);
      } else {
        Serial.println("  right and left readings are the same; going left this time");
        turnLeft(450);
      }
      switchTurn = !switchTurn;
    }

    while (forwardReading <= minDistance && leftReading <= minDistance && rightReading <= minDistance ){

      counterToConfusion++;
      if (counterToConfusion == 3) {
        headTurner.write(forward);
        delay(500);
        alternatingLEDs(5);
        counterToConfusion = 0;
      }

      // turn Ping))) sensor to look forward and then take a reading
      headTurner.write(forward);
      delay(500);
      forwardReading = readPingSensor();
      Serial.print("*forward reading: ");
      Serial.println(forwardReading);
      delay(250);

      // turn Ping))) sensor to look left and then take a reading
      headTurner.write(left);
      delay(500);
      leftReading = readPingSensor();
      Serial.print("*left reading: ");
      Serial.println(leftReading);
      delay(250);

      // turn Ping))) sensor to look right and then take a reading
      headTurner.write(right);
      delay(650);
      rightReading = readPingSensor();
      Serial.print("*right reading: ");
      Serial.println(rightReading);
      delay(250);

      // compare the 'forward' and 'left' readings to see which direction offers more distance to travel
      if (forwardReading >= leftReading) {
        directionToGo = 0;
        Serial.println("* determining i'd rather go forward than left");
      } else {
        directionToGo = 1;
        Serial.println("* determining i'd rather go left than forward");
      }

      // compare the 'right' reading to the winner of the previous comparison to see which direction we should go
      if (directionToGo == 0 && rightReading > forwardReading) {
        Serial.println("* determining i should go right instead of forward");
        turnRight(450);
        if (rightReading > minDistance) { break; }
      } else if (directionToGo == 1 && rightReading > leftReading) {
        Serial.println("* determining i should go right instead of left");
        turnRight(450);
        if (rightReading > minDistance) { break; }
      } else if (directionToGo == 1 && rightReading < leftReading) {
        Serial.println("* determining i should go left instead of right");
        turnLeft(450);
        if (leftReading > minDistance) { break; }
      } else if (directionToGo == 1 && rightReading == leftReading) {
        if (switchTurn == false) {
          Serial.println("* right and left readings are the same; going right this time");
          turnRight(450);
        } else {
          Serial.println("* right and left readings are the same; going left this time");
          turnLeft(450);
        }
        switchTurn = !switchTurn;
        if (leftReading > minDistance) { break; }
      } else if (directionToGo == 0 && rightReading <= forwardReading && leftReading <= forwardReading) {
        if (forwardReading > minDistance) {
          break;
        } else {
          if (switchTurn == false) {
            Serial.println("* forward reading was the largest, but i have to turn; going right this time");
            turnRight(450);
          } else {
            Serial.println("* forward reading was the largest, but i have to turn; going left this time");
            turnLeft(450);
          }
          switchTurn = !switchTurn;
        }
      }
    }
  } else {
    // compare the 'forward' and 'left' readings to see which direction offers more distance to travel
    if (forwardReading >= leftReading) {
      directionToGo = 0;
      Serial.println("  determining i'd rather go forward than left");
    } else {
      directionToGo = 1;
      Serial.println("  determining i'd rather go left than forward");
    }

    // compare the 'right' reading to the winner of the previous comparison to see which direction we should go
    if (directionToGo == 0 && rightReading > forwardReading) {
      Serial.println("  determining i should go right instead of forward");
      turnRight(450);
    } else if (directionToGo == 1 && rightReading > leftReading) {
      Serial.println("  determining i should go right instead of left");
      turnRight(450);
    } else if (directionToGo == 1 && rightReading == leftReading) {
      if (switchTurn == false) {
        Serial.println("  right and left readings are the same; going right this time");
        turnRight(450);
      } else {
        Serial.println("  right and left readings are the same; going left this time");
        turnLeft(450);
      }
      switchTurn = !switchTurn;
    }
  }

  // look forward again in preparation for moving forward again
  headTurner.write(forward);
}


void stopAndSleep() {
  // announce over serial that it's time to sleep
  Serial.println("getting sleepy. think i'll take a nap right here");

  // stop moving forward
  allStop();
  delay(750);

  // fade the antennae LEDs to indicate sleepiness
  for(int fadeValue = defaultBrightness; fadeValue >= 0; fadeValue -= 1) {
    analogWrite(leftAntennaeLED, fadeValue);
    analogWrite(rightAntennaeLED, fadeValue);
    delay(75);
  }
  delay(1000);

  // choose a direction, left or right, and spin in place like a puppy about to lay down
  if (switchTurn == false){
    turnRight(5000);
  } else {
    turnLeft(5000);
  }

  // set this bool so the next time the bot will turn the other direction
  switchTurn = !switchTurn;

  // orient the head forward
  headTurner.write(forward);
  delay(1000);

  snoreLEDs(10);

  // fade the LEDs back on quickly, like the bot is waking up
  for(int fadeValue = 0 ; fadeValue <= defaultBrightness; fadeValue +=2) {
    analogWrite(leftAntennaeLED, fadeValue);
    analogWrite(rightAntennaeLED, fadeValue);
    delay(20);
  }
  delay(1000);

  blinkLEDs(2);

  delay(500);
}


long readPingSensor() {
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingSensorPin, OUTPUT);
  digitalWrite(pingSensorPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingSensorPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingSensorPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingSensorPin, INPUT);
  pingSensorReading = pulseIn(pingSensorPin, HIGH);

  return pingSensorReading;
}


void alternatingLEDs(int numFlashes) {
  Serial.println("flashing my antennae lights");
  for (int i = 0; i < numFlashes; i++) {
    for(int fadeValue = 5; fadeValue <= 255; fadeValue +=5) {  // left brightening, right dimming
      analogWrite(leftAntennaeLED, fadeValue);
      analogWrite(rightAntennaeLED, 260 - fadeValue);
      delay(10);
    }
    for(int fadeValue = 5; fadeValue <= 255; fadeValue +=5) {  // left dimming, right brightening
      analogWrite(leftAntennaeLED, 260 - fadeValue);
      analogWrite(rightAntennaeLED, fadeValue);
      delay(10);
    }
  }
  digitalWrite(leftAntennaeLED, LOW);
  digitalWrite(rightAntennaeLED, LOW);
  delay(200);
  analogWrite(leftAntennaeLED, defaultBrightness);
  analogWrite(rightAntennaeLED, defaultBrightness);
}


void snoreLEDs(int numSnores) {  // "sleep-breathe" the LEDs like a mac
  for (int i = 0; i < numSnores; i++) {
    for(int fadeValue = 0 ; fadeValue <= 15; fadeValue +=1) {  // on
      analogWrite(leftAntennaeLED, fadeValue);
      analogWrite(rightAntennaeLED, fadeValue);
      delay(75);
    }
    for(int fadeValue = 15 ; fadeValue >= 0; fadeValue -=1) {  // off
      analogWrite(leftAntennaeLED, fadeValue);
      analogWrite(rightAntennaeLED, fadeValue);
      delay(75);
    }
    delay(1500);
  }
}


void blinkLEDs(int numBlinks) {
  Serial.println("blinking");
  for (int i = 0; i < numBlinks; i++) {
    analogWrite(leftAntennaeLED, 0);
    analogWrite(rightAntennaeLED, 0);
    delay(100);
    analogWrite(leftAntennaeLED, defaultBrightness);
    analogWrite(rightAntennaeLED, defaultBrightness);
    delay(100);
  }
}


void goForward() {
  Serial.println("driving forward");
  motor1.run(FORWARD);
  motor2.run(FORWARD);
}


void goReverse(int backUpDuration) {
  Serial.println("backing up a little bit");
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  delay(backUpDuration);
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  delay(500);
}


void turnLeft(int turnDuration) {
  headTurner.write(left);   // look left
  delay(300);               // give servo (head) time to turn before beginning turning the bot's body
  Serial.println("turning left");
  // turn left
  motor1.run(BACKWARD);
  motor2.run(FORWARD);
  delay(turnDuration);
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  delay(100);
}


void turnRight(int turnDuration) {
  headTurner.write(right);   // look left
  delay(300);                // give servo (head) time to turn before beginning turning the bot's body
  Serial.println("turning right");
  // turn right
  motor1.run(FORWARD);
  motor2.run(BACKWARD);
  delay(turnDuration);
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  delay(100);
}


void allStop() {
  Serial.println("ALL STOP!");
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  delay(200);
}