//////////////////////////
// Distributed to Class //
//////////////////////////

///////////////
// CONSTANTS //
///////////////
const int minSpeed = 300;        // this is the minimum speed we will test the PID controller at, set in RPM
const int maxSpeed = 5000;       // this is the max speed we will test the PID Controller at, set in RPM
const int zeroSpeed = 0;         // having a design that works at 0 RPM will be extra credit
const int responseTime = 1000;   // hold the motor at this speed during the step function portion of the test program, in ms
const int rampDelay = 500;       // hold the motor at this speed during the ramps and sinusoid testing, in ms

///////////////
// VARIABLES //
///////////////
volatile int motorSpeed;         // used for debugging purposes, units are RPM
volatile int pidSpeed;           // measured speed of the motor controlled by the pid controller, set in an ISR
unsigned long totalError;		 // tracks the total error in the design
int timerISRcnt;                 // this is used to slow down the timer ISR for the speed comparisons
unsigned long measurements;		 // number of measurements performed by the test program

void setup() {
  
  // operate the serial communication as fast as possible
  Serial.begin(115200);
  
  // set up the external and timer interrupts here
  SetupTimer2();
  
  // initialize the motor to half speed
  motorSpeed = maxSpeed >> 1;
}

// Runs in normal mode, interrupt on overflow
void SetupTimer2(){
  
  // Set to run in normal mode
  TCCR2A = 0;
  
  // Timer clock = 16MHz/1024 = 15.625khz
  TCCR2B = 1<<CS22 | 1<<CS21 | 1<<CS20;
  
  // TCNT2 set to result, will count up and overflow
  TCNT2 = 0;
  
  // Enable timer overflow interrupt flag
  TIMSK2 = 1<<TOIE2;
  
  // Set enable interrupts
  sei();
}

/* 
  Test Program: Timer 2 continually prints set speed and measured speed
  
  1) step function to near slow speed
  2) step function to full speed
  3) ramp low
  4) ramp high
  5) sine wave (near zero to full speed), report accumulated error
  6) step function to zero speed
*/
void loop() {
    
  // reset the total error for this loop
  totalError = 0;
  
  // reset the measurement counter for this loop
  measurements = 0;
  
  // write to slow speed and hold
  motorSpeed = minSpeed;
  delay(responseTime);
  
  // write to fast speed and hold
  motorSpeed = maxSpeed;
  delay(responseTime);
  
  // ramp to slow speed
  for(motorSpeed=maxSpeed; motorSpeed>=minSpeed; motorSpeed-=(maxSpeed-minSpeed)>>5){
    delay(rampDelay);
  }
  
  // ramp to fast speed
  for(motorSpeed=minSpeed; motorSpeed<=maxSpeed; motorSpeed+=(maxSpeed-minSpeed)>>5){
    delay(rampDelay);
  }

  // output a sinusoidal wave
  for(float angle=0; angle < 6.28; angle+=6.28/32){
    motorSpeed = int((maxSpeed-minSpeed)*sin(angle)) + (maxSpeed+minSpeed)>>1;
    delay(rampDelay);
  }
  
  // report the error at this point, this is all that will be required
  Serial.print("Error before 0 speed: ");
  Serial.print(totalError);
  Serial.print("\tMeasurements: ");
  Serial.println(measurements);
  
  // step to zero speed now, this is extra credit
  motorSpeed = 0;
  delay(responseTime);
  
  // report the final error
  Serial.print("Final error: ");
  Serial.print(totalError);
  Serial.print("\tMeasurements: ");
  Serial.println(measurements);
}

// ISR calculates the desired motor speed, PID controlled motor speed, and error
ISR(TIMER2_OVF_vect) {

  // track the number of times the ISR has been entered to slow the interrupt to desired frequency
  timerISRcnt++;
  
  // slow the timer ISR frequency to 10 Hz
  if(timerISRcnt == 6){
    
    /////////////////////////////////////////////////
    // assume the motor speeds are calculated here //
    /////////////////////////////////////////////////
    
    // assume the motor speeds (in RPM) have been calculated
    if(motorSpeed >= pidSpeed){
      totalError += motorSpeed - pidSpeed;
    }else{
      totalError += pidSpeed - motorSpeed;
    }
    timerISRcnt = 0;
    
    // track the number of measurements taken during the test program
    measurements++;
    
    // print the speeds to the screen for debug ability
    // can copy and paste these into an Excel Spreadsheet
    Serial.print(motorSpeed);
    Serial.print("\t");
    Serial.println(pidSpeed);
  }
}

// use this for the interrupt of the motor driven by the tester
ISR(motorISR){
  
}

// use this for the motor controlled by the PID controller
ISR(pidISR){
  
}