// for https://forum.arduino.cc/t/how-to-determine-or-improve-bldc-motor-at-lower-speeds-when-using-back-emf-for-timing-the-6-step-commutation/1125804/69
// Sim https://wokwi.com/projects/365008810056259585

/* Sensorless brushless DC motor control with Arduino UNO and IR2101 (Arduino DIY ESC).
   BLDC motor speed is controlled with a potentiometer connected to A0.
   This is a free software with NO WARRANTY.
   https://simple-circuit.com/
*/

#define PWM_MAX_DUTY      255
#define PWM_MIN_DUTY      50
#define PWM_START_DUTY    100

byte bldc_step = 0, motor_speed, pin_state;

void setup()
{
  Serial.begin(115200);
  DDRD  |= 0xE0;  // configure pins 5, 6 and 7 as outputs
  PORTD  = 0x00;
  DDRB  |= 0x0E;  // configure pins 9, 10 and 11 as outputs
  PORTB  = 0x31;
  // Timer1 module setting: set clock source to clkI/O / 1 (no prescaling)
  TCCR1A = 0;
  TCCR1B = 0x01;
  // Timer2 module setting: set clock source to clkI/O / 1 (no prescaling)
  TCCR2A = 0;
  TCCR2B = 0x01;
  // ADC module configuration
  ADMUX  = 0x60;   // configure ADC VCCREF + ADLAR module and select channel 0
  ADCSRA = 0x84;   // enable ADC module with 16 division factor (ADC clock = 1MHz)

  PCICR  = EIMSK = 0;  // disable all external interrupts

  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);

}

// pin change interrupt 2 (PCINT2) ISR
ISR (PCINT2_vect)
{
  if ( (PIND & PCMSK2) != pin_state )
    return;
  // BEMF debounce
  for (byte i = 0; i < 20; i++)
  {
    if (bldc_step & 1) {
      if (PIND & PCMSK2)     i -= 1;
    }
    else {
      if (!(PIND & PCMSK2))  i -= 1;
    }
  }

  bldc_move();
  bldc_step++;
  bldc_step %= 6;
}

// BLDC motor commutation function
void bldc_move()
{
  switch (bldc_step)
  {
    case 0:
      AH_BL();
      BEMF_C_FALLING();
      break;
    case 1:
      AH_CL();
      BEMF_B_RISING();
      break;
    case 2:
      BH_CL();
      BEMF_A_FALLING();
      break;
    case 3:
      BH_AL();
      BEMF_C_RISING();
      break;
    case 4:
      CH_AL();
      BEMF_B_FALLING();
      break;
    case 5:
      CH_BL();
      BEMF_A_RISING();
  }
}

bool spinupMode = true; // state variable for spinup/run control

void loop()
{
  if (spinupMode) {
    SET_PWM_DUTY(PWM_START_DUTY);  // setup starting PWM with duty cycle = PWM_START_DUTY
    static int i = 5000;
    motor_speed = PWM_START_DUTY;
    // motor start
    if (i > 100)
    {
      //Serial.print('s');
      delayMicroseconds(i);
      bldc_move();
      bldc_step++;
      bldc_step %= 6;
      i = i - 20;
    } else { // spin up complete, setup for running
      motor_speed = PWM_START_DUTY;
      PCICR  = 4;  // enable pin change interrupt for pins PCINT23..16 (Arduino 0 to 7)
      ADCSRA |= 1 << ADSC;    // start next conversion
      spinupMode = false;
    }
  } else { // run mode

    //    ADCSRA |= 1 << ADSC;    // start conversion
    if (! (ADCSRA & 0x40)) { // wait for conversion complete
      // Serial.print('a');
      motor_speed = ADCH;     // read ADC data (8 bits only)
      ADCSRA |= 1 << ADSC;    // start next conversion
      if (motor_speed < PWM_MIN_DUTY) {
        motor_speed = PWM_MIN_DUTY;
      }
      SET_PWM_DUTY(motor_speed);
    }
  }
  report();
  simTrigger();
}

void report() {
  int interval = 100;
  static unsigned long last = -interval;
  unsigned long now = millis();
  if (now - last < interval) return;
  last += interval;
  Serial.println(motor_speed);
}

void simTrigger() {
  static bool init = false;
  if (init == false) {
    // sim
    pinMode(A5, OUTPUT); // cycle change signal
    pinMode(A4, OUTPUT); // phase change signal
    init = true;
  }
  int interval = 5; // phase shift speed   << adjust this
  static unsigned long last = -interval;
  unsigned long now = millis();
  if (now - last < interval) return;
  last += interval;

  digitalWrite(A4, digitalRead(A5) == HIGH ? LOW : HIGH); // toggle for vis
  digitalWrite(A5, bldc_step == 0 ? HIGH : LOW); // 
  bldc_move();
  bldc_step++;
  bldc_step %= 6;
}


void BEMF_A_RISING()
{
  PCMSK2 = 0x04;    // enable Arduino pin 2 (PCINT18) interrupt, others are disabled
  pin_state = 0x04;
}
void BEMF_A_FALLING()
{
  PCMSK2 = 0x04;    // enable Arduino pin 2 (PCINT18) interrupt, others are disabled
  pin_state = 0;
}
void BEMF_B_RISING()
{
  PCMSK2 = 0x08;    // enable Arduino pin 3 (PCINT19) interrupt, others are disabled
  pin_state = 0x08;
}
void BEMF_B_FALLING()
{
  PCMSK2 = 0x08;    // enable Arduino pin 3 (PCINT19) interrupt, others are disabled
  pin_state = 0;
}
void BEMF_C_RISING()
{
  PCMSK2 = 0x10;    // enable Arduino pin 4 (PCINT20) interrupt, others are disabled
  pin_state = 0x10;
}
void BEMF_C_FALLING()
{
  PCMSK2 = 0x10;    // enable Arduino pin 4 (PCINT20) interrupt, others are disabled
  pin_state = 0;
}

void AH_BL()
{
  PORTD &= ~0xA0;
  PORTD |=  0x40;
  TCCR1A =  0;      // turn pin 11 (OC2A) PWM ON (pin 9 & pin 10 OFF)
  TCCR2A =  0x81;   //
}
void AH_CL()
{
  PORTD &= ~0xC0;
  PORTD |=  0x20;
  TCCR1A =  0;      // turn pin 11 (OC2A) PWM ON (pin 9 & pin 10 OFF)
  TCCR2A =  0x81;   //
}
void BH_CL()
{
  PORTD &= ~0xC0;
  PORTD |=  0x20;
  TCCR2A =  0;       // turn pin 10 (OC1B) PWM ON (pin 9 & pin 11 OFF)
  TCCR1A =  0x21;    //
}
void BH_AL()
{
  PORTD &= ~0x60;
  PORTD |=  0x80;
  TCCR2A =  0;      // turn pin 10 (OC1B) PWM ON (pin 9 & pin 11 OFF)
  TCCR1A =  0x21;   //
}
void CH_AL()
{
  PORTD &= ~0x60;
  PORTD |=  0x80;
  TCCR2A =  0;       // turn pin 9 (OC1A) PWM ON (pin 10 & pin 11 OFF)
  TCCR1A =  0x81;    //
}
void CH_BL()
{
  PORTD &= ~0xA0;
  PORTD |=  0x40;
  TCCR2A =  0;       // turn pin 9 (OC1A) PWM ON (pin 10 & pin 11 OFF)
  TCCR1A =  0x81;    //
}

void SET_PWM_DUTY(byte duty)
{
  OCR1A  = duty;  // set pin 9  PWM duty cycle
  OCR1B  = duty;  // set pin 10 PWM duty cycle
  OCR2A  = duty;  // set pin 11 PWM duty cycle
}