Counts encoder ticks in order to enable motor characterisation.

Wheel_Tester.ino

/** Sketch to characterise motors.
 *  
 *  Needs the following libaries:
 *    https://github.com/PaulStoffregen/Encoder
 *    https://github.com/pololu/dual-vnh5019-motor-shield
 *    https://github.com/PaulStoffregen/MsTimer2
 */
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
#include <DualVNH5019MotorShield.h>
#include <MsTimer2.h>

/** Settings */
#define SETPOINT 20 // run at 20%, 40% and 60%
#define RATE 10 // ms for 100Hz

/** Encoder pinout */
#define ENC_A 3
#define ENC_B 5

Encoder motorEncoder(ENC_A, ENC_B); // sets up encoder, enables pullups
DualVNH5019MotorShield motorDriver;

void setup(void) {

  Serial.begin(250000);

  unsigned char motorSpeed = 4 * SETPOINT; // 400 is max speed of VNH5019
  
  Serial.print("Motor Tester,");
  Serial.print(SETPOINT, DEC);
  Serial.println("%\r\nTime (ms),Counts,Delta");

  motorEncoder.read(); // clear encoder counts

  while(millis() % 10 != 0); // wait till we're at 10ms
  MsTimer2::set(RATE, printEncoder);
  MsTimer2::start();

  // setup motor
  motorDriver.init();
  motorDriver.setSpeeds(motorSpeed, 0x00);
  
}

void loop(void) {
  // do nothing
}

// read and print time stamps & encoder counts
void printEncoder(void) {
  
  static long oldCount = 0x00L;
  long currentCount = motorEncoder.read();

  Serial.print(millis(), DEC); // time between samples
  Serial.print(",");
  Serial.print(currentCount, DEC); // current cumulative ticks
  Serial.print(",");
  Serial.print(currentCount - oldCount,DEC); // ticks since last sample
  Serial.print("\r\n");
  oldCount = currentCount; // save old ticks value

}

#if SETPOINT > 100 or SETPOINT < 0 // to catch any silly errors
  #error this will break the program!
#endif