Claymore test code for Teensy 3

/* Pin Map */
#define MOTOR_L_IN_1    16
#define MOTOR_L_IN_2    15
#define MOTOR_L_PWM     22 
#define MOTOR_STBY      14
#define MOTOR_R_IN_1    17
#define MOTOR_R_IN_2    18
#define MOTOR_R_PWM     23  
#define MOTOR_L_A       11
#define MOTOR_L_B       12
#define MOTOR_R_A       2
#define MOTOR_R_B       3
#define LED_STATUS      13
#define SENSOR_OPP_L    0
#define SENSOR_OPP_R    0
#define SENSOR_FLR_L    0
#define SENSOR_FLR_R    0
#define RUN             0


/* Global Variables */
volatile unsigned long leftCounter = 0x00000000UL;
volatile unsigned long rightCounter = 0x00000000UL;
volatile char oldLeft, oldRight, newLeft = 0, newRight = 0;
static char encoderQuadrature[16] = {
  0,-1,1,2,1,0,2,-1,-1,2,0,1,2,1,-1,0};

/* Global Functions */
void leftMotorA(void) {
  oldLeft = newLeft;
  newLeft = (digitalRead(MOTOR_L_A) * 2) + digitalRead(MOTOR_L_B);
  leftCounter += encoderQuadrature[(oldLeft * 4) + newLeft];  
}

void leftMotorB(void) {
  oldLeft = newLeft;
  newLeft = (digitalRead(MOTOR_L_A) * 2) + digitalRead(MOTOR_L_B);
  leftCounter += encoderQuadrature[(oldLeft * 4) + newLeft]; 
}

void rightMotorA(void) {
  oldRight = newRight;
  newRight = (digitalRead(MOTOR_R_A) * 2) + digitalRead(MOTOR_R_B);
  rightCounter += encoderQuadrature[(oldRight * 4) + newRight]; 
}
void rightMotorB(void) {
  oldRight = newRight;
  newRight = (digitalRead(MOTOR_R_A) * 2) + digitalRead(MOTOR_R_B);
  rightCounter += encoderQuadrature[(oldRight * 4) + newRight]; 
}

void getMotorSpeed(int &leftSpeed, int &rightSpeed) {
  static unsigned long elapsed = 0x00000000UL;
  unsigned int localLeftCounter = leftCounter;
  unsigned int localRightCounter = rightCounter;
  leftCounter = 0x00UL;
  rightCounter = 0x00UL;
  elapsed = micros() - elapsed;
  leftSpeed = (localLeftCounter * 60000000)/elapsed;
  rightSpeed = (localRightCounter * 60000000)/elapsed;
  return; 
}

void setMotorSpeed(int leftSpeed, int rightSpeed) {
  if(leftSpeed > 0)
  {
    digitalWrite(MOTOR_L_IN_1, LOW);
    digitalWrite(MOTOR_L_IN_2, HIGH);
  }
  else if(leftSpeed < 0)
  {
    digitalWrite(MOTOR_L_IN_1, HIGH);
    digitalWrite(MOTOR_L_IN_2, LOW);
  }
  else
  {
    digitalWrite(MOTOR_L_IN_1, HIGH);
    digitalWrite(MOTOR_L_IN_2, HIGH);
  }
  analogWrite(MOTOR_L_PWM, leftSpeed);

  if(rightSpeed > 0)
  {
    digitalWrite(MOTOR_R_IN_1, LOW);
    digitalWrite(MOTOR_R_IN_2, HIGH);
  }
  else if(rightSpeed < 0)
  {
    digitalWrite(MOTOR_R_IN_1, HIGH);
    digitalWrite(MOTOR_R_IN_2, LOW);
  }
  else
  {
    digitalWrite(MOTOR_R_IN_1, HIGH);
    digitalWrite(MOTOR_R_IN_2, HIGH);
  }
  analogWrite(MOTOR_R_PWM, rightSpeed);
  
  return;
}


void setup(void) {
  Serial.begin(115200);

  pinMode(MOTOR_L_IN_1, OUTPUT);
  pinMode(MOTOR_L_IN_2, OUTPUT);
  pinMode(MOTOR_L_PWM, OUTPUT);
  pinMode(MOTOR_R_IN_1, OUTPUT);
  pinMode(MOTOR_R_IN_2, OUTPUT);
  pinMode(MOTOR_L_PWM, OUTPUT);
  pinMode(MOTOR_STBY, OUTPUT);
  pinMode(LED_STATUS, OUTPUT);
  pinMode(MOTOR_L_A, INPUT);
  pinMode(MOTOR_L_B, INPUT);
  pinMode(MOTOR_R_A, INPUT);
  pinMode(MOTOR_R_B, INPUT);
  pinMode(SENSOR_OPP_L, INPUT);
  pinMode(SENSOR_OPP_R, INPUT);
  pinMode(SENSOR_FLR_L, INPUT);
  pinMode(SENSOR_FLR_R, INPUT);
  pinMode(RUN, INPUT_PULLUP);

  digitalWrite(MOTOR_L_IN_1, LOW);
  digitalWrite(MOTOR_L_IN_2, LOW);
  digitalWrite(MOTOR_L_PWM, LOW);
  digitalWrite(MOTOR_R_IN_1, LOW);
  digitalWrite(MOTOR_R_IN_2, LOW);
  digitalWrite(MOTOR_R_PWM, LOW);
  digitalWrite(MOTOR_STBY, LOW);
  digitalWrite(LED_STATUS, LOW);

  attachInterrupt(MOTOR_L_A, leftMotorA, CHANGE);
  attachInterrupt(MOTOR_L_B, leftMotorB, CHANGE);
  attachInterrupt(MOTOR_R_A, rightMotorA, CHANGE);
  attachInterrupt(MOTOR_R_B, rightMotorB, CHANGE);
  
  analogWriteFrequency(MOTOR_L_PWM, 18000);

}

void loop(void) {
  int left, right;

  digitalWrite(MOTOR_STBY, HIGH); // enable motors
  setMotorSpeed(200, 200);

  while(1)
  {
    getMotorSpeed(left, right);
    Serial.print("\f\rCounts/minute is: ");
    Serial.print(right, DEC);
    digitalWrite(LED_STATUS, !digitalRead(LED_STATUS));
    delay(2000);
  }
}