 #include <Wire.h>
 #include <digitalWriteFast.h>
 #include <math.h>
+
+/*
+This sketch can be used to collect data to be used in the system ID of a wheel motor. Essentially
+it sends a series of random voltage commands to the wheel motor for random durations and then
+periodically measures the average angular velocity of the wheels over a period.
+
+Modify the parameters as desired below, upload the sketch, and place the robot on a clear space on
+the ground. Establish a terminal connection with the communication port of the XBEE and switch
+on the robot. The robot will turn in place (or more accurately, turn about one of its wheels)
+for a period of time. When the test is complete, the onboard XBEE will transmit the results.
+Note that all time measurements are in milliseconds and the transmitted velocity measurements
+are in encoder ticks per velocity interval, where the velocity interval is user-defined. One 
+can easily convert this to radians per second using:
+
+  w = 1000 * 2 * PI * MEASURED_TICKS / TOTAL_TICKS
+
+where TOTAL_TICKS is the number of ticks per revolution, which is approximately 3000.
+
+Author: Ryan J. Marcotte
+Date: 15 March 2014
+*/
+
+//#####SYSTEM ID PARAMETERS#######
+//#####MODIFY THESE FIELDS########
+#define TEST_PIN FORWARD_LEFT //the pin of the motor you are working with (options: FORWARD_RIGHT,
+                                  //FORWARD_LEFT, REVERSE_RIGHT, REVERSE_LEFT)
+const int TOTAL_VOL_COMS = 20; //the total number of voltage commands to be executed by the robot
+                               //if this is set too high, memory can become an issue
+                               //if the board runs out of memory, it resets itself which can be 
+                               //a challenging bug to diagnose
+const int MIN_COM_DUR = 5; //the minimum duration a command could be executed for
+const int MAX_COM_DUR = 2000; //the maximum duration a command could be executed for
+const int COM_DUR_RESOLUTION = 5; //command durations can take on values as multiples of
+                                          //this parameter; 5 ms recommended
+const int VELOCITY_INTERVAL = 20; //length of interval (in ms) velocity measurements are taken over
+                                 //20 ms is probably optimal for most situations
+
+//##########END PARAMETERS###########
+//###NO MODIFICATIONS NEEDED BELOW###
 
 //######ENCODERS######
 //Left Encoder

  digitalWrite(c_RightEncoderPinB, LOW); //turns on pullupresistors
  attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
  
  Prev_RightEncoderTicks = _RightEncoderTicks;
  Prev_LeftEncoderTicks = _LeftEncoderTicks;
  
  //Motors
  pinMode(reverseLeftMotorPin, OUTPUT);
  pinMode(forwardLeftMotorPin, OUTPUT);
  pinMode(reverseRightMotorPin, OUTPUT);
  pinMode(forwardRightMotorPin, OUTPUT);
  
  generateCommandArrays();
  
  outputResults();
  
  performTest();
}

void loop(){
}

void generateCommandArrays(){
  randomSeed(analogRead(0));
  
  for(int i = 0; i < totalVolComs; i++){
    volComs[i] = random(256);
    volComTimes[i] = 5*random(1,400);
  }  
}

void performTest(){
  const unsigned long baseTime = millis();
  currentTime = baseTime;
  analogWrite(reverseLeftMotorPin, volComs[volComsIndex]);
  lastCommandTime = currentTime;
  lastMeasureTime = currentTime;
  
  while(currentTime - baseTime < totalTime){
    currentTime = millis();
    commandNow = (currentTime - lastCommandTime >= volComTimes[volComsIndex] && lastCommandTime != currentTime) ? true : false;
    measureNow = (currentTime - lastMeasureTime >= 20 && lastMeasureTime != currentTime) ? true : false;
    
    if(commandNow || measureNow){
      if(commandNow){
        analogWrite(reverseLeftMotorPin, volComs[++volComsIndex]);
        lastCommandTime = currentTime;
      }
      
      if(measureNow){
        vels[velsIndex] = _LeftEncoderTicks - Prev_LeftEncoderTicks;
        lastMeasureTime = currentTime;
        Prev_LeftEncoderTicks = _LeftEncoderTicks;
        velsTimes[velsIndex] = currentTime - baseTime;
        velsCommandIndex[velsIndex++] = volComsIndex;
      }
    }
  }
  
  digitalWrite(reverseLeftMotorPin, LOW);
}

void outputResults(){
  for(int i = 0; i < totalVolComs; i++){
    Serial.print(volComs[i]);
    delay(5);
    Serial.print(" ");
    delay(5);
    Serial.println(volComTimes[i]);
    delay(5);
  }
  
  Serial.println("End Commands\n\nStart Measurements");
  delay(20);
  
  for(int i = 0; i < velsSize; i++){
    Serial.print(velsTimes[i]);
    delay(5);
    Serial.print(" ");
    delay(5);
    Serial.print(velsCommandIndex[i]);
    delay(5);
    Serial.print(" ");
    delay(5);
    Serial.println(vels[i]);
    delay(5);
  }
  


//######MOTORS#########
//Left Motor
const int REVERSE_LEFT = 11;
const int FORWARD_LEFT = 10;

//Right Motor
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
