simplefoc · runger1101001 · Dec 1, 2023 · May 24, 2023 · May 24, 2023 · Sep 13, 2023
diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml
@@ -58,15 +58,15 @@ jobs:
             sketch-names: single_full_control_example.ino
 
           - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2
-            platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
-            sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone 
+            platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json
+            sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino
 
           - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3
-            platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
+            platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json
             sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino
 
           - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 
-            platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
+            platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json
             sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino
 
           - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples

diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml
@@ -0,0 +1,41 @@
+name: PlatformIO - Teensy build
+
+on: [push]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest        
+    steps:
+      - uses: actions/checkout@v3
+      - uses: actions/cache@v3
+        with:
+          path: |
+            ~/.cache/pip
+            ~/.platformio/.cache
+          key: ${{ runner.os }}-pio
+      - uses: actions/setup-python@v4
+        with:
+          python-version: '3.9'
+      - name: Install PlatformIO Core
+        run: pip install --upgrade platformio
+
+      - name: PIO Run Teensy 4
+        run: pio ci --lib="." --board=teensy41 --board=teensy40
+        env:
+          PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino
+
+      - name: PIO Run Teensy 4
+        run: pio ci --lib="." --board=teensy41 --board=teensy40
+        env:
+          PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino
+
+      - name: PIO Run Teensy 3
+        run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36
+        env:
+          PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy3/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino
+
+      - name: PIO Run Teensy 3
+        run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36
+        env:
+          PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino
diff --git a/README.md b/README.md
@@ -1,7 +1,8 @@
 # SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library** <br> 
 ### A Cross-Platform FOC implementation for BLDC and Stepper motors<br> based on the Arduino IDE and PlatformIO 
 
-![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg)
+![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![PlatformIO - Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) 
+
 ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc)
 ![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue)
 ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev)
@@ -24,20 +25,20 @@ Therefore this is an attempt to:
    - *Medium-power* BLDC driver (<30Amps): [Arduino <span class="simple">Simple<b>FOC</b>PowerShield</span> ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield).
    - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller)
 
-> NEW RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.1
-> - Support for Arduino UNO R4 Minima (Renesas R7FA4M1 MCU - note UNO R4 WiFi is not yet supported)
-> - Support setting PWM polarity on ESP32 (thanks to [@mcells](https://github.com/mcells))
-> - Expose I2C errors in MagneticSensorI2C (thanks to [@padok](https://github.com/padok))
-> - Improved default trig functions (sine, cosine) - faster, smaller
-> - Overridable trig functions - plug in your own optimized versions
-> - Bugfix: microseconds overflow in velocity mode [#287](https://github.com/simplefoc/Arduino-FOC/issues/287)
-> - Bugfix: KV initialization ([5fc3128](https://github.com/simplefoc/Arduino-FOC/commit/5fc3128d282b65c141ca486327c6235089999627))
-> - And more bugfixes - see the [complete list of 2.3.1 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.1_Release)
-> - Change: simplify initFOC() API ([d57d32d](https://github.com/simplefoc/Arduino-FOC/commit/d57d32dd8715dbed4e476469bc3de0c052f1d531). [5231e5e](https://github.com/simplefoc/Arduino-FOC/commit/5231e5e1d044b0cc33ede67664b6ef2f9d0a8cdf), [10c5b87](https://github.com/simplefoc/Arduino-FOC/commit/10c5b872672cab72df16ddd738bbf09bcce95d28))
-> - Change: check for linked driver in currentsense and exit gracefully ([5ef4d9d](https://github.com/simplefoc/Arduino-FOC/commit/5ef4d9d5a92e03da0dd5af7f624243ab30f1b688))
-> - Compatibility with newest versions of Arduino framework for STM32, Renesas, ESP32, Atmel SAM, Atmel AVR, nRF52 and RP2040
-
-## Arduino *SimpleFOClibrary* v2.3.1
+> NEW RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.2
+> - Improved [space vector modulation code](https://github.com/simplefoc/Arduino-FOC/pull/309) thanks to [@Candas1](https://github.com/Candas1)
+> - Bugfix for stepper motor initialization
+> - Bugfix for current sensing when only 2 phase currents available - please re-check your current sense PID tuning
+> - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321)
+> - Added teensy3/4 compile to the github CI using platformio
+> - Fix compile issues with recent versions of ESP32 framework
+> - Add ADC calibration on STM32 MCUs
+> - Bugfix for crash when using ADC2 on ESP32s - [thanks to @mcells](https://github.com/simplefoc/Arduino-FOC/pull/346)
+> - Bugfix for renesas PWM on UNO R4 WiFi - [thanks to @facchinm](https://github.com/simplefoc/Arduino-FOC/pull/322)
+> - And more bugfixes - see the complete list of 2.3.2 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/9?closed=1)
+
+
+## Arduino *SimpleFOClibrary* v2.3.2
 
 <p align="">
 <a href="https://youtu.be/Y5kLeqTc6Zk">

diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino
@@ -96,6 +96,7 @@ void setup() {
 
 void loop() {
   // main FOC algorithm function
+  motor.loopFOC();
 
   // Motion control function
   motor.move();

diff --git a/..._examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/..._examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino
@@ -29,7 +29,7 @@
 
 // Motor instance
 BLDCMotor motor = BLDCMotor(11);
-BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INH_A, INH_B,INH_B, INH_C,INL_C, EN_GATE);
+BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INL_A, INH_B,INL_B, INH_C,INL_C, EN_GATE);
 
 // encoder instance
 Encoder encoder = Encoder(2, 3, 8192);

diff --git a/keywords.txt b/keywords.txt
@@ -133,6 +133,7 @@ sensor_direction	KEYWORD2
 sensor_offset	KEYWORD2
 zero_electric_angle	KEYWORD2
 verbose	KEYWORD2
+verboseMode	KEYWORD1
 decimal_places	KEYWORD2
 phase_resistance	KEYWORD2
 phase_inductance	KEYWORD2

diff --git a/library.properties b/library.properties
@@ -1,5 +1,5 @@
 name=Simple FOC
-version=2.3.1
+version=2.3.2
 author=Simplefoc <info@simplefoc.com>
 maintainer=Simplefoc <info@simplefoc.com>
 sentence=A library demistifying FOC for BLDC motors

diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
@@ -422,7 +422,7 @@ void BLDCMotor::move(float new_target) {
       shaft_angle_sp = target;
       // calculate velocity set point
       shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
-      shaft_angle_sp = _constrain(shaft_angle_sp,-velocity_limit, velocity_limit);
+      shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit);
       // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
@@ -545,6 +545,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
     break;
 
     case FOCModulationType::SinePWM :
+    case FOCModulationType::SpaceVectorPWM :
       // Sinusoidal PWM modulation
       // Inverse Park + Clarke transformation
       _sincos(angle_el, &_sa, &_ca);
@@ -553,107 +554,34 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
       Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;
 
-      // center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
-      center = driver->voltage_limit/2;
       // Clarke transform
-      Ua = Ualpha + center;
-      Ub = -0.5f * Ualpha  + _SQRT3_2 * Ubeta + center;
-      Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center;
+      Ua = Ualpha;
+      Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta;
+      Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta;
+
+      center = driver->voltage_limit/2;
+      if (foc_modulation == FOCModulationType::SpaceVectorPWM){
+        // discussed here: https://community.simplefoc.com/t/embedded-world-2023-stm32-cordic-co-processor/3107/165?u=candas1
+        // a bit more info here: https://microchipdeveloper.com/mct5001:which-zsm-is-best
+        // Midpoint Clamp
+        float Umin = min(Ua, min(Ub, Uc));
+        float Umax = max(Ua, max(Ub, Uc));
+        center -= (Umax+Umin) / 2;
+      } 
 
       if (!modulation_centered) {
         float Umin = min(Ua, min(Ub, Uc));
         Ua -= Umin;
         Ub -= Umin;
         Uc -= Umin;
+      }else{
+        Ua += center;
+        Ub += center;
+        Uc += center;
       }
 
       break;
 
-    case FOCModulationType::SpaceVectorPWM :
-      // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
-      // https://www.youtube.com/watch?v=QMSWUMEAejg
-
-      // the algorithm goes
-      // 1) Ualpha, Ubeta
-      // 2) Uout = sqrt(Ualpha^2 + Ubeta^2)
-      // 3) angle_el = atan2(Ubeta, Ualpha)
-      //
-      // equivalent to 2)  because the magnitude does not change is:
-      // Uout = sqrt(Ud^2 + Uq^2)
-      // equivalent to 3) is
-      // angle_el = angle_el + atan2(Uq,Ud)
-
-      float Uout;
-      // a bit of optitmisation
-      if(Ud){ // only if Ud and Uq set
-        // _sqrt is an approx of sqrt (3-4% error)
-        Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit;
-        // angle normalisation in between 0 and 2pi
-        // only necessary if using _sin and _cos - approximation functions
-        angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
-      }else{// only Uq available - no need for atan2 and sqrt
-        Uout = Uq / driver->voltage_limit;
-        // angle normalisation in between 0 and 2pi
-        // only necessary if using _sin and _cos - approximation functions
-        angle_el = _normalizeAngle(angle_el + _PI_2);
-      }
-      // find the sector we are in currently
-      sector = floor(angle_el / _PI_3) + 1;
-      // calculate the duty cycles
-      float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
-      float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout;
-      // two versions possible
-      float T0 = 0; // pulled to 0 - better for low power supply voltage
-      if (modulation_centered) {
-        T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
-      }
-
-      // calculate the duty cycles(times)
-      float Ta,Tb,Tc;
-      switch(sector){
-        case 1:
-          Ta = T1 + T2 + T0/2;
-          Tb = T2 + T0/2;
-          Tc = T0/2;
-          break;
-        case 2:
-          Ta = T1 +  T0/2;
-          Tb = T1 + T2 + T0/2;
-          Tc = T0/2;
-          break;
-        case 3:
-          Ta = T0/2;
-          Tb = T1 + T2 + T0/2;
-          Tc = T2 + T0/2;
-          break;
-        case 4:
-          Ta = T0/2;
-          Tb = T1+ T0/2;
-          Tc = T1 + T2 + T0/2;
-          break;
-        case 5:
-          Ta = T2 + T0/2;
-          Tb = T0/2;
-          Tc = T1 + T2 + T0/2;
-          break;
-        case 6:
-          Ta = T1 + T2 + T0/2;
-          Tb = T0/2;
-          Tc = T1 + T0/2;
-          break;
-        default:
-         // possible error state
-          Ta = 0;
-          Tb = 0;
-          Tc = 0;
-      }
-
-      // calculate the phase voltages and center
-      Ua = Ta*driver->voltage_limit;
-      Ub = Tb*driver->voltage_limit;
-      Uc = Tc*driver->voltage_limit;
-      break;
-
   }
 
   // set the voltages in driver

diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
@@ -134,7 +134,7 @@ int StepperMotor::alignSensor() {
   SIMPLEFOC_DEBUG("MOT: Align sensor.");
 
   // if unknown natural direction
-  if(!_isset(sensor_direction)){
+  if(sensor_direction == Direction::UNKNOWN){
     // check if sensor needs zero search
     if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
     // stop init if not found index
@@ -311,7 +311,7 @@ void StepperMotor::move(float new_target) {
       shaft_angle_sp = target;
       // calculate velocity set point
       shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
-      shaft_angle_sp = _constrain(shaft_angle_sp, -velocity_limit, velocity_limit);
+      shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
       // calculate the torque command
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
@@ -440,4 +440,4 @@ float StepperMotor::angleOpenloop(float target_angle){
   open_loop_timestamp = now_us;
 
   return Uq;
-}
+}
diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp
@@ -16,12 +16,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
         // if only two measured currents
         i_alpha = current.a;  
         i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
-    }if(!current.a){
+    }else if(!current.a){
         // if only two measured currents
         float a = -current.c - current.b;
         i_alpha = a;  
         i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
-    }if(!current.b){
+    }else if(!current.b){
         // if only two measured currents
         float b = -current.a - current.c;
         i_alpha = current.a;  
@@ -62,12 +62,12 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
         // if only two measured currents
         i_alpha = current.a;  
         i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
-    }if(!current.a){
+    }else if(!current.a){
         // if only two measured currents
         float a = -current.c - current.b;
         i_alpha = a;  
         i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
-    }if(!current.b){
+    }else if(!current.b){
         // if only two measured currents
         float b = -current.a - current.c;
         i_alpha = current.a;  

diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp
@@ -6,6 +6,8 @@
 
 void Sensor::update() {
     float val = getSensorAngle();
+    if (val<0) // sensor angles are strictly non-negative. Negative values are used to signal errors.
+        return; // TODO signal error, e.g. via a flag and counter
     angle_prev_ts = _micros();
     float d_angle = val - angle_prev;
     // if overflow happened track it as full rotation

diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp
@@ -44,6 +44,33 @@ __attribute__((weak)) void _sincos(float a, float* s, float* c){
   *c = _cos(a);
 }
 
+// fast_atan2 based on https://math.stackexchange.com/a/1105038/81278
+// Via Odrive project
+// https://github.com/odriverobotics/ODrive/blob/master/Firmware/MotorControl/utils.cpp
+// This function is MIT licenced, copyright Oskar Weigl/Odrive Robotics
+// The origin for Odrive atan2 is public domain. Thanks to Odrive for making
+// it easy to borrow.
+__attribute__((weak)) float _atan2(float y, float x) {
+    // a := min (|x|, |y|) / max (|x|, |y|)
+    float abs_y = fabsf(y);
+    float abs_x = fabsf(x);
+    // inject FLT_MIN in denominator to avoid division by zero
+    float a = min(abs_x, abs_y) / (max(abs_x, abs_y));
+    // s := a * a
+    float s = a * a;
+    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
+    float r =
+        ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
+    // if |y| > |x| then r := 1.57079637 - r
+    if (abs_y > abs_x) r = 1.57079637f - r;
+    // if x < 0 then r := 3.14159274 - r
+    if (x < 0.0f) r = 3.14159274f - r;
+    // if y < 0 then r := -r
+    if (y < 0.0f) r = -r;
+
+    return r;
+  }
+
 
 // normalizing radian angle to [0,2PI]
 __attribute__((weak)) float _normalizeAngle(float angle){
@@ -60,14 +87,10 @@ float _electricalAngle(float shaft_angle, int pole_pairs) {
 // https://reprap.org/forum/read.php?147,219210
 // https://en.wikipedia.org/wiki/Fast_inverse_square_root
 __attribute__((weak)) float _sqrtApprox(float number) {//low in fat
-  // float x;
-  // const float f = 1.5F; // better precision
-
-  // x = number * 0.5F;
-  float y = number;
-  long i = * ( long * ) &y;
-  i = 0x5f375a86 - ( i >> 1 );
-  y = * ( float * ) &i;
-  // y = y * ( f - ( x * y * y ) ); // better precision
-  return number * y;
+  union {
+    float    f;
+    uint32_t i;
+  } y = { .f = number };
+  y.i = 0x5f375a86 - ( y.i >> 1 );
+  return number * y.f;
 }