Motor test

//Control pins
#define PWM_PIN 9
#define DIR_PIN 5

//Showing dir with Arduino led
#define LED_PIN 13

//Dead time insertion in microseconds
#define DTI_US 1000

//Change direction every N milliseconds
#define CHANGEDIR_MS 100

long lastDTIStart = 0;
long lastDirChange = 0;
boolean changeDir = false;
boolean motorDir = false;

void setup() {
  //---------------------------------------------- Set PWM frequency for D9 & D10 ------------------------------
  TCCR1B = TCCR1B & B11111000 | B00000010;    // set timer 1 divisor to     8 for PWM frequency of  3921.16 Hz

  timeDirChange();
  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  digitalWrite(LED_PIN, motorDir);
}

void loop() {
  if(changeDir && !checkDirChange())
  {
    //Sleep in small steps for smaller error
    delayMicroseconds(DTI_US / 100);
    return;
  }

  if(millis() - lastDirChange > CHANGEDIR_MS)
  {
    timeDirChange();
    lastDirChange = millis();
  }

}

void timeDirChange()
{
  //PWM to 0% duty cycle
  analogWrite(PWM_PIN, 0);
  lastDTIStart = micros();
  changeDir = true;
}

boolean checkDirChange()
{
  if(changeDir)
  {
    if(micros() - lastDTIStart > DTI_US)
    {
      changeDir = false;
      motorDir = !motorDir;
      digitalWrite(DIR_PIN, motorDir);
      digitalWrite(LED_PIN, motorDir);
      delayMicroseconds(100);
      //Enable PWM
      analogWrite(PWM_PIN, 128);

      return true;
    }

    return false;
  }
  return true;
}