Motor spin test for my Segway: http://blog.tkjelectronics.dk/2014/07/full-size-diy-balancing-robot

MotorSpinTest.ino

#if 0 // Right
  #define PWM  9
  #define DIR  8
  
  #define PIN_REG COM1A1
  #define PWM_REG OCR1A
#else // Left
  #define PWM  10
  #define DIR  12
  
  #define PIN_REG COM1B1
  #define PWM_REG OCR1B
#endif

#define MAX_SPEED 100

#include <Usb.h>
#include <SPI.h>

/*#define leftF1 P5
#define leftF2 P6*/

#define rightF1 P3
#define rightF2 P4

const uint16_t PWM_FREQUENCY = 20000; // The motor driver can handle a PWM frequency up to 20kHz
const uint16_t PWMVALUE = F_CPU / PWM_FREQUENCY / 2; // The frequency is given by F_CPU/(2*N*ICR) - where N is the prescaler, prescaling is used so the frequency is given by F_CPU/(2*ICR) - ICR = F_CPU/PWM_FREQUENCY/2

boolean motor_direction;
boolean count_dir = true;
uint8_t count = 0;

void setup() {
  Serial.begin(115200);
  pinMode(PWM, OUTPUT);
  pinMode(DIR, OUTPUT);
  digitalWrite(DIR, motor_direction);

  /* Set PWM frequency to 20kHz - see the datasheet http://www.atmel.com/Images/Atmel-8272-8-bit-AVR-microcontroller-ATmega164A_PA-324A_PA-644A_PA-1284_P_datasheet.pdf page 129-139 */
  // Set up PWM, Phase and Frequency Correct on pin 18 (OC1A) & pin 17 (OC1B) with ICR1 as TOP using Timer1
  TCCR1B = (1 << WGM13) | (1 << CS10); // Set PWM Phase and Frequency Correct with ICR1 as TOP and no prescaling
  ICR1 = PWMVALUE; // ICR1 is the TOP value - this is set so the frequency is equal to 20kHz

  /* Enable PWM on pin 10 (OC1B) */
  // Clear OC1A/OC1B on compare match when up-counting
  // Set OC1A/OC1B on compare match when down-counting
  TCCR1A = 1 << PIN_REG;

  /*leftF1::SetDirRead();
  leftF2::SetDirRead();*/
  rightF1::SetDirRead();
  rightF2::SetDirRead();
}

void loop() {
#if 0
  if (leftF1::IsSet() || leftF2::IsSet() || rightF1::IsSet() || rightF2::IsSet()) {
    Serial.print(F("Diagnostic error: "));
    Serial.print(leftF1::IsSet());
    Serial.write('\t');
    Serial.print(leftF2::IsSet());
    Serial.write('\t');
    Serial.print(rightF1::IsSet());
    Serial.write('\t');
    Serial.println(rightF2::IsSet());
  }
#endif
#if 1
  if (count_dir && count <= MAX_SPEED) {
    if (count == MAX_SPEED)
      count_dir = false;
    else
      count++;
  }
  else if (!count_dir && count > 0)
    count--;
  else if (count == 0) {
    count_dir = true;
    motor_direction = !motor_direction;

    digitalWrite(DIR, motor_direction);
  }
#else
  count = 100;
#endif

  setPWM(count);
  Serial.println(count);
  /*Serial.write('\t');
  double voltage = analogRead(A0) * 5.0 / 1023.0 - 2.5;
  double current = voltage / 0.066;
  Serial.print(voltage, 4);
  Serial.write('\t');
  Serial.println(current, 4);*/

  delay(150);
}

void setPWM(double speedRaw) {
  PWM_REG = speedRaw * ((double)PWMVALUE) / 100.0; // Scale from 0-100 to 0-PWMVALUE;
}