Autonomous Controllable LEGO Car

#include "Servo.h"
#include "IRremote.h"
#include "Stepper.h"
#define POTPIN 1
#define IRRECIEVER 2
#define NXTMOTORONE 6 // two directions for each motor using motor driver l9110
#define NXTMOTORTWO 10
#define NXTMOTORONEBACK 5
#define NXTMOTORTWOBACK 9
#define WAITTIME 150 // time stopped in each of 4 directions or used for setting motors speeds to zero
#define STEPPERDEGREERADIUS 135
#define TRIGUS 13 // sends out U.S. pulse 
#define BUTTONFORWARDONE A4 // push buttons uses as touch sensors
#define BUTTONFORWARDTWO A5
#define BUTTONBACKWARDONE A2
#define BUTTONBACKWARDTWO A3
byte echoUS = 3; // recieves U.S. pulse and uses time elapsed to determine distance
long numberOfLoops = 0; // used to run specific autonomous functions periodically such as checkNearby() and checkSurroundings()
int cmDistance = 0; // uses ping U.S. function to determine distance
int currentCm = 0;
int turnTime; // 90 degree turn dependent on surface
double valPot; // value from potentiometer from 0 - 1023
int motorSpeedPot; // valPot converted to analog value for motors from 0 - 255
double fractionOfFullSpeed;
boolean pathClear = false; // returns true if object within 65 cm and can cause clockwise rotation, else no changes to straight path
boolean backwardsMotion = false; // control of either driving forward or backward
int controlSpeed = 255; // used in remote control with ir remote and reciever, default: fast, straight, and forward
int controlDir = 90;
int controlDirVolume = 90;
int controlDirVolIncrement = 10;
boolean controlBackwards = false;
boolean autoOff = false; // stores two possibilities of drive mode- autonomous or manual control (from ir remote)
int distanceStepper[360]; // array to store maximum of distance from U.S. in each degree in a circle when rotated using stepper motor
int maxDistanceStepper;
int degreeMaxDistanceStepper;
boolean manualStepper; // ir remote control of stepper motor
boolean stepperClockwise = false;
int turnTimeFarthest; // time for one nxt motor to rotate car to allign with degree which corresponds to farthest valid distance
int stepperRadiusCount = 0; // always turn stepper over input radius while moving forward 
int stepDisplacement = 0; // used to find current displacement and revert stepper to forward facing if needed
boolean stepperTurnDirection = false;
int sameDegreeCount; // used to check for repetitive distance readings and holds the longest set and associated degree in variables
int sameDegreeCountMax;
int sameDegreeValueMax; 
boolean forwardButtonControl = false; // push buttons to trigger overriding backward or forward movement when car crashes
boolean backwardButtonControl = false;
boolean buttonDriverOverride = false; // for autonomous control using only buttons, no U.S., activated using 4 on ir remote
boolean buttonDriveForward = true;
byte buttonCycleCount = 0;
Servo myServo; 
IRrecv irrecv(IRRECIEVER);
decode_results results;
Stepper stepper(32, 12, 8, 11, 7); // specific arrangement of ports to allow for two-directional movement of stepper motor, 32 is number of rotations by internal shaft- 2048 by external shaft

void setup(void) {
  Serial.begin(57600);
  Serial.setTimeout(1);
  irrecv.enableIRIn(); // Start the receiver  
  myServo.attach(4);
  myServo.write(90); // starts servo at 90 degrees
  pinMode(NXTMOTORONE, OUTPUT); // motors connected to output pins which support analog, forward drive motors
  pinMode(NXTMOTORTWO, OUTPUT);
  pinMode(NXTMOTORONEBACK, OUTPUT); // backward drive motors
  pinMode(NXTMOTORTWOBACK, OUTPUT);
  pinMode(TRIGUS, OUTPUT); // send sound pulse every 5 ms
  pinMode(echoUS, INPUT);  // detects time for sound pulse to return
  pinMode(BUTTONFORWARDONE, INPUT);
  pinMode(BUTTONFORWARDTWO, INPUT);
  pinMode(BUTTONBACKWARDONE, INPUT);
  pinMode(BUTTONBACKWARDTWO, INPUT);
  digitalWrite(NXTMOTORONEBACK, LOW); // since input pullup has resistor at voltage, starts high and needs to be written low
  digitalWrite(NXTMOTORTWOBACK, LOW);
}

void forwardObstacleButton(void){ // go backwards, then rotate 180 degrees
  halt(WAITTIME);
  backwardsMotion = true;
  moveDirection(90, motorSpeedPot, 2 * turnTime, backwardsMotion);
  moveDirection(0, motorSpeedPot, 5 * turnTime / 2, backwardsMotion);
  backwardsMotion = false; 
}

void backwardObstacleButton(void){ // go forward, then turn around
  halt(WAITTIME);
  backwardsMotion = false;
  moveDirection(90, motorSpeedPot, 2 * turnTime, backwardsMotion);
  moveDirection(0, motorSpeedPot, 2 * turnTime, backwardsMotion);
}

void stepDegree(double degree) { // can turn stepper to any degree in circle in both directions
  stepper.step((int)(1800.0 * degree / 360.0)); // 1800 provides more accurate turning than the stated 2048 
}

void stepperCircleDegreeCheck(void) { // take U.S. measurements every degree over the course of a whole circle
  stepDegree( -1 * stepDisplacement); // ensure starting ultrasonic measurements from forward facing
  stepDisplacement = 0;  
  for (int i = 0; i <= 360; ++i) {
    cmDistance = ping(echoUS);
    distanceStepper[i] = cmDistance; // stores each degree in 360 length array
   // Serial.print("Dis at "); Serial.print(i); Serial.print(" deg: "); Serial.print(distanceStepper[i]); Serial.print(" cm"); Serial.print("\n");
    stepDegree(1.0);
    delay(17); // takes maximum of 10 ms for ultrasonic reading
  }
  delay(100);
  sameDegreeCount = 0;  
  sameDegreeCountMax = 0; 
  maxDistanceStepper = distanceStepper[0];    
  degreeMaxDistanceStepper = 0;
  for (int i = 1; i <= 360; ++i) { // if greater, replace max
    if (distanceStepper[i] > maxDistanceStepper) {
      maxDistanceStepper = distanceStepper[i];
      degreeMaxDistanceStepper = i;
      sameDegreeCount = 0;
    }
    else if (distanceStepper[i] == maxDistanceStepper) { // if equal, count how many times degree equality is held (how many degrees is farthest distance constant) 
      ++sameDegreeCount;
      if (sameDegreeCount > sameDegreeCountMax)
      {
        sameDegreeCountMax = sameDegreeCount;
        sameDegreeValueMax = i;
      }
    }
    else if (distanceStepper[i] < maxDistanceStepper){
      sameDegreeCount = 0;
    }
  }
  if (distanceStepper[sameDegreeValueMax] == maxDistanceStepper){ // check if max is equal to degree where most repetitive degree (typically 200 since too far to create actual measurement due to timeout)
    degreeMaxDistanceStepper = sameDegreeValueMax - sameDegreeCountMax / 2; // find median degree in which farthest data point is recorded and use it for motor rotation to position car to that degree
  }
 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount); Serial.print("\n"); Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax); Serial.print("\n"); Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax); Serial.print("\n");
  Serial.print("Max dis: "); Serial.print(maxDistanceStepper); Serial.print(" cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\n");
  stepDegree(-360.0); // return to original position
  // stepDegree(degreeMaxDistanceStepper); // returns to degree where farthest U.S. reading was taken
}

void stepperDegreeCheck(int lowerBound, int higherBound){ // U.S. uses stepper to record each degree within bounds
  stepDegree( -1 * stepDisplacement); 
  stepDisplacement = 0;  
  stepDegree(lowerBound); // position stepper to start at lower bound
  for (int i = lowerBound; i <= higherBound; ++i) { // goes to lower bound, then takes one degree incremental measurements until higher bound
    cmDistance = ping(echoUS);
    distanceStepper[i] = cmDistance;
   // Serial.print("Dis at: "); Serial.print(i); Serial.print(" deg: "); Serial.print(distanceStepper[i]); Serial.print(" cm"); Serial.print("\n");
    stepDegree(1.0);
    delay(17);
  }
  delay(100);
  sameDegreeCount = 0;  
  sameDegreeCountMax = 0; 
  maxDistanceStepper = distanceStepper[0];    
  degreeMaxDistanceStepper = 0;
  for (int i = 1; i <= 360; ++i) { 
    if (distanceStepper[i] > maxDistanceStepper) {
      maxDistanceStepper = distanceStepper[i];
      degreeMaxDistanceStepper = i;
      sameDegreeCount = 0;
    }
    else if (distanceStepper[i] == maxDistanceStepper) { 
      ++sameDegreeCount;
      if (sameDegreeCount > sameDegreeCountMax)
      {
        sameDegreeCountMax = sameDegreeCount;
        sameDegreeValueMax = i;
      }
    }
    else if (distanceStepper[i] < maxDistanceStepper){
      sameDegreeCount = 0;
    }
  }
  if (distanceStepper[sameDegreeValueMax] == maxDistanceStepper){
    degreeMaxDistanceStepper = sameDegreeValueMax - sameDegreeCountMax / 2; 
  }
 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount); Serial.print("\n"); Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax); Serial.print("\n"); Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax); Serial.print("\n");
  Serial.print("Dis max: "); Serial.print(maxDistanceStepper); Serial.print(" cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\n");
  stepDegree(-1 * higherBound); // returns to original position
}

void stepperDegreeCheck(int lowerBound, int higherBound, int manualDegreeReturn, int speedControl){ // adds control of degrees returns to avoid extra waiting, along with stepper speed control
  stepper.setSpeed(speedControl);
  stepDegree( -1 * stepDisplacement); 
  stepDisplacement = 0;
  stepDegree(lowerBound);
  for (int i = lowerBound; i <= higherBound; ++i) { // creates array of U.S. measurement values between limits
    cmDistance = ping(echoUS);
    distanceStepper[i] = cmDistance;
   // Serial.print("Dis at: "); Serial.print(i); Serial.print(" deg: "); Serial.print(distanceStepper[i]); Serial.print(" cm"); Serial.print("\n");
    stepDegree(1.0);
    delay(17);
  }
  delay(100);
  sameDegreeCount = 0;  
  sameDegreeCountMax = 0; 
  maxDistanceStepper = distanceStepper[lowerBound];    
  degreeMaxDistanceStepper = lowerBound;
  for (int i = ++lowerBound; i <= higherBound; ++i) { 
    if (distanceStepper[i] > maxDistanceStepper) {
      maxDistanceStepper = distanceStepper[i];
      degreeMaxDistanceStepper = i;
      sameDegreeCount = 0;
    }
    else if (distanceStepper[i] == maxDistanceStepper) {  
      ++sameDegreeCount;
      if (sameDegreeCount > sameDegreeCountMax)
      {
        sameDegreeCountMax = sameDegreeCount;
        sameDegreeValueMax = i;
      }
    }
    else if (distanceStepper[i] < maxDistanceStepper){
      sameDegreeCount = 0;
    }
  }
  if (distanceStepper[sameDegreeValueMax] == maxDistanceStepper){ 
    degreeMaxDistanceStepper = sameDegreeValueMax - sameDegreeCountMax / 2; 
  }
 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount); Serial.print("\n"); Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax); Serial.print("\n"); Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax); Serial.print("\n");
  Serial.print("Dis max: "); Serial.print(maxDistanceStepper); Serial.print(" cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\n");
  stepDegree(manualDegreeReturn); // goes to specified end position
}

void kick(boolean leftKick) {
  halt(WAITTIME); // halt motion to kick
  if (leftKick) {
    myServo.write(115); // backs up 25 degrees, delay 1/4 seconds, kicks to full, hold for one second
    delay(250);
    myServo.write(0);
    delay(1000);
  }
  else if (!leftKick) { // performs right kick
    myServo.write(65);
    delay(250);
    myServo.write(180);
    delay(1000);
  }
  myServo.write(90); // resets servo to 90 (pointed up)
}

void halt(int duration) {
  unsigned long previousMillis = millis();
  while ((previousMillis + duration) >= millis()) {
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    digitalWrite(NXTMOTORONEBACK, LOW);
    digitalWrite(NXTMOTORTWOBACK, LOW);
  }
}

void motorHigh(boolean backwards) {
  if (!backwards){
    digitalWrite(NXTMOTORONE, HIGH);
    digitalWrite(NXTMOTORTWO, HIGH);
    digitalWrite(NXTMOTORONEBACK, LOW);
    digitalWrite(NXTMOTORTWOBACK, LOW);    
  }
  else if (backwards){
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    digitalWrite(NXTMOTORONEBACK, HIGH);
    digitalWrite(NXTMOTORTWOBACK, HIGH);        
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean backwards) { // turnDegree from 0 to 180 (90 is straight), speedIntensity from 0 - 255 for analog motor, true/false for backwards movement
  // if degree is 0, left wheel turns (clockwise), if degree is 180, right wheel turns (counterclockwise)
  double totalDelay = 250.0; // combined motor delay
  double ratioLeft = ((double)(turnDegree)) / 180.0; // degree determines how totalDelay is split up for each motor
  double ratioRight = 1.0 - ratioLeft;
  int delayPtOne = (int)(ratioRight * totalDelay);
  int delayPtTwo = (int)(ratioLeft * totalDelay);
  if (!backwards) {
    digitalWrite(NXTMOTORONEBACK, LOW); // run both motors for lowest time, then run only one motor for the remaining time needed for that ratio
    digitalWrite(NXTMOTORTWOBACK, LOW);
    analogWrite(NXTMOTORONE, speedIntensity);
    analogWrite(NXTMOTORTWO, speedIntensity);
    if (delayPtOne > delayPtTwo) {
      delay(delayPtTwo);
      digitalWrite(NXTMOTORTWO, LOW);
      delay(delayPtOne - delayPtTwo);
    }
    else if (delayPtOne < delayPtTwo) {
      delay(delayPtOne);
      digitalWrite(NXTMOTORONE, LOW);
      delay(delayPtTwo - delayPtOne);
    }
    else if (delayPtOne == delayPtTwo) {
      delay(totalDelay);
    }
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    delay(1);
  }
  else if (backwards) {
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    analogWrite(NXTMOTORONEBACK, speedIntensity);
    analogWrite(NXTMOTORTWOBACK, speedIntensity);
    if (delayPtOne > delayPtTwo) {
      delay(delayPtTwo);
      digitalWrite(NXTMOTORTWOBACK, LOW);
      delay(delayPtOne - delayPtTwo);
    }
    else if (delayPtOne < delayPtTwo) {
      delay(delayPtOne);
      digitalWrite(NXTMOTORONEBACK, LOW);
      delay(delayPtTwo - delayPtOne);
    }
    else if (delayPtOne == delayPtTwo) {
      delay(totalDelay);
    }
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    delay(1);
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean pathClear, boolean backwards) { // only runs if pathClear
  if (pathClear) {
    moveDirection(turnDegree, speedIntensity, backwards);
  }
  else {
    moveDirection(0, speedIntensity, backwards);
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean pathClear, boolean backwards, int stepperTurnRadius) { // runs motors under path clear while turning stepper over input radius, not stimultaneosly
  stepper.setSpeed(700);
  moveDirection(turnDegree, speedIntensity, pathClear, backwards); // alternate between moving motors forward and turning stepper motor
  if (stepperRadiusCount < stepperTurnRadius){ // turn +- stepperTurnRadius from forward facing
    if (stepperTurnDirection){
      stepDegree(1.0); // use small stepper increments so that autonomous can run for most of the time 
    }
    else if (!stepperTurnDirection){
      stepDegree(-1.0);
    }
    ++stepperRadiusCount;
  }
  else if (stepperRadiusCount < 2 * stepperTurnRadius){
    if (!stepperTurnDirection){
      stepDegree(1.0);
    }
    else if (stepperTurnDirection){
      stepDegree(-1.0);
    }
    ++stepperRadiusCount;
  }
  else{
    stepperRadiusCount = 0;
    stepperTurnDirection = !stepperTurnDirection;
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean pathClear, boolean backwards, int stepperTurnRadius, int stepperStepIndividual) { // adds step feature to move specified degrees each time
  stepper.setSpeed(700);
  moveDirection(turnDegree, speedIntensity, pathClear, backwards); 
  motorHigh(backwards);
  if (stepperRadiusCount < stepperTurnRadius){ 
    if (stepperTurnDirection){
      stepDegree(stepperStepIndividual); // use specified degree increment
      stepDisplacement += stepperStepIndividual;  
    }
    else if (!stepperTurnDirection){
      stepDegree(-1.0 * stepperStepIndividual);
      stepDisplacement += (-1 * stepperStepIndividual);
    }
    stepperRadiusCount += stepperStepIndividual;
  }
  else if (stepperRadiusCount < (2 * stepperTurnRadius)){
    if (!stepperTurnDirection){
      stepDegree(stepperStepIndividual);
      stepDisplacement += stepperStepIndividual;  
    }
    else if (stepperTurnDirection){
      stepDegree(-1.0 * stepperStepIndividual);
      stepDisplacement += (-1 * stepperStepIndividual);
    }
    stepperRadiusCount += stepperStepIndividual;
  }
  else{
    stepperRadiusCount = 0;
    stepperTurnDirection = !stepperTurnDirection;
  }
 // Serial.print("stepperRadiusCount: "); Serial.print(stepperRadiusCount); Serial.print("\n");
}

void moveDirection(int turnDegree, int speedIntensity, int duration, boolean backwards) { // runs for specific time duration regardless of pathClear
  unsigned long previousMillis = millis();
  while ((previousMillis + duration) >= millis()) {
    moveDirection(turnDegree, speedIntensity, backwards);
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean pathClear, int duration, boolean backwards) { // runs for specific time duration if pathClear
  unsigned long previousMillis = millis();
  while ((previousMillis + duration) >= millis()) {
    moveDirection(turnDegree, speedIntensity, pathClear, backwards);
  }
}

void checkSurroundings(void) {
  halt(WAITTIME); // stop motor rotation
  stepDegree( -1 * stepDisplacement); // allign stepper to forward facing before recording data
  stepDisplacement = 0;
  int cmDistanceNorth = ping(echoUS);
  int cmDistanceMax = cmDistanceNorth;
  String dirMaxDis = "NORTH"; // max distance direction stored in string
 // Serial.print("Dis no: "); Serial.print(cmDistanceNorth); Serial.print("\n");
  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion); // rotate 1/4 of a circle
  halt(WAITTIME);   // temporarily stop to ensure precise measurement
  int cmDistanceWest = ping(echoUS);
 // Serial.print("Dis we: "); Serial.print(cmDistanceWest); Serial.print("\n");
  if (cmDistanceWest > cmDistanceMax) {
    cmDistanceMax = cmDistanceWest;
    dirMaxDis = "WEST"; // dirMaxDis replaced if needed
  }
  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);
  halt(WAITTIME);
  int cmDistanceSouth = ping(echoUS);
 // Serial.print("Dis so: "); Serial.print(cmDistanceSouth); Serial.print("\n");
  if (cmDistanceSouth > cmDistanceMax) {
    cmDistanceMax = cmDistanceSouth;
    dirMaxDis = "SOUTH";
  }
  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);
  halt(WAITTIME);
  int cmDistanceEast = ping(echoUS);
 // Serial.print("Dis ea: "); Serial.print(cmDistanceEast); Serial.print("\n");
  if (cmDistanceEast > cmDistanceMax) {
    cmDistanceMax = cmDistanceEast;
    dirMaxDis = "EAST";
  }
  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);
  halt(WAITTIME);   // back to facing north
  Serial.print("Dis max: "); Serial.print(cmDistanceMax); Serial.print(" in "); Serial.print(dirMaxDis); Serial.print("\n");
  if (dirMaxDis.equals("NORTH")) { // returns to distance with obstacle farthest in sight by associating dirMaxDis with direction
    halt(WAITTIME);
  }
  else if (dirMaxDis.equals("WEST")) {
    moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);
  }
  else if (dirMaxDis.equals("SOUTH")) {
    moveDirection(180, motorSpeedPot, 2 * turnTime, backwardsMotion);
  }
  else if (dirMaxDis.equals("EAST")) {
    moveDirection(180, motorSpeedPot, 3 * turnTime, backwardsMotion);
  }
}

void checkNearby(double checkDegreeRadius) { // takes three measurements, straight, + checkDegreeRadius, - checkDegreeRadius 
  halt(WAITTIME);
  int rotationTime = (int)(((double)turnTime / 90.0) * checkDegreeRadius); // turnTime / 90 = time to turn 1 degree, multiply by input degree to find total rotation time
  stepDegree( -1 * stepDisplacement); 
  stepDisplacement = 0;
  int cmDistanceStraight = ping(echoUS);
  int cmSideDistanceMax = cmDistanceStraight;
  String sideMaxDis = "STRAIGHT";
 // Serial.print("Dis str: "); Serial.print(cmDistanceStraight); Serial.print("\n");
  moveDirection(180, motorSpeedPot, rotationTime, backwardsMotion);
  halt(WAITTIME);
  int cmDistanceLeft = ping(echoUS);
 // Serial.print("Dis left: "); Serial.print(cmDistanceLeft); Serial.print("\n");
  if (cmDistanceLeft > cmSideDistanceMax) {
    cmSideDistanceMax = cmDistanceLeft;
    sideMaxDis = "LEFT";
  }
  moveDirection(0, motorSpeedPot, 2 * rotationTime, backwardsMotion);
  halt(WAITTIME);
  int cmDistanceRight = ping(echoUS);
 // Serial.print("Dis right: "); Serial.print(cmDistanceRight); Serial.print("\n");
  if (cmDistanceRight > cmSideDistanceMax) {
    cmSideDistanceMax = cmDistanceRight;
    sideMaxDis = "RIGHT";
  }
  moveDirection(180, motorSpeedPot, rotationTime, backwardsMotion);
  halt(WAITTIME);
  Serial.print("Dis max: "); Serial.print(cmSideDistanceMax); Serial.print(" in "); Serial.print(sideMaxDis); Serial.print("\n");
  if (sideMaxDis.equals("STRAIGHT")) { // goes either straight, or left or right degree input depending on where the farthest distance was measured
    halt(WAITTIME);
  }
  else if (sideMaxDis.equals("LEFT")) {
    moveDirection(180, motorSpeedPot, rotationTime, backwardsMotion);
  }
  else if (sideMaxDis.equals("RIGHT")) {
    moveDirection(0, motorSpeedPot, rotationTime, backwardsMotion);
  }
}

void buttonDriver(void){ // auto control using only buttons
  if (digitalRead(BUTTONFORWARDONE) == LOW || digitalRead(BUTTONFORWARDTWO) == LOW){ // checks if any push button pushed
    buttonDriveForward = false;
    ++buttonCycleCount;
    if (buttonCycleCount % 4 == 0){ // turn every 2 button pushes
      moveDirection(180, 255, 2 * turnTime, false); 
      buttonCycleCount = 0;
    }
  }
  else if (digitalRead(BUTTONBACKWARDONE) == LOW || digitalRead(BUTTONBACKWARDTWO) == LOW){
    buttonDriveForward = true;
    ++buttonCycleCount;
    if (buttonCycleCount % 2 == 0){
      moveDirection(0, 255, turnTime, false); 
      buttonCycleCount = 0;
    }
  }
  if (buttonDriveForward){ // move in opposite direction of push after pushed impulse
    moveDirection(90, 255, false);
  }
  else if (!buttonDriveForward){
    moveDirection(90, 255, true);
  }
}

void autonomous(void){
  unsigned long currentMillis = millis();
  ++numberOfLoops; // used to make checkSurroundings and checkNearby which run periodically

  /*valPot = analogRead(POTPIN);
    motorSpeedPot = (int)(valPot / 4.01); // ratio to convert pot reading to analog
    if (motorSpeedPot > 255){ // ensures max speed is 255 and min speed is 0
    motorSpeedPot = 255;
    }
    else if (motorSpeedPot < 0){
    motorSpeedPot = 0;
    }*/
  motorSpeedPot = 255; // valPot changed and varied around 255, sometimes going to zero and stopping autonomous car functions
  fractionOfFullSpeed = ((double)motorSpeedPot) / 255.0;
  turnTime = (int)(1350.0 / fractionOfFullSpeed); // for carpet 1350

  backwardsMotion = false;
  cmDistance = ping(echoUS);
  currentCm = cmDistance;
  double randomTurnDegreeRadius = (double)(random(12, 37));
  
  if (currentCm <= 8) { // if object within 8 cm, robot can't do anything other than kick to defend itself, then turns stepper 360 degrees and moves to that position
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");    
    kick(false);
    backwardsMotion = true;
    moveDirection(90, motorSpeedPot, 2 * turnTime, backwardsMotion); // back up robot
    halt(WAITTIME);
    stepperCircleDegreeCheck();
    kick(false);
    int turnTimeFarthest = (turnTime / 90) * degreeMaxDistanceStepper;
   // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest); Serial.print("\n");
    backwardsMotion = false;
    moveDirection(180, motorSpeedPot, turnTimeFarthest, backwardsMotion); // turn to degree where saw farthest with U.S. circle rotation from stepper
  }
  else if (currentCm <= 16) { // if object within 8 to 16 cm
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");    
    kick(false);
    backwardsMotion = true;
    moveDirection(90, motorSpeedPot, 4 * turnTime / 3, backwardsMotion); 
    halt(WAITTIME);
    stepDegree(-1 * STEPPERDEGREERADIUS); 
    stepperDegreeCheck(0, 2 * STEPPERDEGREERADIUS, -1 * STEPPERDEGREERADIUS, 600);
    int turnTimeInitial = (turnTime / 90) * (STEPPERDEGREERADIUS); // time for motors to turn depending on which angle stepper says max distance is
    backwardsMotion = false;
    // turn to degree where saw farthest with U.S. circle rotation from stepper  
    if (degreeMaxDistanceStepper < STEPPERDEGREERADIUS){ // for east side of measurements 
      turnTimeFarthest = (turnTime / 90) * (STEPPERDEGREERADIUS - degreeMaxDistanceStepper); // calculate turnTimeFarthest (stepper only uses positive degreees), and drive motors to either direction      
     // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest); Serial.print("\n");
      moveDirection(0, motorSpeedPot, turnTimeFarthest, backwardsMotion); 
    }
    else if (degreeMaxDistanceStepper > STEPPERDEGREERADIUS){ // for west side of measurements
      turnTimeFarthest = (turnTime / 90) * (degreeMaxDistanceStepper - STEPPERDEGREERADIUS);   
     // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest); Serial.print("\n");
      moveDirection(180, motorSpeedPot, turnTimeFarthest, backwardsMotion);
    }
  }
  else if (currentCm <= 28) { // checkSurroundings if object within 16 to 28 
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");
    checkSurroundings();
  }
  else if (currentCm <= 44) { // checkNearby if object within 28 to 44 cm to find a slight deviation
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");
    checkNearby(randomTurnDegreeRadius);
  }
  else if (currentCm <= 53) { // pathClear false between 44 to 53 cm so robot rotates counterclockwise
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");
    pathClear = false;
  }
  else {
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");
   // Serial.print("Speed: "); Serial.print(motorSpeedPot); Serial.print("\n");
    pathClear = true; // goes straight if no object within 52 cm is detected
  }
  
  // interval of repeat is random within boundaries
  if (numberOfLoops % random(70, 141) == 0) { // checkSurroundings once every random or 140 loops
    checkSurroundings(); // sometimes doesn't run
  }
  else if (numberOfLoops % random(35, 71) == 0) { // once every random or 35 loops, checks +-15 degrees or random setting
    randomTurnDegreeRadius = (double)(random(18, 37)); // average of 27 degrees turn radius for three distance checks
    checkNearby(randomTurnDegreeRadius);
  }
  else {
    moveDirection(90, motorSpeedPot, pathClear, backwardsMotion, 56, 7); // drive north at motorSpeedPot if pathClear with background stepper turning over radius of 56 degrees with 8-7 degree increments
  }
}

void volumeUpIRDirControl(void){
  if (controlDirVolume <= (180 - controlDirVolIncrement)){ // increment 5 degrees, or specified increment, every time volume up pressed until 180 degrees
    controlDirVolume += controlDirVolIncrement;
    controlDir = controlDirVolume;
  }
  else{
    controlDir = 180;
  }
}

void volumeDownIRDirControl(void){ // not using volume down key, called when channel up key pressed
  if (controlDirVolume >= controlDirVolIncrement){ // decrease specific degree increment
    controlDirVolume -= controlDirVolIncrement;
    controlDir = controlDirVolume;  
    }
  else{
    controlDir = 0;
  }
}

void remoteInput(void) { // data of ir reciever from ir transmitor remote from sony tv remote
  manualStepper = false; // only true when their specific button pushed
  buttonDriverOverride = false; 
  switch (results.value) // robot actions for pushing buttons 
  { 
    case 0xA90: controlSpeed = 0;                     controlBackwards = false;             stepDegree(-1 * stepDisplacement); stepDisplacement = 0; autoOff = !(autoOff); break; // power button: change between autopilot and manual modes
    case 0x910: controlSpeed = 0;                                                                                                                                          break; // 0 button: stop nxt motion
    case 0x710: controlSpeed = 0;                                                                                                                                          break; // guide button: stop nxt motion (same as above)
    case 0x110: controlSpeed = 255; controlDir = 0;                                                                                                                        break; // 9 button: 0 degrees
    case 0xA10: controlSpeed = 255; controlDir = 30;                                                                                                                       break; // 6 button: 30 degrees
    case 0x410: controlSpeed = 255; controlDir = 60;                                                                                                                       break; // 3 button: 60 degrees
    case 0x810: controlSpeed = 255; controlDir = 90;                                                                                                                       break; // 2 button: 90 degrees (straight)
    case 0x10:  controlSpeed = 255; controlDir = 120;                                                                                                                      break; // 1 button: 120 degrees
    case 0xC10: controlSpeed = 255; controlDir = 150;                                                                                                                      break; // 4 button: 150 degrees
    case 0x610: controlSpeed = 255; controlDir = 180;                                                                                                                      break; // 7 button: 180 degrees
    case 0x210: controlSpeed = 255; controlDir = 90;  controlBackwards = false;                                                                                            break; // 5 button: north (forward)
    case 0xE10: controlSpeed = 255; controlDir = 90;  controlBackwards = true;                                                                                             break; // 8 button: south (backward) 
    case 0x2D0: controlSpeed = 255;                                                         volumeUpIRDirControl();                                                        break; // left arrow button: increase left turn angle
    case 0xCD0:  controlSpeed = 255;                                                        volumeDownIRDirControl();                                                      break; // vol down button: kick servo forward
    case 0x2F0: controlSpeed = 255; controlDir = 90;                                        controlDirVolume = 90;                                                         break; // up arrow button: reset volume turn degree to forward (90 degrees)    
    case 0xC90: controlSpeed = 0;                                                           kick(false);                                                                   break; // right arrow button: increase right turn angle
    case 0x890: controlSpeed = 0;                                                           kick(true);                                                                    break; // ch down button: kick servo backward
    case 0x490: controlSpeed = 0;                                                           manualStepper = true; stepperClockwise = false;                                break; // volume up button: turn stepper counterclockwise
    case 0x90: controlSpeed = 0;                                                            manualStepper = true; stepperClockwise = true;                                 break; // channel up button: turn stepper clockwise
    case 0x3B0: controlSpeed = 0;                                                           stepDegree(-1 * stepDisplacement); stepDisplacement = 0;                       break; // freeze button: reset ultrasonic sensor to facing forward by moving stepper
    case 0xA70: controlSpeed = 0;                     controlBackwards = !controlBackwards;                                                                                break; // center of arrows button: change directions from backwards to forwards and vise versa
    case 0x270: controlSpeed = 0;                                                           buttonDriverOverride = true;                                                   break; // picture button: autonomous movemment dependent only on push buttons on bumpers  
  }
  irrecv.resume();
}

void loop(void) {
  stepper.setSpeed(450); // max speed around 700, various functions manipulate stepper speed
  if (digitalRead(BUTTONFORWARDONE) == LOW || digitalRead(BUTTONFORWARDTWO) == LOW){ // read button state (pushed or not)
    forwardButtonControl = true;
  }
  else if (digitalRead(BUTTONBACKWARDONE) == LOW || digitalRead(BUTTONBACKWARDTWO) == LOW){
    backwardButtonControl = true;
  }
  if (!autoOff) { // if autonomous, check for remoteInput or run autonomous mode
    if (irrecv.decode(&results)) {
      remoteInput();
    }
    else {
      if (forwardButtonControl){ // if push button pushed, autonomous reacts quickly, else runs autonomous 
        forwardObstacleButton();
        forwardButtonControl = false;
      }
      else if (backwardButtonControl){  
        backwardObstacleButton();
        backwardButtonControl = false;
      }
      else{
        autonomous();        
      }
    }
  }
  else if (autoOff) { // if autonomous off, check for another remoteInput or run based on previous remoteInput
    if (irrecv.decode(&results)) {
      remoteInput();
    }
    else {
      if (!buttonDriverOverride){
        if (!manualStepper) { // rotate stepper if stated, or run remoteInput task
          moveDirection(controlDir, controlSpeed, controlBackwards);
        }
        else if (manualStepper) { 
          if (stepperClockwise) {
            stepDegree(-1.0);
            --stepDisplacement;
          }
          else if (!stepperClockwise) { 
            stepDegree(1.0);
            ++stepDisplacement;
          }
        }
      }
      else if (buttonDriverOverride){
        buttonDriver();
      }
    }
  }
}

int ping(byte echoUS)
{
  long duration;
  long cm;
  pinMode(TRIGUS, OUTPUT);
  digitalWrite(TRIGUS, LOW); // give short low pulse to ensure clean high pulse
  delayMicroseconds(2);
  digitalWrite(TRIGUS, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGUS, LOW);
  pinMode(echoUS, INPUT);
  duration = pulseIn(echoUS, HIGH, 10000);  // limits pulse check to 10 ms avoid stuttering (bug due to ultrasonic sensor not recieving pulse back on long distances since not strong enough)
  cm = microsecondsToCentimeters(duration);
  if (cm == 0) {
    cm = 200; // returns high distance of 200 if nothing found, actual zero cm is never read, lowest reading seems to be around 3 cm
  }
  return (int)cm ; // int type used with ping to determine cmDistance
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 58; // convert time to distance
}

// TO DO/ FIX:
// function which allows robot to go back to starting place (integrator) constantly calculating 2d position
// fix potentiometer analogIn: not consistent values, leads to one motor off, only controls speed of one motor when used, try interrupt
// l9110 motor only drives one motor with analog values and one motor with digital values (even when analog in code)
// make millis non blocking: run stepper at same time as nxt motors, not rapid alteration
// test to find exact number for steps to make a circle- less than 2048 when fast rotation with no measurements; then 2048 when making measurements
// find a way to know if motors hung up and not rotating- find power consumption
// in remote mode, doesn't stop moving backwards when stepper pressed
// recenter robot to degree found when moving staight and sees object, then run avoidance task, backDir = false
// use bluetooth module to tranfer data