Arduino 3-phase motor control

#define USE_LCD
#define REQUIRE_ENABLE_BUTTON


#ifdef USE_LCD
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

#define LCD_UPDATEINTERVAL_MS 50

#endif //USE_LCD

/*
//NOTE: Changing timer 0 (or was it some other...) will fuck up delay(), millis() etc., need to scale the values again
//---------------------------------------------- Set PWM frequency for D5 & D6 -------------------------------

//TCCR0B = TCCR0B & BRIDGE11111000 | B00000001;    // set timer 0 divisor to     1 for PWM frequency of 62500.00 Hz
//TCCR0B = TCCR0B & BRIDGE11111000 | B00000010;    // set timer 0 divisor to     8 for PWM frequency of  7812.50 Hz
  TCCR0B = TCCR0B & BRIDGE11111000 | B00000011;    // set timer 0 divisor to    64 for PWM frequency of   976.56 Hz (The DEFAULT)
//TCCR0B = TCCR0B & BRIDGE11111000 | B00000100;    // set timer 0 divisor to   256 for PWM frequency of   244.14 Hz
//TCCR0B = TCCR0B & BRIDGE11111000 | B00000101;    // set timer 0 divisor to  1024 for PWM frequency of    61.04 Hz


//---------------------------------------------- Set PWM frequency for D9 & D10 ------------------------------

//TCCR1B = TCCR1B & BRIDGE11111000 | B00000001;    // set timer 1 divisor to     1 for PWM frequency of 31372.55 Hz
//TCCR1B = TCCR1B & BRIDGE11111000 | B00000010;    // set timer 1 divisor to     8 for PWM frequency of  3921.16 Hz
  TCCR1B = TCCR1B & BRIDGE11111000 | B00000011;    // set timer 1 divisor to    64 for PWM frequency of   490.20 Hz (The DEFAULT)
//TCCR1B = TCCR1B & BRIDGE11111000 | B00000100;    // set timer 1 divisor to   256 for PWM frequency of   122.55 Hz
//TCCR1B = TCCR1B & BRIDGE11111000 | B00000101;    // set timer 1 divisor to  1024 for PWM frequency of    30.64 Hz

//---------------------------------------------- Set PWM frequency for D3 & D11 ------------------------------

//TCCR2B = TCCR2B & BRIDGE11111000 | B00000001;    // set timer 2 divisor to     1 for PWM frequency of 31372.55 Hz
//TCCR2B = TCCR2B & BRIDGE11111000 | B00000010;    // set timer 2 divisor to     8 for PWM frequency of  3921.16 Hz
//TCCR2B = TCCR2B & BRIDGE11111000 | B00000011;    // set timer 2 divisor to    32 for PWM frequency of   980.39 Hz
  TCCR2B = TCCR2B & BRIDGE11111000 | B00000100;    // set timer 2 divisor to    64 for PWM frequency of   490.20 Hz (The DEFAULT)
//TCCR2B = TCCR2B & BRIDGE11111000 | B00000101;    // set timer 2 divisor to   128 for PWM frequency of   245.10 Hz
//TCCR2B = TCCR2B & BRIDGE11111000 | B00000110;    // set timer 2 divisor to   256 for PWM frequency of   122.55 Hz
//TCCR2B = TCCR2B & BRIDGE11111000 | B00000111;    // set timer 2 divisor to  1024 for PWM frequency of    30.64 Hz

 */

#ifdef REQUIRE_ENABLE_BUTTON
//DEBUGGING: Enable switch must be held down for commutation to occur
#define ENABLE_PIN 12
#endif //REQUIRE_ENABLE_BUTTON

//PWM pins
#define BRIDGE1_HIGH 5
#define BRIDGE1_LOW 6
#define BRIDGE2_HIGH 9
#define BRIDGE2_LOW 10
#define BRIDGE3_HIGH 3
#define BRIDGE3_LOW 11

#define HALL1_PIN 2
#define HALL2_PIN 4
#define HALL3_PIN 7

#define H1 1
#define H2 2
#define H3 4
#define H1H2 3
#define H1H3 5
#define H2H3 6

#define DTI_MICROSECONDS 100

bool hall1 = false;
bool hall2 = false;
bool hall3 = false;

//"Old" values for detecting changes, different values than the h1-h3 above so first call will cause commutation
bool oldHall1 = true;
bool oldHall2 = true;
bool oldHall3 = true;

//Controlling simple dead time insertion
unsigned long dtiStart = 0;

bool phaseSwitch = false;

#ifdef USE_LCD
unsigned long lcdUpdate = 0;
#endif //USE_LCD

void setup()
{
#ifdef REQUIRE_ENABLE_BUTTON
  //NOTE: Pullup-setup, the button needs to connect to GND
  pinMode(ENABLE_PIN, INPUT_PULLUP);
#endif

  pinMode(HALL1_PIN, INPUT);
  pinMode(HALL2_PIN, INPUT);
  pinMode(HALL3_PIN, INPUT);

  pinMode(BRIDGE1_HIGH, OUTPUT);
  pinMode(BRIDGE1_LOW, OUTPUT);
  pinMode(BRIDGE2_HIGH, OUTPUT);
  pinMode(BRIDGE2_LOW, OUTPUT);
  pinMode(BRIDGE3_HIGH, OUTPUT);
  pinMode(BRIDGE3_LOW, OUTPUT);

  killPWM();

#ifdef USE_LCD
  lcd.begin(16,2);
  lcd.home ();
  lcd.print("Initializing...");
#endif //USE_LCD


}

void loop()
{

#ifdef REQUIRE_ENABLE_BUTTON
  if(digitalRead(ENABLE_PIN))
  {
    killPWM();
    #ifdef USE_LCD
      lcd.home ();
      lcd.print("Not enabled    ");
      delay(100);
    #endif
    return;
  }
#endif

  if(readHalls())
  {
    killPWM();
  }


  commutate();

#ifdef USE_LCD
   if((millis() - lcdUpdate) > LCD_UPDATEINTERVAL_MS)
   {
    lcd.home ();
    lcd.print("H1:");
    lcd.print(hall1 ? "1" : "0");
    lcd.print(" H2:");
    lcd.print(hall2 ? "1" : "0");
    lcd.print(" H3:");
    lcd.print(hall3 ? "1" : "0");
    lcdUpdate = millis();
   }

#endif

}

/**
 * Reads the inputs from hall-pins and returns whether changes were detected
 * @return True, if a hall-sensor state has changed, otherwise false
 */
bool readHalls()
{
  oldHall1 = hall1;
  oldHall2 = hall2;
  oldHall3 = hall3;
  hall1 = digitalRead(HALL1_PIN);
  hall2 = digitalRead(HALL2_PIN);
  hall3 = digitalRead(HALL3_PIN);

  if(oldHall1 != hall1 || oldHall2 != hall2 || oldHall3 != hall3)
  {
    return true;
  }

  return false;
}

void killPWM()
{
  analogWrite(BRIDGE1_HIGH, 0);
  analogWrite(BRIDGE1_LOW, 0);
  analogWrite(BRIDGE2_HIGH, 0);
  analogWrite(BRIDGE2_LOW, 0);
  analogWrite(BRIDGE3_HIGH, 0);
  analogWrite(BRIDGE3_LOW, 0);

  //Prepare to switch phase PWMs after DTI
  dtiStart = micros();
  phaseSwitch = true;
}

void commutate()
{
  //TODO: Constant duty cycle for now
  int dutyCycle = 255;
  if(phaseSwitch && (micros() - dtiStart) > DTI_MICROSECONDS)
  {
    //Read latest states
    readHalls();

    unsigned int hallState = (hall1 << 0) | (hall2 << 1) | (hall3 << 2);

    switch(hallState)
    {
      case H1:
        analogWrite(BRIDGE1_HIGH, dutyCycle);
        analogWrite(BRIDGE3_LOW, dutyCycle);
        break;
      case H2:
        analogWrite(BRIDGE2_HIGH, dutyCycle);
        analogWrite(BRIDGE1_LOW, dutyCycle);
        break;
      case H3:
        analogWrite(BRIDGE3_HIGH, dutyCycle);
        analogWrite(BRIDGE2_LOW, dutyCycle);
        break;
      case H1H2:
        analogWrite(BRIDGE2_HIGH, dutyCycle);
        analogWrite(BRIDGE3_LOW, dutyCycle);
        break;
      case H1H3:
        analogWrite(BRIDGE1_HIGH, dutyCycle);
        analogWrite(BRIDGE2_LOW, dutyCycle);
        break;
      case H2H3:
        analogWrite(BRIDGE3_HIGH, dutyCycle);
        analogWrite(BRIDGE1_LOW, dutyCycle);
        break;
      default:
        #ifdef USE_LCD
        lcd.home();
        lcd.print("Unknown hallstate:");
        lcd.setCursor(0, 1);
        lcd.print(hallState);
        delay(1000);
        #endif
        break;
    }

    #ifdef USE_LCD
    lcd.setCursor(0, 1);
    lcd.print(hallState);
    #endif

    phaseSwitch = false;
  }
}