 #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


//######MOTORS#########
//Left Motor
const int reverseLeftMotorPin = 11;
const int forwardLeftMotorPin = 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;

void setup(){
  Serial.begin(9600);
  
  //Left Encoder
  pinMode(c_LeftEncoderPinA, INPUT);     //sets pin A as input
  digitalWrite(c_LeftEncoderPinA, LOW);  //turns on pullup resistors

  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);
  }
  
