//---------------------------------------------------------------------------

// Pololu MC 33926 motor driver with 'CPR' counts per revolution at the output shaft.
//
// Designed to drive the motor using a pot and read the velocity and the accelerometer values.
//
// Created  07/11/13 Joan Aguilar
// Modified 05/01/14 Joan Aguilar
// Modified 04/29/16 Brendan + Diogo + Sumner

//---------------------------------------------------------------------------

// If using interrupts, it only works with motor 2
const int D2 = 4;  // D2 pin, disables the motors if low
const int M2DIR = 8;  // Motor 2 direction 
const int M2PWM = 10;  // Motor 2 input (PWM)
const int M2EN  = 12;  // Motor 2 Diag
const int M2FB  = 15;  // (Analog1 pin)  Motor 2 current sense
const float CPR = 464.1;  // Encoder counts per revolution

// Encoder/interrupt pins
const int encoderApin = 2;
const int encoderBpin = 3;

// Input pot pins
const int potValPin = 19; // A5

// Accelerometer pin
const int accelPin = 18; // A4

// Encoder status
volatile int encoderA;
volatile int encoderB;

// Time variables
unsigned long currentMicros;
unsigned long oldMicros;
unsigned long oldPrintMicros;

// Position variables
const int origPos = 0;  // Original position
volatile long rawPos;  // Actual position
float Pos;
float oldPos;  // Old position value

// Direction variable
boolean forward;

// Velocity variables
float Vel;
float filteredVel;
float intVel = 0;
float rpmVel;

// Pot value
int potVal;

// Accelerometer value
const int acceln = 200;
const int accelMean = 263;
const int accelRange= 51;
float accelVal;
float accelValArray[acceln];
float accelValMax;
int accelMaxIndex;

// Filter variables
const int nFilterVal = 200;
float filterVal [nFilterVal];

// Control
float u;




//---------------------------------------------------------------------------




void setup() {
  // Enable motors
  pinMode(D2, OUTPUT);
  digitalWrite(D2, HIGH);
  
  // Enable motor direction pin
  pinMode(M2DIR, OUTPUT);
  
  // Enable current sense
  pinMode(M2FB, INPUT);
  
  // Enable encoder
  pinMode(encoderApin, INPUT);
  pinMode(encoderBpin, INPUT);
  
  // Enable pot pin
  pinMode(potValPin, INPUT);
  
  // Enable accelerometer pin
  pinMode(accelPin, INPUT);
  
  // Read encoder status
  encoderA = digitalRead(encoderApin);
  encoderB = digitalRead(encoderBpin);
  
  // Attach interrupts
  attachInterrupt(0, encoderAchange, CHANGE);
  attachInterrupt(1, encoderBchange, CHANGE);
  
  // Read pot value
  potVal = analogRead(potValPin);
  
  // Initialize accelerometer
  for (int n = 0; n < acceln; n++) {
    accelValArray[n] = 0.0;
  }
  accelVal = abs((float)(analogRead(accelPin) - accelMean)/accelRange);
  accelValArray[0] = accelVal;
  accelValMax = accelVal;
  accelMaxIndex = 0;
  
  
  // Set time variables
  currentMicros = micros();
  oldMicros = currentMicros;
  oldPrintMicros = currentMicros;
  
  // Set pos variables
  rawPos = origPos;  // Set raw position  
  Pos = 2*PI*rawPos/CPR;  // Set position
  oldPos = Pos;  // Set old position
  
  // Set velocity
  Vel = 0;
  
  // Set direction
  forward = true;
  
  // Initialize filter
  for (int n = 0; n < nFilterVal; n++) {
    filterVal[n] = 0;
  }
  filteredVel = filter(filterVal, nFilterVal);
  rpmVel = 30*filteredVel/PI;
  
  // Initialize control
  u = 0.0;
  
  
  // Open serial port
  Serial.begin(115200);
}




//---------------------------------------------------------------------------




void loop() {
  
  // Read/compute status
  currentMicros = micros();
  Pos = 2*PI*rawPos/CPR;  // In rad
  
  if (currentMicros != oldMicros) {
    Vel = 1000000.0*(Pos - oldPos)/(currentMicros - oldMicros);  // In rad/s
  }
  potVal = analogRead(potValPin);
  accelVal = ((float)(analogRead(accelPin) - accelMean)/accelRange);
  
  u = velController(Vel, intVel, PI*potVal/35);
  runMotor(u);
  
  
  // Update values
  oldPos = Pos;
  oldMicros = currentMicros;
  updateArray(accelVal, accelValArray, acceln);
  accelMaxIndex++;
  if (accelVal >= accelValMax) {
    accelValMax = accelVal;
    accelMaxIndex = 0;
  }
  if(accelMaxIndex >= acceln) {
    searchMax(accelValArray, &accelValMax, &accelMaxIndex, acceln);
  }
  
  // Filter values
  updateArray(Vel, filterVal, nFilterVal);
  filteredVel = filter(filterVal, nFilterVal);
  rpmVel = 30*filteredVel/PI;
  
  // Print if needed
  if (currentMicros > oldPrintMicros + 10000) {
    oldPrintMicros = currentMicros;  // Update oldMillis
    // Flush the buffer and print on screen
    Serial.flush();
    Serial.print("Time");
    Serial.print("\t");
    Serial.print(currentMicros/1000000.0, 3);
    Serial.print("\t");
    Serial.print("g");
    Serial.print("\t");
    Serial.println(accelVal);
    
//    Serial.flush();
//    Serial.print(currentMicros/1000000.0, 3);
//    Serial.print("\t");
//    Serial.print(potVal);
//    Serial.print("\t");
//    Serial.print(abs(rpmVel), 1);
//    Serial.print("\t");
//    Serial.print(u);
//    Serial.print("\t");
//    Serial.println(accelValMax);
  }
      
}




//---------------------------------------------------------------------------



float velController(float Vel, float intVel, float desVel) {
  float errorVel;
  float Kp = 10;
  float Ki = 1e-6;
  float u;
  
  errorVel = (desVel - Vel);
  intVel += errorVel*(currentMicros - oldMicros);
  u = Kp*errorVel + Ki*intVel; // P velocity control
  
  return u;
}



void encoderAchange() {
  if (digitalRead(encoderApin) == digitalRead(encoderBpin)){
    rawPos --;
  }
  else if (digitalRead(encoderApin) != digitalRead(encoderBpin)){
    rawPos ++;
  }
}

void encoderBchange() {
  if (digitalRead(encoderApin) == digitalRead(encoderBpin)){
    rawPos ++;
  }
  else if (digitalRead(encoderApin) != digitalRead(encoderBpin)){
    rawPos --;
  }
}


void updateArray(float newVal, float *array, int N) {
  for (int n = N-1; n > 0; n--) {
    array[n] = array[n-1];
  }
  array[0] = newVal;
}


void searchMax(float *array, float *maxVal, int *index, int N) {
  *maxVal = 0;
  for (int n = 0; n < acceln; n++) {
    if (array[n] > *maxVal) {
      *maxVal = array[n];
      *index = n;
    }
  }
}
  
float filter(float *filterArray, int N) {
  float cumSum = 0;
  for (int n = 0; n < N; n++) {
    cumSum += filterArray[n];
  }
  return (cumSum/N);
}



void runMotor(float u) {
  boolean forward = (u > 0.0);
  
  if (forward) {
    digitalWrite(M2DIR, LOW);
    if (u > 255.0) {analogWrite(M2PWM, 255);}
    else {analogWrite(M2PWM, (int)u);}
  }
  else {
    digitalWrite(M2DIR, HIGH);
    if (u < -255.0) {analogWrite(M2PWM, 255);}
    else {analogWrite(M2PWM, -(int)u);} 
  }
}
    
  



//---------------------------------------------------------------------------