Better encoder alignment and refactoring

Author: askuric

README.md

@@ -3,7 +3,7 @@
 
 ![Library Compile](https://github.com/askuric/Arduino-FOC/workflows/Library%20Compile/badge.svg)
 [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT)
-
+[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
 
 Proper low cost FOC supporting boards are very hard to find these days and even may not exist. The reason may be that the hobby community has not yet dug into it properly. Therefore this is the attempt to demistify the Field Oriented Control (FOC) algorithm and make a robust but simple implementation for usage with Arduino hadrware.
 
@@ -13,7 +13,6 @@ Proper low cost FOC supporting boards are very hard to find these days and even
 - Simple usage and scalability (Arduino)
  and demistify FOC control in a simple way. 
 
-
 For minimal version of the code more suitable for experimenting please visit the [minimal branch](https://github.com/askuric/Arduino-FOC/tree/minimal).
 
 #### The closest you can get to FOC support and low cost (I was able to find) is:
@@ -28,7 +27,7 @@ For minimal version of the code more suitable for experimenting please visit the
 
 <a href="https://www.infineon.com/cms/en/product/evaluation-boards/bldc_shield_tle9879/" >Infineon</a> | <a href="https://github.com/gouldpa/FOC-Arduino-Brushless">FOC-Arduino-Brushless</a>
 ------------ | -------------
-<img src="https://www.infineon.com/export/sites/default/_images/product/evaluation-boards/BLDC_Motor_Shild_with_TLE9879QXA40.jpg_1711722916.jpg" height="300px" width="400px">| <img src="https://hackster.imgix.net/uploads/attachments/998086/dev_kit_89eygMekks.jpg?auto=compress%2Cformat&w=1280&h=960&fit=max" width="400px">
+<img src="https://www.infineon.com/export/sites/default/_images/product/evaluation-boards/BLDC_Motor_Shild_with_TLE9879QXA40.jpg_1711722916.jpg" width="400px">| <img src="https://hackster.imgix.net/uploads/attachments/998086/dev_kit_89eygMekks.jpg?auto=compress%2Cformat&w=1280&h=960&fit=max" width="400px">
 :x: Open Source | :heavy_check_mark: Open Source
 :heavy_check_mark:Simple to use | :x: Simple to use
 :heavy_check_mark:Low cost ($40) | :heavy_check_mark: Low cost
@@ -182,12 +181,12 @@ First thing you can configure is your `power_supply_voltage` value. The default
 // power supply voltage
 motor.power_supply_voltage = 12;
 ```
-You can also change driver type by changing the value of the variable `motor.driver`. It tells the algorithm to generate unipolar of bipolar FOC voltages. This basically means if your BLDC driver can only output voltages in range `[0,power_supply_voltage]` your driver is `DriverType::unipolar` and if it can output voltage in range `[-power_supply_voltage, power_supply_voltage]` than you driver is `DriverType::bipolar` what is case in most of the drivers and what is default value as well.
+You can also change driver type by changing the value of the variable `motor.driver`. It tells the algorithm to generate unipolar of bipolar FOC voltages. This basically means if your BLDC driver can only output voltages in range `[0,power_supply_voltage]` your driver is `DriverType::half_bridge` and if it can output voltage in range `[-power_supply_voltage, power_supply_voltage]` than you driver is `DriverType::full_bridge` what is case in most of the drivers and what is default value as well.
 ```cpp
 // set driver type
-//  DriverType::unipolar   // HMBGC
-//  DriverType::bipolar    // L6234 (default)
-motor.driver = DriverType::bipolar;
+//  DriverType::full_bridge   // HMBGC
+//  DriverType::half_bridge    // L6234 (default)
+motor.driver = DriverType::half_bridge;
 ```
 ## Control loop setup
 First parameter you can change is the variable you want to control. You set it by changing the `motor.controller` variable. If you want to control the motor angle you will set the `controller` to `ControlType::angle`, if youy seek the DC motor behavior behaviour by controlling the voltage use `ControlType::voltage`, if you wish to control motor angular velocity `ControlType::velocity`. If you wish to control velocities which are very very slow, typically around ~0.01 rad/s you can use the `ControlType::velocity_ultra_slow` controller.
@@ -327,7 +326,7 @@ The real time execution of the Arduino Simple FOC library is govenred by two fun
 // the best would be to be in ~10kHz range
 motor.loopFOC();
 ```
-The funciton `loopFOC()` gets the current motor angle from the encoder, turns in into the electrical angle and computes Clarke transfrom to set the desired $U_q$ voltage to the motor. Basically it implements the funcitonality of the [velocity control loop](#voltage-control-loop).
+The funciton `loopFOC()` gets the current motor angle from the encoder, turns in into the electrical angle and computes Clarke transfrom to set the desired $U_q$ voltage to the motor. Basically it implements the funcitonality of the [voltage control loop](#voltage-control-loop).
 - The faster you can run this funciton the better 
 - In the empty arduino loop it runs at ~1kHz but idealy it would be around ~10kHz
 

examples/HMBGC_example/HMBGC_example.ino

@@ -37,9 +37,9 @@ void setup() {
   PciManager.registerListener(&listenerB);
 
   // set driver type
-  //  DriverType::unipolar
-  //  DriverType::bipolar    - default
-  motor.driver = DriverType::unipolar;
+  //  DriverType::full_bridge
+  //  DriverType::half_bridge    - default
+  motor.driver = DriverType::half_bridge
 
   // power supply voltage
   // default 12V

examples/angle_control/angle_control.ino

@@ -31,9 +31,9 @@ void setup() {
   encoder.init(doA, doB);
 
   // set driver type
-  //  DriverType::unipolar
-  //  DriverType::bipolar    - default
-  motor.driver = DriverType::bipolar;
+  //  DriverType::full_bridge
+  //  DriverType::half_bridge    - default
+  motor.driver = DriverType::half_bridge
 
   // power supply voltage
   // default 12V

examples/angle_control_serial/angle_control_serial.ino

@@ -32,10 +32,11 @@ void setup() {
 
   // initialise encoder hardware
   encoder.init(doA, doB);
+
   // set driver type
-  //  DriverType::unipolar
-  //  DriverType::bipolar    - default
-  motor.driver = DriverType::bipolar;
+  //  DriverType::full_bridge
+  //  DriverType::half_bridge    - default
+  motor.driver = DriverType::half_bridge
 
   // power supply voltage
   // default 12V

examples/change_direction/change_direction.ino

@@ -31,9 +31,9 @@ void setup() {
   encoder.init(doA, doB);
 
   // set driver type
-  //  DriverType::unipolar
-  //  DriverType::bipolar    - default
-  motor.driver = DriverType::bipolar;
+  //  DriverType::full_bridge
+  //  DriverType::half_bridge    - default
+  motor.driver = DriverType::half_bridge
 
   // power supply voltage
   // default 12V

examples/velocity_control/velocity_control.ino

@@ -31,9 +31,9 @@ void setup() {
   encoder.init(doA, doB);
 
   // set driver type
-  //  DriverType::unipolar
-  //  DriverType::bipolar    - default
-  motor.driver = DriverType::bipolar;
+  //  DriverType::full_bridge
+  //  DriverType::half_bridge    - default
+  motor.driver = DriverType::half_bridge
 
   // power supply voltage
   // default 12V

examples/velocity_control_serial/velocity_control_serial.ino

@@ -35,9 +35,9 @@ void setup() {
   encoder.init(doA, doB);
 
   // set driver type
-  //  DriverType::unipolar
-  //  DriverType::bipolar    - default
-  motor.driver = DriverType::bipolar;
+  //  DriverType::full_bridge
+  //  DriverType::half_bridge    - default
+  motor.driver = DriverType::half_bridge
 
   // power supply voltage
   // default 12V

examples/velocity_ultraslow_control_serial/velocity_ultraslow_control_serial.ino

@@ -35,9 +35,9 @@ void setup() {
   encoder.init(doA, doB);
 
   // set driver type
-  //  DriverType::unipolar
-  //  DriverType::bipolar    - default
-  motor.driver = DriverType::bipolar;
+  //  DriverType::full_bridge
+  //  DriverType::half_bridge    - default
+  motor.driver = DriverType::half_bridge
 
   // power supply voltage
   // default 12V

examples/voltage_control/voltage_control.ino

@@ -31,9 +31,9 @@ void setup() {
   encoder.init(doA, doB);
 
   // set driver type
-  //  DriverType::unipolar
-  //  DriverType::bipolar    - default
-  motor.driver = DriverType::bipolar;
+  //  DriverType::full_bridge
+  //  DriverType::half_bridge    - default
+  motor.driver = DriverType::half_bridge
 
   // power supply voltage
   // default 12V

src/BLDCMotor.cpp

@@ -29,13 +29,17 @@ BLDCMotor::BLDCMotor(int phA, int phB, int phC, int pp, int en)
   PI_velocity.Ti = DEF_PI_VEL_TI;
   PI_velocity.timestamp = micros();
   PI_velocity.u_limit = -1;
+  PI_velocity.uk_1 = 0;
+  PI_velocity.ek_1 = 0;
 
   // Ultra slow velocity
   // PI contoroller
   PI_velocity_ultra_slow.K = DEF_PI_VEL_US_K;
   PI_velocity_ultra_slow.Ti = DEF_PI_VEL_US_TI;
   PI_velocity_ultra_slow.timestamp = micros();
   PI_velocity_ultra_slow.u_limit = -1;
+  PI_velocity_ultra_slow.uk_1 = 0;
+  PI_velocity_ultra_slow.ek_1 = 0;
 
   // position loop config
   // P controller constant
@@ -44,7 +48,7 @@ BLDCMotor::BLDCMotor(int phA, int phB, int phC, int pp, int en)
   P_angle.velocity_limit = DEF_P_ANGLE_VEL_LIM;
 
   // driver deafault type
-  driver = DriverType::bipolar;
+  driver = DriverType::half_bridge;
 }
 
 // init hardware pins
@@ -118,11 +122,13 @@ void BLDCMotor::linkEncoder(Encoder* enc) {
 	Encoder alignment to electrical 0 angle
 */
 void BLDCMotor::alignEncoder() {
-  setPhaseVoltage(12, -M_PI/2);
-  delay(1000);
+  //setPhaseVoltage(12, M_PI/2);
+  setPwm(pwmA,12);
+  setPwm(pwmB,0);
+  setPwm(pwmC,0);
+  delay(1500);
   encoder->setCounterZero();
-
-  
+  delay(500);
   setPhaseVoltage(0, 0);
 }
 
@@ -191,84 +197,53 @@ void BLDCMotor::move(float target) {
 /**
 	FOC methods
 */
-/*
-	Method using FOC to set Uq to the motor at the optimal angle
-  - for unipolar drivers - only positive values
-*/
-void BLDCMotor::setPhaseVoltageUnipolar(double Uq, double angle_el) {
-
-  // Uq sign compensation
-  float angle = Uq > 0 ? angle_el : normalizeAngle( angle_el + M_PI );
-  // Park transform
-  Ualpha = abs(Uq) * cos(angle);
-  Ubeta = abs(Uq) * sin(angle);
-
-  // determine the segment I, II, III
-  if ((angle >= 0) && (angle <= _120_D2R)) {
-    // section I
-    Ua = Ualpha + _1_SQRT3 * Ubeta;
-    Ub = _2_SQRT3 * Ubeta;
-    Uc = 0;
-
-  } else if ((angle > _120_D2R) && (angle <= (2 * _120_D2R))) {
-    // section III
-    Ua = 0;
-    Ub = _1_SQRT3 * Ubeta - Ualpha;
-    Uc = -_1_SQRT3 * Ubeta - Ualpha;
-
-  } else if ((angle > (2 * _120_D2R)) && (angle <= (3 * _120_D2R))) {
-    // section II
-    Ua = Ualpha - _1_SQRT3 * Ubeta;
-    Ub = 0;
-    Uc = - _2_SQRT3 * Ubeta;
-  }
-
-  // set phase voltages
-  setPwm(pwmA, Ua);
-  setPwm(pwmB, Ub);
-  setPwm(pwmC, Uc);
-}
 /*
   Method using FOC to set Uq to the motor at the optimal angle
-  - for bipolar drivers - posiitve and negative voltages
 */
-void BLDCMotor::setPhaseVoltageBipolar(double Uq, double angle_el) {
-
-  // q component angle
-  float angle = angle_el + M_PI/2;
+void BLDCMotor::setPhaseVoltage(double Uq, double angle_el) {
+  
   // Uq sign compensation
-  angle = Uq > 0 ? angle : normalizeAngle( angle + M_PI );
-
+  float angle = angle_el + M_PI/2.0;
+  // Uq sign compensation
+  angle = normalizeAngle(Uq > 0 ? angle :  angle + M_PI );
   // Park transform
   Ualpha = abs(Uq) * cos(angle);
   Ubeta = abs(Uq) * sin(angle);
-
-  // determine the segment I, II, III
-  // section I
-  Ua = Ualpha;
-  Ub = -0.5 * Ualpha  + _SQRT3_2 * Ubeta;
-  Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta;
-
-  // set phase voltages
-  setPwm(pwmA, Ua);
-  setPwm(pwmB, Ub);
-  setPwm(pwmC, Uc);
-}
-
-/*
-  Method using FOC to set Uq to the motor at the optimal angle
-*/
-void BLDCMotor::setPhaseVoltage(double Uq, double angle_el) {
+  
   switch (driver) {
-    case DriverType::bipolar :
-      // L6234
-      setPhaseVoltageBipolar(Uq, angle_el);
+    case DriverType::full_bridge :
+      // full Clarke transform
+      Ua = Ualpha;
+      Ub = -0.5 * Ualpha  + _SQRT3_2 * Ubeta;
+      Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta;
       break;
-    case DriverType::unipolar :
-      // HMBGC
-      setPhaseVoltageUnipolar(Uq, angle_el);
+    case DriverType::half_bridge :
+      // HMBGC & L6234
+      // Unipolar Clarke transform
+      // determine the segment I, II, III
+      if ((angle >= 0) && (angle <= _120_D2R)) {
+        // section I
+        Ua = Ualpha + _1_SQRT3 * Ubeta;
+        Ub = _2_SQRT3 * Ubeta;
+        Uc = 0;
+      } else if ((angle > _120_D2R) && (angle <= (2 * _120_D2R))) {
+        // section III
+        Ua = 0;
+        Ub = _1_SQRT3 * Ubeta - Ualpha;
+        Uc = -_1_SQRT3 * Ubeta - Ualpha;
+      } else if ((angle > (2 * _120_D2R)) && (angle <= (3 * _120_D2R))) {
+        // section II
+        Ua = Ualpha - _1_SQRT3 * Ubeta;
+        Ub = 0;
+        Uc = - _2_SQRT3 * Ubeta;
+      }
       break;
   }
+  
+  // set phase voltages
+  setPwm(pwmA, Ua);
+  setPwm(pwmB, Ub);
+  setPwm(pwmC, Uc);
 }
 
 
@@ -280,23 +255,21 @@ void BLDCMotor::setPwm(int pinPwm, float U) {
   int U_max = power_supply_voltage;
   // uniploar or bipolar FOC
   switch (driver) {
-    case DriverType::bipolar :
+    case DriverType::full_bridge :
       // sets the voltage [-U_max,U_max] to pwm [0,255]
-      // - U_max you can set in header file - default 12V
-      // - support for L6234 drive
       U_pwm = ((float)U + (float)U_max) / (2.0 * (float)U_max) * 255.0;
       break;
-    case DriverType::unipolar :
+    case DriverType::half_bridge :
       // HMBGC
       // sets the voltage [0,12V(U_max)] to pwm [0,255]
-      // - U_max you can set in header file - default 12V
       // - support for HMBGC controller
-      U_pwm = 255.0 * (float)U / (float)U_max;
+      // - support for L6234 drive
+      U_pwm = (int)(255.0 * (float)U / (float)U_max);
       break;
   }
   // limit the values between 0 and 255;
   U_pwm = U_pwm < 0 ? 0 : U_pwm;
-  U_pwm = U_pwm > 255 ? 255 : U_pwm;
+  U_pwm = U_pwm >= 255 ? 255 : U_pwm;
 
   // write hardware pwm
   analogWrite(pinPwm, U_pwm);
@@ -325,7 +298,7 @@ float BLDCMotor::velocityPI(float ek) {
 
   // u(s) = Kr(1 + 1/(Ti.s))
   float uk = PI_velocity.uk_1;
-  uk += PI_velocity.K * (Ts / (2 * PI_velocity.Ti) + 1) * ek + PI_velocity.K * (Ts / (2 * PI_velocity.Ti) - 1) * PI_velocity.ek_1;
+  uk += PI_velocity.K * (Ts / (2.0 * PI_velocity.Ti) + 1.0) * ek + PI_velocity.K * (Ts / (2.0 * PI_velocity.Ti) - 1.0) * PI_velocity.ek_1;
   if (abs(uk) > PI_velocity.u_limit) uk = uk > 0 ? PI_velocity.u_limit : -PI_velocity.u_limit;
 
   PI_velocity.uk_1 = uk;