Published February 16, 2024 © MIT

Using BLDC motor in low speed

Control a BLDC motor in low speed with minimum cogging effects.

IntermediateFull instructions provided2 hours787

Things used in this project

Hardware components

Arduino UNO

Simple FOC Shield V2.04

AS5600 Magnetic Sensor Board with magnet

Software apps and online services

Arduino IDE

coolTerm - Serial Terminal for Windows

Story

This project is assumed that you have already know how to drive a BLDC motor, by using Arduino UNO (mini) with Simple FOC Shielded interface board for BLDC motor.

Calibration of a BLDC motor takes time, may be more than 30mins. Please prepare a cup of coffee with you.

In this project, we are trying to drive a BLDC motor in very low speed. This means we need to have a very low PWM values update rate. However, the PWM values update rate is constant, the rotating speed of motor's rotor may not being what you expected. It will not spin with constant speed.

We are facing a cogging torque of electrical motors. It is a due to the interaction between the permanent magnets of the rotor and the stator slots of a permanent magnet machine. This torque is position dependent and its periodicity per revolution depends on the number of magnetic poles and the number of teeth on the stator.

Cogging torque is an undesirable component for the operation of such a motor. It is especially prominent at lower speeds, with the symptom of jerkiness. Cogging torque results in torque as well as speed ripple; however, at high speed the motor moment of inertia filters out the effect of cogging torque. (Check the right un-calibrated motor in the given video)

In order to minimize cogging torque, we will use a variable PWM values update rate, to drive BLDC motor. With a lookup table maps to rotor's mechanical rotation angle, we will have different time delay to update the PWM values to the motor. A calibration program is needed to find out the values of the lookup table.

Program:

For the calibration program, it fills the lookup table with constant value first. The main loop() of the program will get readings from AS5600 sensor board, in every 20ms. The goal of calibration is to obtain one count of delta change of angle from the sensor within 20ms. If it is more than 1 count, it increases the corresponding value in the lookup table. If the count is zero or negative, it decreases the value in the lookup table, mapped with last angle read from the sensor.

The PWM values update at a rate given in the lookup table, while the lookup table is update on the fly. When no. of iteration is reached, the calibration process will terminate and print the lookup table. You may copy and paste these values into you own program.

In the Demo program, it uses the two lookup table as PWM update rate to drive BLDC motor. The rotor spins from slow to fast, then slow down and rotates again at another direction.

Construction:

1. Select the options on the Sample FOC Shielded board:

Simple FOC Shielded board

2. Mount the Simple FOC Shielded board on the Arduino UNO board:

Connect +5V power lines and I2C interface from the AS5600 sensor board to Simple FOC Shielded board. Connect the main power supply to VCC and GND. For the BLDC motor, the main power supply is +12V dc.

3. The BLDC motor:

4. The magnet mount on the rotor of this motor:

5. and the AS5600 sensor board mounted on the back of the BLDC motor

Video:

This video shows difference in between un-calibrated and calibrated motor is driving at low speed.

Usage:

Use coolTerm.exe program in Windows, please configure the Baud rate to 115200. After reset, a simple main menu will show.

Press 'R' to calibrate the clockwise direction

Press 'L' to calibrate the anti-clockwise direction

Press 'X' to printout the lookup table

While during calibration, and '*' tells you one iteration is done. You may press 'S' to stop it. The lookup table than show up.

Note:

The no. of iteration is defined as "ITERATION" in the calibration program. You may change this value. However, greatly increase no. of iteration may not have a good result when most of values in the lookup table goes to its maximum or minimum value.

For better result, you may need to adjust the value of "MOTOR_DRIVERPWR", on both programs. This controls the driving current to the BLDC motor. What I suggest is about 30% to 40% of maximum power needed.

==================================================================

Hoping this project may help you.....

Please feel free to send me advice or questions.

//
// Program Name : uGM100-Cal.ino
// Version      : 1.00
// Release Date : 20 Feb 2024
// Arduino IDE  : V2.30
// ==========================================================================
// uGM100_Demo.ino code is placed under the MIT license
// Copyright(c) 2024 Kevin LO (LO MAN FAI)

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
// ==========================================================================

// 
// This program generates a 512 bytes lookup table, in which can minimize the cogging effects from drive
// BLDC motor in lower speed. We need two tables for each of rotating directions
// 
// With the magnetic mounted on the rotor, the magnetic sensor detects mechanical angle of the rotor. The
// calibration program take readings from sensor on every 20ms. It will adjust the default value of lookup
// table, in which changes the time delay for next PWM values update for the motor. This changes speed of 
// motor's rotor. The goal is to have one 1 count readings from sensor, in every 20ms. If the rotor is moving
// too fast, it increase the corresponding value in the table. If it goes too slow, that value will decrease. 
//
// For the lookup table, it is a map of rotor's mechanical angle. In which is the resoution of magnetic 
// sensor's output. As we have limited memeory resources, we scale down to 512 bytes 
// 
// The default calibration iteration time is defined as ITERATION. When no. of iteration time matched this 
// value, the calibration will stop and print out the lookup table for current rotor direction.
//
// Pleae be noted that: 
//   - The electrical condition for BLDC applicaton and calibration should be same. i.e., same voltage
//     supply, and driving power. 
//   - We assumed that you already know how to drive a BLDC motor. In this program, we are using a sinewave PWM, with
//     three phases are 120 Degree apart.
//
//
// Hardware Specification:
//   - BLDC motor used have 7-pole pairs, and rated dc voltage is 12V.
//     (You may also use other types of BLDC motor)
//   - Magnetic Angular Sensor : AS5600 with I2C Interface
//   - Arduino UNO mini board
//   - Simple FOC Shield V2.04 
//
//
// Reference: 
//   -A common issue, especially with sensorless brushless DC motors
//    https://zikodrive.com/ufaqs/what-is-brushless-dc-motor-cogging-and-how-do-i-get-rid-of-it/
//
//   -Anti-Cogging Algorithm Brings Out The Best In Your Hobby Brushless Motors
//    https://hackaday.com/2016/02/23/anti-cogging-algorithm-brings-out-the-best-in-your-hobby-brushless-motors/
//


#include <Wire.h>
#include <Arduino.h>
#include <stdio.h>

#define ITERATION            49                               // No. of iteration that you what in calibration mode, for each
                                                              //   rotate directions. A larger value may not be good choice.
                                                              //   The values of lookup table may goes to either maximum or 
                                                              //   minimum.

// 
// The PWM update rates for the motor depends on  
// TIMER2 in CTC mode (Compare Match Counter).
// The compare value is from 1 to 255. The actual
// time delay from 64us to 16.32ms.
//
#define PWM_UPDATE_DELAY     180                              // Initial value for lookup table, 8.192ms
#define PWM_UPDATE_DELAY_MIN 30                               // Min. value for lookup table, 1ms
#define PWM_UPDATE_DELAY_MAX 255                              // Max. value for lookup table, 16ms

#define SAMPLE_INTERVAL      600                              // Time in between take readings from megnatic sensor, 
                                                              //   By TIMER1 Overflow interrupt, the time is about
                                                              //   16us x 620 x 2 = 10ms


#define AS5600_CPR           4096                             // Magnetic Angular Sensor Resolution
#define ANGLE_RES            (AS5600_CPR/8)                   // Scale down for BLDC Motor, we need only 512 steps
#define BLDC_ANGLE_RES       (TWO_PI / (float)ANGLE_RES)      // Motor shaft min. rotation angle
                                                              //   it affects the array size in byte lookup tables

#define PHASE_120            ((float)TWO_PI / 3.0f)           // Angle difference for three phases BLDC motor

#define FULL_PWM             255                              // PWM max. value in 8-bit PWM mode                       

#define CLOCKWISE            0                                // Motor shaft turns clockwise
#define ANTICLOCKWISE        1                                // Motor shaft turns anticlockwise


#define MOTOR_DRIVEPWR       23                               // BLDC motor driving power, somewhere around 240mA for 
                                                              //   calibration, use the same voltage supply for your
                                                              //   application.

typedef enum {                                                // main loop() program states
  idle,                                                       //   do nothing
  mainMenu,                                                   //   show the main menu on screen
  calMotor,                                                   //   prepare to calibrate the motor
  calMotorProcess,                                            //   motor is under calibrating
  PrintOutTable,                                              //   print out lookup table
} menuItems;

volatile int rotorAngle;                                      // Current rotorAngle reading from AS5600
volatile int oldrotorAngle;                                   // Last rotorAngle reading
volatile int rotorAngleDiff;                                  // Difference in between current angle and last reading

volatile float pwmAngle;                                      // Sinewave angle for PWM

volatile unsigned int timeTick;                               // Time sync. for main loop() to get readings from magnetic sensor

volatile boolean tickRun;                                     // 
volatile boolean motorDIR;                                    // Current motor spinning direction
volatile unsigned char motorPWR;                              // BLDC motor driving power
menuItems menuMode = mainMenu;    

volatile unsigned int calLoopCount;                           // No. of turns being rotated (motor shaft)


//
// Strings of menu items
//
const unsigned char menu1[] = {"\n\n[BLDC motor calibrator V1.00]\n"};
const unsigned char menu2[] = {"Press (L) AntiClock Calibration\n"};
const unsigned char menu3[] = {"Press (R) Clockwise Calibration\n"};
const unsigned char menu4[] = {"Press (X) Print out Lookup Table\n"};
const unsigned char menu5[] = {"Calibration starts ......\n"};
const unsigned char menu6[] = {"Press (S) to stop\n"};

const unsigned char Hex2Asc[] = {"0123456789ABCDEF"};         // Used for decimal to hexadecimal convert

unsigned char pwmDelay[ANGLE_RES];                            // Array of lookup table, 512 steps per turn of rotor

//
// SerialEvent occurs whenever a new data comes in the
// hardware serial RX.  This routine is run between each
// time loop() runs, so using delay inside loop can delay
// response.  Multiple bytes of data may be available.
//
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();

    if (menuMode == calMotorProcess) {                        // Stop current calibration process when 's' is pressed
      if (inChar == 's' || inChar =='S')
        calLoopCount = ITERATION;
    }
    else if (menuMode == idle) {
      if (inChar == 'l' || inChar =='L') {                    // Start anticlockwise calibration when 'l' is pressed
        motorDIR = ANTICLOCKWISE;
        menuMode = calMotor;
      }
      if (inChar == 'r' || inChar =='R') {                    // Start clockwise calibration when 'r' is pressed
        motorDIR = CLOCKWISE;
        menuMode = calMotor;
      }
      if (inChar == 'x' || inChar =='X') {                    // Print out lookup table when 'x' is pressed
        menuMode = PrintOutTable;
      }
    }
  }
}

void setup() {
  // put your setup code here, to run once:
  unsigned int i;

  Wire.begin();                               // Start I2c communication
  Serial.begin(115200);                       // Serial communication in 115200 baud

  pinMode(9, OUTPUT);                         // PWM A, OC1A (8bit mode)
  pinMode(5, OUTPUT);                         // PWM B, OC0A (8bit mode)
  pinMode(6, OUTPUT);                         // PWM C, OC0B (8bit mode)

  pinMode(8, OUTPUT);                         // PWM output ENABLE for Sample FOC Shield v2.04
  pinMode(7, OUTPUT);                         // Debug OUTPUT

  rotorAngle = 0;
  oldrotorAngle = 0;
  
  pwmAngle = 0.0f;

  motorPWR = MOTOR_DRIVEPWR;
  motorDIR = CLOCKWISE;

  timeTick = 0;
  tickRun = false;

  for (i=0; i<ANGLE_RES; i++)
                                                pwmDelay[i] = PWM_UPDATE_DELAY;

  digitalWrite(8,HIGH);                       // Enable PWM output to motor
  cli();                                      // Stop interrupts

  //
  // Config TIMER1
  //
  TCCR1A = (TCCR1A & B00111100) | B10000010;   // Phase and frequency correct, Non-inverting mode, TOP defined by ICR1
  TCCR1B = (TCCR1B & B11100000) | B00010000;   // No prescale, timer stopped. PWM in phase correct mode for OC1A
  TCNT1 = 0;
  ICR1 = 255;                                  // Top = 0xff;
  TIMSK1 |= (1<<TOIE1);                        // Enable timer overflow interrupt, in which used to in sync. with main loop
                                               //   to take reading from magnetic sensor in given time

  //
  // Config TIMER0
  //
  TCCR0A = B10100001;                           // Clear OC0A/OC0B on compare match, PWM in phase correct mode
  TCCR0B = B00000000;                           // for OC0A and OC0B. Timer stopped
  TCNT0 = 0;
  
  //
  // Config TIMER2
  //
  TCCR2A = B00000010;                           // Clear Timer on Compare Match (CTC)
  TCCR2B = B00000000;                           // Stop timer
  TCNT2 = 0;
  OCR2A = 180;                                  // 10ms Timer interrupt
  TIMSK2 |= (1<<OCIE2A);                        // Enable timer compare interrupt, controls the time delay for update PWM values

  // Start Timers
  TCCR1B |= B00000001;                          // Prescaler = 1, timer starts
  TCCR0B |= B00000001;                          // Prescaler = 1, timer starts
  TCCR2B |= B00000111;                          // Prescaler = 1024, timer starts

  sei();                                        // Enable ALL interrupts
}

//
// TIMER1 Interrupt Routine
//   Use a flag to let main loop() get readings from magnetic sensor and
//   update delta angle difference
//
ISR(TIMER1_OVF_vect)
{
  timeTick ++;                                  // Update interupt times
  if (timeTick >= SAMPLE_INTERVAL) {            // Check to see if it's time to update flag
    timeTick = 0;                               //   reset counter
    tickRun = true;                             //   set flag to true
  }
}

//
// TIMER2 Interrupt Routine
// - update new PWM values in according to lookup table with given mechanical shaft angle
// - update phaser angle of sinewave, that is rotor speed control
//
ISR(TIMER2_COMPA_vect) {
  int u,v,w;
  float k;

  //
  //  Check direction needed, use corresponding lookup table and update new delay value
  //
  OCR2A = pwmDelay[rotorAngle]; 

  //
  // Calculate the PWM value from sinewave in according to the phaseer angle with 
  //   given motor power required
  //
  u = (int)(sin(pwmAngle) * (float)motorPWR) + ((float)FULL_PWM / 2.0f);
  v = (int)(sin(pwmAngle+PHASE_120) * (float)motorPWR) + ((float)FULL_PWM / 2.0f);
  w = (int)(sin(pwmAngle-PHASE_120) * (float)motorPWR) + ((float)FULL_PWM / 2.0f);

  OCR0A = w;
  OCR0B = u;
  OCR1A = v;

  //
  // Wrap around the phaser angle in bewteen 0 and 2PI
  //
  if (motorDIR == CLOCKWISE) {
    pwmAngle += BLDC_ANGLE_RES;
    if (pwmAngle >= (float)TWO_PI)
      pwmAngle = 0.0f;
  }
  else {
    pwmAngle -= BLDC_ANGLE_RES;
    if (pwmAngle < 0.0f)
      pwmAngle = (float)TWO_PI-BLDC_ANGLE_RES;
  }
}

//
// Print out the content of lookup table in hexadecimal format
//
void PrintTable()
{
  Serial.write('\n');

  unsigned int i = 0;
  for (unsigned int j=0; j<ANGLE_RES; j++) {
    Serial.write('0');
    Serial.write('x');
    Serial.write(Hex2Asc[((pwmDelay[j]>>4)&0x0f)]);
    Serial.write(Hex2Asc[(pwmDelay[j]&0x0f)]);
    Serial.write(',');

    i++;
    if (i == 16) {
      i = 0;
      Serial.write('\n');
    }
  }
  Serial.write('\n');
}

//
// Calibration, execution at 20ms once
//
void Calibration()
{
  if (calLoopCount < ITERATION) {               // check for iteration times          
    if (rotorAngleDiff > (ANGLE_RES-2)) {       // handle the roll over condition
      rotorAngleDiff -= (ANGLE_RES);            // from 511 -> 0
      calLoopCount ++;
      Serial.write('*');
    }
  
    if (rotorAngleDiff < -(ANGLE_RES-2)) {      // handle the roll over condition
      rotorAngleDiff += (ANGLE_RES);            // from 0 => 511
      calLoopCount ++;    
      Serial.write('*');
    }


    //
    // If delta angle changed more than or equal to 2 counts,
    // the rotor is moving too fast, increase time delay at last angle
    //
    if (rotorAngleDiff >= 2) {               
      if (pwmDelay[oldrotorAngle] < PWM_UPDATE_DELAY_MAX)
        pwmDelay[oldrotorAngle] += 1;
    }
  
    //
    // if delta angle is negative or zero counts,
    // the rotor is moving too slow, decrase the time delay at last angle
    //
    if (rotorAngleDiff <= 0) {
      if (pwmDelay[oldrotorAngle] > PWM_UPDATE_DELAY_MIN)
        pwmDelay[oldrotorAngle] -= 1;
    }
  }
}

void loop() {
  // put your main code here, to run repeatedly:
  unsigned char da[2];

  while (tickRun == false);
  tickRun = false;

  Wire.beginTransmission(0x36);                 // Connect to the sensor
  Wire.write(0x0C);                             // figure 21 - register map: Raw angle (11:8)
  Wire.endTransmission();                       // end transmission
  Wire.requestFrom(0x36, 2);                    // request from the sensor

  int i = 0;  
  while(Wire.available())                       // Read two bytes raw angle from sensor
    da[i++] = Wire.read();                      //

  //
  // Convert raw angle to mechanical angle, and scale down
  //
  rotorAngle = (unsigned int)da[0]<<8 | (unsigned int)da[1];
  rotorAngle = rotorAngle / (AS5600_CPR / ANGLE_RES);
  rotorAngle = rotorAngle & (ANGLE_RES-1);  

  //
  // Calculate the angle difference in between current and last angle
  //
  rotorAngleDiff = rotorAngle - oldrotorAngle;

  switch (menuMode) {
    case idle:
      break;
    case mainMenu:
      Serial.write(menu1, sizeof(menu1));
      Serial.write(menu2, sizeof(menu2));
      Serial.write(menu3, sizeof(menu3));
      Serial.write(menu4, sizeof(menu4));
      menuMode = idle;
      break;
    case calMotor:
      Serial.write(menu5, sizeof(menu5));
      Serial.write(menu6, sizeof(menu6));
      for (i=0; i<ANGLE_RES; i++)
         pwmDelay[i] = PWM_UPDATE_DELAY;
      calLoopCount = 0;
      menuMode = calMotorProcess;
      break;      
    case calMotorProcess:
      if (calLoopCount < ITERATION) 
        Calibration();
      else {
        PrintTable();
        menuMode = mainMenu;
      }
      break;
    case PrintOutTable:
      PrintTable();
      menuMode = mainMenu;
      break;  
    otherwise:
      menuMode = idle;
      break;        
  }

  oldrotorAngle = rotorAngle;
  digitalWrite(7, !digitalRead(7));
}

//
// Program Name : uGM100_Demo.ino
// Version      : 1.00
// Release Date : 20 Feb 2024
// Arduino IDE  : V2.30
// ==========================================================================
// uGM100_Demo.ino code is placed under the MIT license
// Copyright(c) 2024 Kevin LO (LO MAN FAI)

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
// ==========================================================================

//
// This programm shows you how to drive a BLDC motor in ultra-low spinning speed. It drives a BLDC motor rotates
// clockwise direction with speed increasing, then slow down to stop. After that, BLDC motor rotates in anti-clocksise
// direction.
//
// The program using a table lookup method to minimize the cogging effect, in which is noticable when BLDC is in 
// low speed.The table stores the time delay for next PWM values update, in according to the rotor's mechanical position.
// We need two tables for each of directions. These tables are generated by "uGM100-Cal.ino", in which is a 
// calibration program for the BLDC motor that is using. 
//
// Pleae be noted that: 
//   - The electrical condition for BLDC applicaton and calibration should be same. i.e., same voltage
//     supply, and driving power. 
//   - We assumed that you already know how to drive a BLDC motor. In this program, we are using a sinewave PWM, with
//     three phases are 120 Degree apart.
//
//
// Hardware Specification:
//   - BLDC motor used have 7-poles, and rated dc voltage is 12V. (You may also use other types of BLDC motor)
//   - Magnetic Angular Sensor : AS5600 with I2C Interface
//   - Arduino UNO mini board
//   - Simple FOC Shield V2.04 
//
//
// Reference: 
//   -A common issue, especially with sensorless brushless DC motors
//    https://zikodrive.com/ufaqs/what-is-brushless-dc-motor-cogging-and-how-do-i-get-rid-of-it/
//
//   -Anti-Cogging Algorithm Brings Out The Best In Your Hobby Brushless Motors
//    https://hackaday.com/2016/02/23/anti-cogging-algorithm-brings-out-the-best-in-your-hobby-brushless-motors/
//

#include <Wire.h>
#include <Arduino.h>
#include <stdio.h>

#define AS5600_CPR           4096                             // Magnetic Angular Sensor Resolution
#define ANGLE_RES            (AS5600_CPR/8)                   // Scale down for BLDC Motor, we need only 512 steps
                                                              //   it affects the array size in bytes for lookup tables

#define PHASE_120            ((float)TWO_PI / 3.0f)           // Angle difference for three phases of BLDC motor

#define FULL_PWM             255                              // PWM max. value in 8-bit PWM mode                       

#define CLOCKWISE            0                                // Motor shaft turns clockwise
#define ANTICLOCKWISE        1                                // Motor shaft turns anticlockwise


#define SAMPLE_INTERVAL      600                              // Time in between take readings from megnatic sensor, 
                                                              //   By TIMER1 Overflow interrupt, the time is about
                                                              //   16us x 620 = 10ms

#define MOTOR_DRIVEPWR       23                               // BLDC motor driving power, somewhere around 240mA for 
                                                              //   calibration, use the same voltage supply for your
                                                              //   application.


#define DELTACHANGE_MIN      ((float)TWO_PI/5760.0f)          // To control the motor shaft rotating speed, min. speed
#define DELTACHANGE_MAX      ((float)TWO_PI/288.0f)           // To control the motor shaft rotating speed, max. speed
#define DELTACHANGE          0.00004f                         // delta change in angle of sinewave PWM, per 10ms


volatile int rotorAngle;                                      // Current rotorAngle reading from AS5600
volatile int oldrotorAngle;                                   // Last rotorAngle reading

volatile float pwmAngle;                                      // Sinewave angle for PWM
volatile float deltapwmAngle;                                 // Delta angle for sinewave 
volatile float delta;                                         // Rotor speed control in main loop()

volatile unsigned int timeTick;                               // Time sync. for main loop() to get readings from magnetic sensor

volatile boolean tickRun;                                     // 
volatile boolean motorDIR;                                    // Current motor spinning direction
volatile unsigned char motorPWR;                              // BLDC motor driving power


//
// Lookup table for pwm update delay in according to mechanical shaft angle
// Please just place the output from "uGM100-Cal.ino here (Clockwise)
//
const unsigned char rotorClockwise[ANGLE_RES] =
{
  0x55,0x82,0x70,0x57,0x42,0x52,0x2C,0x59,0x8C,0x9A,0x7D,0x7D,0x90,0xA0,0x99,0x82,
  0x68,0x7C,0x49,0x29,0x5E,0x2D,0x4B,0x50,0x52,0x50,0x63,0x5D,0x4D,0x8F,0x63,0x77,
  0x60,0x92,0x6A,0x82,0x5B,0x68,0x66,0x5A,0x27,0x41,0x26,0x7E,0x7D,0xAD,0x8C,0x8A,
  0x88,0x8D,0x88,0x84,0x7A,0x62,0x25,0x3C,0x30,0x63,0x70,0x8C,0x78,0x65,0x93,0x77,
  0x68,0x66,0x21,0x64,0x34,0x96,0x8A,0x77,0x7F,0x7A,0x82,0x7E,0x81,0x29,0x68,0x1E,
  0x69,0x6B,0x81,0x6B,0x8B,0xA3,0xA2,0x7B,0x88,0x6E,0x39,0x28,0x4C,0x41,0x58,0x6F,
  0x58,0x71,0x6B,0x71,0xA1,0x9A,0x57,0x81,0x68,0x64,0x36,0x6D,0x55,0x60,0x51,0x6F,
  0x4B,0x3C,0x52,0x4F,0x52,0x89,0x8E,0x9D,0x80,0x72,0x76,0x8A,0x52,0x73,0x28,0x37,
  0x30,0x52,0x64,0x98,0x56,0x7E,0x8E,0x5D,0x73,0x6A,0x40,0x4F,0x7B,0x56,0x98,0xA2,
  0x90,0x6D,0x6B,0x89,0x5A,0x6F,0x23,0x25,0x2E,0x6C,0x90,0xA9,0x8D,0x97,0xBB,0x9C,
  0x8C,0x79,0x5F,0x3C,0x22,0x29,0x41,0x71,0x51,0x6C,0x72,0x5E,0x9E,0x58,0x5A,0x74,
  0x57,0x4F,0x91,0x6A,0x8C,0x6C,0x66,0x73,0x67,0x9D,0x5B,0x40,0x5D,0x62,0x3D,0x7C,
  0x73,0x51,0x83,0x64,0x72,0x7D,0x75,0x3C,0x3D,0x52,0x39,0x66,0x63,0x6E,0x47,0x7B,
  0x4A,0x88,0x44,0x37,0x72,0x49,0x9A,0x9F,0x95,0x7D,0x60,0x89,0x55,0x85,0x42,0x4D,
  0x2A,0x39,0x80,0x9A,0xAA,0xB9,0x91,0x8E,0x87,0x86,0x93,0x49,0x44,0x1E,0x29,0x4B,
  0x64,0x82,0x71,0x89,0x6E,0x83,0x82,0x84,0x39,0x50,0x2F,0x73,0x73,0x77,0x6A,0x85,
  0x56,0x7E,0x7C,0x39,0x47,0x49,0x42,0x87,0x7B,0x79,0x85,0x83,0x85,0x8C,0x84,0x89,
  0x6C,0x7B,0x32,0x47,0x33,0x3E,0x5E,0x42,0x71,0x48,0x8C,0x48,0x5E,0x66,0xA9,0x7E,
  0x5E,0x8A,0x71,0x5A,0x70,0x5B,0x5A,0x43,0x49,0x29,0x39,0x6E,0x8A,0x98,0xB4,0x8B,
  0x8A,0x88,0x92,0x7F,0x83,0x5F,0x23,0x41,0x32,0x60,0x69,0x93,0x8D,0x4D,0x8F,0x71,
  0x6F,0x4F,0x3E,0x33,0x3E,0x91,0x8C,0x94,0x96,0x79,0x6B,0x8C,0x6E,0x53,0x2C,0x28,
  0x48,0x72,0x77,0x82,0x84,0x89,0x9B,0x7B,0x84,0x7E,0x3C,0x34,0x42,0x36,0x65,0x7B,
  0x49,0x7F,0x66,0x7A,0x97,0x9E,0x7F,0x72,0x46,0x8C,0x5B,0x45,0x60,0x50,0x52,0x60,
  0x6B,0x33,0x77,0x36,0x41,0x8E,0xB3,0x7E,0x73,0x86,0x81,0x85,0x64,0x73,0x65,0x23,
  0x40,0x2D,0x58,0x81,0x6C,0x71,0x76,0x75,0x5B,0x9B,0x49,0x5A,0x57,0x52,0x65,0xB4,
  0x7E,0x96,0x73,0x7D,0x7A,0x67,0x60,0x21,0x28,0x33,0x6E,0x87,0xC2,0x9B,0x89,0xA6,
  0xAF,0x93,0x5E,0x71,0x29,0x32,0x20,0x4D,0x67,0x5C,0x77,0x6D,0x68,0x91,0x4D,0x64,
  0x59,0x86,0x4D,0x8D,0x6C,0x7F,0x69,0x6D,0x5F,0x78,0x9D,0x47,0x55,0x77,0x48,0x43,
  0x80,0x6E,0x50,0x88,0x6C,0x72,0x7D,0x6E,0x47,0x28,0x75,0x3A,0x5E,0x5D,0x7B,0x4A,
  0x63,0x58,0x62,0x3F,0x57,0x4E,0x46,0xA8,0x97,0xA3,0x6B,0x81,0x8A,0x58,0x7A,0x51,
  0x30,0x38,0x3C,0x87,0x90,0xAE,0xBD,0x9C,0x8A,0x8B,0x89,0x5B,0x5F,0x26,0x28,0x22,
  0x4B,0x88,0x79,0x63,0x90,0x89,0x77,0x7C,0x84,0x2F,0x76,0x30,0x68,0x89,0x52,0x88,
};

//
// Lookup table for pwm update delay in according to mechanical shaft angle
// Please just place the output from "uGM100-Cal.ino here (AntiClockwise)
//
const unsigned char rotorAntiClockwise[ANGLE_RES] = 
{
  0x56,0x77,0x36,0x47,0x52,0x28,0x6D,0x84,0x9B,0x7F,0x85,0x82,0x86,0x88,0x76,0x77,
  0x78,0x30,0x2C,0x75,0x2E,0x55,0x60,0x40,0x50,0x47,0x35,0x62,0x54,0x5C,0x5F,0x91,
  0xB3,0x87,0x8F,0x6E,0x74,0x49,0x50,0x27,0x32,0x29,0x6E,0x6C,0x83,0xB4,0xA7,0x89,
  0x86,0x91,0x9A,0x84,0x5A,0x25,0x30,0x29,0x62,0x77,0x96,0x81,0x70,0x76,0x66,0x58,
  0x63,0x1F,0x5D,0x2B,0x9F,0x91,0x83,0x86,0x76,0x68,0x7F,0x5F,0x3B,0x44,0x1E,0x50,
  0x6C,0x80,0x84,0x7D,0x82,0x88,0x7C,0x8A,0x66,0x38,0x23,0x4A,0x35,0x6F,0x85,0x55,
  0x70,0x54,0x5D,0x7D,0x46,0x8C,0x84,0x5B,0x6E,0x50,0x85,0x56,0x63,0x50,0x56,0x41,
  0x25,0x6D,0x39,0x6A,0x87,0x87,0xAC,0x85,0x83,0x61,0x7C,0x67,0x52,0x2C,0x2C,0x29,
  0x54,0x64,0x95,0x80,0x61,0x75,0x54,0x6E,0x3A,0x3D,0x7A,0x40,0x67,0xAC,0x92,0xB2,
  0x98,0x78,0x64,0x85,0x46,0x28,0x2F,0x1E,0x7C,0x7D,0x9A,0xBC,0x92,0x90,0xBC,0xA2,
  0x7C,0x66,0x4E,0x1E,0x31,0x38,0x6B,0x6E,0x46,0x6F,0x54,0x6D,0x3B,0x96,0x4C,0x55,
  0x53,0xA5,0x85,0x8B,0x78,0x81,0x4C,0x82,0x58,0x4A,0x40,0x67,0x49,0x4C,0x91,0x6E,
  0x76,0x70,0x5F,0x6E,0x62,0x61,0x36,0x39,0x49,0x3C,0x5F,0x74,0x8D,0x43,0x75,0x44,
  0x57,0x2E,0x42,0x68,0x3A,0xAE,0x8E,0xB1,0x98,0x85,0x57,0x83,0x59,0x4F,0x28,0x42,
  0x26,0x92,0x77,0xB1,0x99,0xB4,0xA7,0x84,0x9B,0x93,0x68,0x45,0x1E,0x23,0x36,0x73,
  0x72,0x6E,0x87,0x67,0x75,0x80,0x44,0x56,0x4B,0x2B,0x88,0x6E,0x8A,0x78,0x6A,0x6A,
  0x58,0x5F,0x35,0x4F,0x3F,0x43,0x77,0x83,0x81,0x93,0x84,0x87,0x85,0x7D,0x8A,0x61,
  0x5D,0x30,0x4B,0x31,0x51,0x57,0x5F,0x4A,0x49,0x49,0x33,0x4A,0x96,0x79,0x52,0x9A,
  0xB0,0x85,0x88,0x6B,0x52,0x56,0x43,0x1E,0x47,0x27,0x72,0x95,0x81,0x98,0xBC,0xA1,
  0x7F,0x89,0x8D,0x84,0x5A,0x25,0x27,0x3A,0x55,0x78,0x8D,0x84,0x72,0x6F,0x67,0x59,
  0x4B,0x39,0x2B,0x3B,0x91,0x93,0x96,0x8B,0x83,0x77,0x5F,0x78,0x38,0x30,0x37,0x35,
  0x67,0x72,0x88,0x82,0x80,0x80,0x79,0x8C,0x6D,0x33,0x3A,0x37,0x33,0x7C,0x63,0x84,
  0x55,0x71,0x55,0x72,0x54,0x71,0x5D,0x87,0xA1,0x44,0x65,0x68,0x54,0x6B,0x43,0x5A,
  0x2C,0x61,0x22,0x52,0x92,0x85,0xA1,0x83,0x94,0x83,0x80,0x7F,0x4B,0x6A,0x23,0x28,
  0x33,0x4F,0x8C,0x95,0x5A,0x78,0x5D,0x63,0x72,0x31,0x79,0x3E,0x42,0x97,0x9D,0x98,
  0x99,0x90,0x90,0x6F,0x72,0x4E,0x21,0x2B,0x22,0x6B,0x76,0x92,0xB8,0xA0,0x85,0x9D,
  0xC3,0x7E,0x5C,0x3B,0x1E,0x24,0x47,0x66,0x61,0x69,0x5A,0x54,0x54,0x52,0x47,0x9B,
  0x62,0x4E,0xAC,0x80,0x94,0x67,0x7B,0x56,0x72,0x5B,0x3E,0x63,0x4B,0x43,0x61,0x88,
  0x65,0x7F,0x5F,0x7D,0x65,0x6C,0x63,0x35,0x2A,0x76,0x31,0x67,0x6F,0x8C,0x43,0x62,
  0x4B,0x4D,0x31,0x4D,0x36,0x75,0xA0,0x8C,0x95,0xA6,0x9A,0x5B,0x83,0x54,0x50,0x2A,
  0x40,0x29,0x8A,0x82,0xB4,0xA7,0xB4,0xA1,0x82,0x8C,0x7E,0x45,0x43,0x1E,0x1F,0x52,
  0x6F,0x86,0x66,0x80,0x60,0x6F,0x7B,0x44,0x43,0x63,0x2F,0x87,0x73,0x7C,0x67,0x6E,
};

void setup() {
  // put your setup code here, to run once:
  unsigned int i;

  Wire.begin();                               // Start I2c communication
  Serial.begin(115200);                       // Serial communication in 115200 baud

  pinMode(9, OUTPUT);                         // PWM A, OC1A (8bit mode)
  pinMode(5, OUTPUT);                         // PWM B, OC0A (8bit mode)
  pinMode(6, OUTPUT);                         // PWM C, OC0B (8bit mode)

  pinMode(8, OUTPUT);                         // PWM output ENABLE for Sample FOC Shield v2.04
  pinMode(7, OUTPUT);                         // Debug OUTPUT

  rotorAngle = 0;
  oldrotorAngle = 0;
  
  pwmAngle = 0.0f;

  motorPWR = MOTOR_DRIVEPWR;
  motorDIR = CLOCKWISE;

  delta = DELTACHANGE;
  deltapwmAngle = 0.0f;

  timeTick = 0;
  tickRun = false;

  digitalWrite(8,HIGH);                       // Enable PWM output to motor
  cli();                                      // Stop interrupts

  //
  // Config TIMER1
  //
  TCCR1A = (TCCR1A & B00111100) | B10000010;   // Phase and frequency correct, Non-inverting mode, TOP defined by ICR1
  TCCR1B = (TCCR1B & B11100000) | B00010000;   // No prescale, timer stopped. PWM in phase correct mode for OC1A
  TCNT1 = 0;
  ICR1 = 255;                                  // Top = 0xff;
  TIMSK1 |= (1<<TOIE1);                        // Enable timer overflow interrupt, in which used to in sync. with main loop
                                               //   to take reading from magnetic sensor in given time

  //
  // Config TIMER0
  //
  TCCR0A = B10100001;                           // Clear OC0A/OC0B on compare match, PWM in phase correct mode
  TCCR0B = B00000000;                           // for OC0A and OC0B. Timer stopped
  TCNT0 = 0;
  
  //
  // Config TIMER2
  //
  TCCR2A = B00000010;                           // Clear Timer on Compare Match (CTC)
  TCCR2B = B00000000;                           // Stop timer
  TCNT2 = 0;
  OCR2A = 180;                                  // 10ms Timer interrupt
  TIMSK2 |= (1<<OCIE2A);                        // Enable timer compare interrupt, controls the time delay for update PWM values

  // Start Timers
  TCCR1B |= B00000001;                          // Prescaler = 1, timer starts
  TCCR0B |= B00000001;                          // Prescaler = 1, timer starts
  TCCR2B |= B00000111;                          // Prescaler = 1024, timer starts

  sei();                                        // Enable ALL interrupts
}

//
// TIMER1 Interrupt Routine
//   Use a flag to let main loop() get readings from magnetic sensor and
//   update delta angle difference
//
ISR(TIMER1_OVF_vect)
{
  timeTick ++;                                  // Update interupt times
  if (timeTick >= SAMPLE_INTERVAL) {            // Check to see if it's time to update flag
    timeTick = 0;                               //   reset counter
    tickRun = true;                             //   set flag to true
  }
}

//
// TIMER2 Interrupt Routine
// - update new PWM values in according to lookup table with given mechanical shaft angle
// - update phaser angle of sinewave, that is rotor speed control
//
ISR(TIMER2_COMPA_vect) {
  int u,v,w;
  float k;

  //
  //  Check direction needed, use corresponding lookup table and update new delay value
  //
  if (motorDIR == CLOCKWISE)                      
    OCR2A = rotorClockwise[rotorAngle]; 
  else
    OCR2A = rotorAntiClockwise[rotorAngle]; 

  //
  // Calculate the PWM value from sinewave in according to the phaseer angle with 
  //   given motor power required
  //
  u = (int)(sin(pwmAngle) * (float)motorPWR) + ((float)FULL_PWM / 2.0f);
  v = (int)(sin(pwmAngle+PHASE_120) * (float)motorPWR) + ((float)FULL_PWM / 2.0f);
  w = (int)(sin(pwmAngle-PHASE_120) * (float)motorPWR) + ((float)FULL_PWM / 2.0f);

  OCR0A = w;
  OCR0B = u;
  OCR1A = v;

  //
  // Check the phaser change to see if it is within it's maximum or minimum value
  //
  if (deltapwmAngle < DELTACHANGE_MIN)
    deltapwmAngle = DELTACHANGE_MIN;
  if ( deltapwmAngle > DELTACHANGE_MAX)
    deltapwmAngle = DELTACHANGE_MAX;

  //
  // Wrap around the phaser angle in bewteen 0 and 2PI
  //
  if (motorDIR == CLOCKWISE) {
    pwmAngle += deltapwmAngle;
    if (pwmAngle >= (float)TWO_PI)
      pwmAngle = 0.0f;
  }
  else {
    pwmAngle -= deltapwmAngle;
    if (pwmAngle < 0.0f)
      pwmAngle = (float)TWO_PI-deltapwmAngle;
  }
}

void loop() {
  // put your main code here, to run repeatedly:
  unsigned char da[2];
  int rotorAngleDiff, i;

  while (tickRun == false);
  tickRun = false;

  Wire.beginTransmission(0x36);                 // Connect to the sensor
  Wire.write(0x0C);                             // figure 21 - register map: Raw angle (11:8)
  Wire.endTransmission();                       // end transmission
  Wire.requestFrom(0x36, 2);                    // request from the sensor

  i = 0;  
  while(Wire.available())                       // Read two bytes raw angle from sensor
    da[i++] = Wire.read();                      //

  //
  // Convert raw angle to mechanical angle, and scale down
  //
  rotorAngle = (unsigned int)da[0]<<8 | (unsigned int)da[1];
  rotorAngle = rotorAngle / (AS5600_CPR / ANGLE_RES);
  rotorAngle = rotorAngle & (ANGLE_RES-1);  

  //
  // Calculate the angle difference in between current and last angle
  //
  rotorAngleDiff = rotorAngle - oldrotorAngle;
  oldrotorAngle = rotorAngle;

  //
  // Change rotor spinning speed, by alter phaser angle
  //
  deltapwmAngle += delta;
  if (deltapwmAngle >= DELTACHANGE_MAX) {
    deltapwmAngle = DELTACHANGE_MAX;
    delta = delta * -1.0f;
  }

  if (deltapwmAngle <= DELTACHANGE_MIN) {
    deltapwmAngle = DELTACHANGE_MIN;
    delta = delta * -1.0f;

    //
    // Change the spinning direction if needed
    //
    motorDIR = !motorDIR;
  }

  //
  // Toggle a digital I/O pin, for debug purpose
  //
  digitalWrite(7, !digitalRead(7));
}