 
 //######MOTORS#########
 //Left Motor
-const int reverseLeftMotorPin = 11;
-const int forwardLeftMotorPin = 10;
+const int REVERSE_LEFT = 11;
+const int FORWARD_LEFT = 10;
 
 //Right Motor
-const int reverseRightMotorPin = 9;
-const int forwardRightMotorPin = 8;
-
-//######SYSTEM ID#######
-//Modify these fields
-const int totalVolComs = 20; //the total number of voltage commands to be executed by the robot
-const int testPin1 = forwardLeftMotorPin;
-const int testPin2 = forwardRightMotorPin;
-
-int volComs[totalVolComs]; //the voltage commands to be executed by robot
-int volComsIndex = 0; //keeps track of which voltage command the robot is current executing
-unsigned long volComTimes[totalVolComs]; //the lengths of time each voltage command is executed
-
-const int totalTime = 20000; //the total amount of time the robot should measure angular velocity of the wheels
-const int velsSize = totalTime / 20;
-int vels[velsSize]; //the velocities of the wheels, measured every 20 ms
-int velsIndex = 0; //keeps track of which measurement is currently being executed
-int velsTimes[velsSize];
-int velsCommandIndex[velsSize];
-
-unsigned long currentTime;
-unsigned long lastMeasureTime;
-unsigned long lastCommandTime;
-boolean commandNow = false;
-boolean measureNow = false;
-
+const int REVERSE_RIGHT = 9;
+const int FORWARD_RIGHT = 8;
+
+/*
+ * Drives the execution of the system ID routine. Initializes the pins to be used. Generates
+ * the arrays of voltage commands (magnitude & duration). Determines the total runtime based
+ * on the generated command durations. Performs the system ID test. Outputs a summary of the 
+ * parameters of the test as well as the velocity results.
+ */
 void setup(){
   Serial.begin(9600);
-  
+  long runtime;
+  int velsSize;
+  
+  initializePins();
+  
+  Prev_RightEncoderTicks = _RightEncoderTicks;
+  Prev_LeftEncoderTicks = _LeftEncoderTicks;
+  
+  int volComs[TOTAL_VOL_COMS];
+  unsigned long volComTimes[TOTAL_VOL_COMS];
+  generateCommandArrays(volComs, volComTimes);
+  
+  runtime = determineRuntime(volComTimes);  
+  velsSize = runtime / VELOCITY_INTERVAL;
+  int vels[velsSize];
+  int velsTimes[velsSize];
+  int velsCommandIndexes[velsSize];
+  
+  performTest(runtime, velsSize, vels, velsTimes, velsCommandIndexes, volComs, volComTimes);
+  outputSummary(runtime, volComs, volComTimes);
+  outputResults(velsSize, vels, velsTimes, velsCommandIndexes);
+}
+
+/*
+ * Intentionally left empty. Robot simply idles upon completion of setup routine.
+ */
+void loop(){
+}
+
+/*
+ * Sets up encoder pins and attaches interrupts. Sets all motor pins to output mode.
+ */
+void initializePins(){
   //Left Encoder
   pinMode(c_LeftEncoderPinA, INPUT);     //sets pin A as input
   digitalWrite(c_LeftEncoderPinA, LOW);  //turns on pullup resistors

#include <Wire.h>
#include <digitalWriteFast.h>
#include <math.h>

//######ENCODERS######
//Left Encoder

  digitalWrite(c_RightEncoderPinB, LOW); //turns on pullupresistors
  attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
  
  //Motors
  pinMode(FORWARD_RIGHT, OUTPUT);
  pinMode(FORWARD_LEFT, OUTPUT);
  pinMode(REVERSE_RIGHT, OUTPUT);
  pinMode(REVERSE_LEFT, OUTPUT);
}

/*
 * Randomly generates the lists of command magnitudes and durations based
 * on the user's desired parameters of the minimum and maximum command times
 * as well as the command time resolution. If identical command lists are 
 * desired repeatedly for test purposes, comment out the fist line that seeds
 * the random number generator.
 */
void generateCommandArrays(int volComs[], unsigned long volComTimes[]){
  //randomSeed(analogRead(0));
  unsigned long min;
  unsigned long max;  
  
  for(int i = 0; i < TOTAL_VOL_COMS; i++){
    volComs[i] = random(256);
    min = COM_DUR_RESOLUTION * MIN_COM_DUR;
    max = COM_DUR_RESOLUTION * MAX_COM_DUR;
    volComTimes[i] = COM_DUR_RESOLUTION * random(min, max);
  }  
}

/*
 * Determines the total runtime of the test, calculated as the sum of 
 * the voltage command durations.
 */
long determineRuntime(unsigned long volComTimes[]){
  unsigned long runtime = 0;
  
  for(int i = 0; i < TOTAL_VOL_COMS; i++) {
    runtime += volComTimes[i];
  }
  
  runtime += (VELOCITY_INTERVAL - runtime % VELOCITY_INTERVAL);
  
  return runtime;
}

/*
 * Measures repeated step responses on the motor being tested. Each time the main loop iterates, the
 * function checks to see if the current time dictates that a measurement should be taken or a new 
 * command should be executed.
 *
 * Note: An unresolved issue remains with utilizing the millis timing. This code is written under the 
 * assumption that the main while loop iterates many times each millisecond. Microsecond timing tests
 * showed that it executed in approximately 50-100 us, which supports this assumption. However, repeated
 * tests of the code also showed that the while loop appears to "skip over" certain millisecond times.
 * For example, if voltage command or velocity measurement is supposed to be sent or taken at a time
 * 80 ms after the base time, the loop skips over that time and does not take the appropriate action.
 * As a result, a command or measurement may be delayed by up to a millisecond. This is reported in the
 * final results that are output to the user, so it is not terribly significant. However, this is an
 * issue to be explored.
 */
void performTest(const unsigned long RUNTIME, const int VELS_SIZE, int vels[], int velsTimes[], 
                   int velsCommandIndexes[], const int VOL_COMS[], const unsigned long VOL_COM_TIMES[]){
  int velsIndex = 0;
  int volComsIndex = 0;
  boolean commandNow = false;
  boolean measureNow = false;

  const unsigned long BASE_TIME = millis();
  unsigned long currentTime = BASE_TIME;
  unsigned long lastCommandTime = currentTime;
  unsigned long  lastMeasureTime = currentTime;
  
  analogWrite(TEST_PIN, VOL_COMS[volComsIndex]);
  
  while(currentTime - BASE_TIME < RUNTIME){
    currentTime = millis();
    commandNow = (currentTime - lastCommandTime >= VOL_COM_TIMES[volComsIndex] && lastCommandTime != currentTime) ? true : false;
    measureNow = (currentTime - lastMeasureTime >= VELOCITY_INTERVAL && lastMeasureTime != currentTime) ? true : false;
    
    if(commandNow || measureNow){
      if(commandNow){
        analogWrite(TEST_PIN, VOL_COMS[++volComsIndex]);
        lastCommandTime = currentTime;
      }
      
      if(measureNow){
        if(TEST_PIN == FORWARD_RIGHT || TEST_PIN == REVERSE_RIGHT) {
          vels[velsIndex] = _RightEncoderTicks - Prev_RightEncoderTicks;
          Prev_RightEncoderTicks = _RightEncoderTicks;
        } else {
          vels[velsIndex] = _LeftEncoderTicks - Prev_LeftEncoderTicks;
          Prev_LeftEncoderTicks = _LeftEncoderTicks;
        }
        
        lastMeasureTime = currentTime;
        velsTimes[velsIndex] = currentTime - BASE_TIME;
        velsCommandIndexes[velsIndex++] = volComsIndex;
      }
    }
  }
  
  digitalWrite(TEST_PIN, LOW);
}

/*
 * Outputs a summary of the test, including the user-selected parameters and the generated list 
 * of voltage command magnitudes and durations.
 */
void outputSummary(const long RUNTIME, const int VOL_COMS[], const unsigned long VOL_COM_TIMES[]){
  Serial.println("System ID Summary");
  delay(20);
  Serial.print("Motor tested: ");
  delay(20);
  Serial.println(getPinName());
  delay(20);
  Serial.print("Total commands executed: ");
  delay(20);
  Serial.println(TOTAL_VOL_COMS);
  delay(20);
  Serial.print("Total run time: ");
  delay(20);
  Serial.println(RUNTIME);
  delay(20);
  Serial.print("Velocity interval: ");
  delay(20);
  Serial.println(VELOCITY_INTERVAL);
  delay(20);
  Serial.println("\n\nVoltage Commands");
  delay(20);
  Serial.println("Magnitude   Duration");
  delay(20);
  
  for(int i = 0; i < TOTAL_VOL_COMS; i++){
    Serial.print(VOL_COMS[i]);
    delay(5);
    Serial.print("   ");
    delay(5);
    Serial.println(VOL_COM_TIMES[i]);
    delay(5);
  }
  
  Serial.println("End Summary");
}

/*
 * Returns the name of the pin being tested as a string.
 */
String getPinName() {
  switch(TEST_PIN) {
    case FORWARD_RIGHT:
      return "Forward Right";
    case FORWARD_LEFT:
      return "Forward Left";
    case REVERSE_RIGHT:
      return "Reverse Right";
    case REVERSE_LEFT:
      return "Reverse Left";
  }
}

/*
 * Outputs the results of the measurements, including the time of the measurements and the index of the voltage
 * command being executed during that measurement.
 */
void outputResults(const int VELS_SIZE, const int VELS[], const int VELS_TIMES[], const int VELS_COMMAND_INDEXES[]){  
  Serial.println("\n\nVelocity Measurements");
  delay(20);
  Serial.print("Elapsed Time   ");
  delay(20);
  Serial.print("Command Index   ");
  delay(20);
  Serial.print("Measurement");
  delay(20);
  
  for(int i = 0; i < VELS_SIZE; i++){
    Serial.print(VELS_TIMES[i]);
    delay(5);
    Serial.print(" ");
    delay(5);
    Serial.print(VELS_COMMAND_INDEXES[i]);
    delay(5);
    Serial.print(" ");
    delay(5);
    Serial.println(VELS[i]);
    delay(5);
  }
  
