Initial 8pwm

.github/workflows/ccpp.yml

@@ -20,6 +20,7 @@ jobs:
           - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8   # stm32 bluepill
           - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo
           - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX  # stm32f405 - odrive
+          - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX  # stm32l475
           - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1      # B-G431-ESC1
           - arduino:mbed_rp2040:pico                 # rpi pico
 
@@ -75,6 +76,10 @@ jobs:
             platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json 
             sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino, stm32_current_control_low_side.ino
 
+          - arduino-boards-fqbn: STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX  # stm32l475
+            platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json 
+            sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino
+
           - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example
             platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json 
             sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino

README.md

@@ -24,7 +24,7 @@ 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)
 
-> NEXT RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.0
+> NEW RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.0
 > - Arduino Mega 6pwm more timers supported 
 > - Arduino boards - frequency change support either 32kHz or 4kHz
 > - Arduino Uno - synched timers in 3pwm and 6pwm mode [#71](https://github.com/simplefoc/Arduino-FOC/issues/71)

src/StepperMotor.cpp

@@ -371,6 +371,70 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
   driver->setPwm(Ualpha, Ubeta);
 }
 
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Sine PWM algorithms
+// - space vector not implemented yet
+//
+
+int32_t float_to_q31(float input) {
+    int32_t q31 = (int32_t) (input * 2147483648.0f);
+    q31 = (q31 > 0) ? (q31 << 1) : (-q31 << 1);
+    return q31;
+}
+
+#define pi 3.1415926535897932384626433f
+
+int32_t q31_value;
+float value_f32_sine = 0;
+float value_f32_cosine = 0;
+float cordic_cosine = 0.0f;
+float cordic_sine = 0.0f;
+
+float wrap_to_1(float x) {
+    while (x > 1.0f) {
+        x -= 2.0f;
+    }
+    while (x < -1.0f) {
+        x += 2.0f;
+    }
+    return x;
+}
+
+void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) {
+  // Sinusoidal PWM modulation
+  // Inverse Park transformation
+
+  // WHO U GONNA CALL? CORDIC ->
+
+q31_value = float_to_q31(angle_el / (2.0f * pi));
+
+CORDIC->WDATA = q31_value;
+cordic_sine = CORDIC->RDATA;
+cordic_cosine = CORDIC->RDATA;
+
+value_f32_sine = (float)cordic_sine/(float)0x80000000;
+value_f32_cosine = (float)cordic_cosine/(float)0x80000000;
+
+if (angle < 0){
+value_f32_sine = wrap_to_1(value_f32_sine);
+value_f32_sine = value_f32_sine * -1;
+}
+
+if (angle > 0){
+value_f32_sine = wrap_to_1(value_f32_sine);
+}
+
+value_f32_cosine = wrap_to_1(value_f32_cosine);
+
+  // Inverse park transform
+  Ualpha =  value_f32_cosine * Ud - value_f32_sine * Uq;  // -sin(angle) * Uq;
+  Ubeta =  value_f32_sine * Ud + value_f32_cosine * Uq;    //  cos(angle) * Uq;
+
+  // set the voltages in hardware
+  driver->setPwm(Ualpha, Ubeta);
+}
+
+
 // Function (iterative) generating open loop movement for target velocity
 // - target_velocity - rad/s
 // it uses voltage_limit variable

src/StepperMotor.h

@@ -89,6 +89,10 @@ class StepperMotor: public FOCMotor
     */
     void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
 
+    // CORDIC
+
+    void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override;
+
   private:
 
     /** Sensor alignment to electrical 0 angle of the motor */

src/common/base_classes/FOCMotor.h

@@ -136,6 +136,10 @@ class FOCMotor
     * @param angle_el current electrical angle of the motor
     */
     virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0;
+
+	// CORDIC
+
+	 virtual void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el)=0;
 
     // State calculation methods 
     /** Shaft angle calculation in radians [rad] */

src/common/base_classes/StepperDriver.h

@@ -5,28 +5,30 @@
 
 class StepperDriver{
     public:
-
-        /** Initialise hardware */
-        virtual int init() = 0;
-        /** Enable hardware */
-        virtual void enable() = 0;
-        /** Disable hardware */
-        virtual void disable() = 0;
-
-        long pwm_frequency; //!< pwm frequency value in hertz
-        float voltage_power_supply; //!< power supply voltage 
-        float voltage_limit; //!< limiting voltage set to the motor
-
-        bool initialized = false; // true if driver was successfully initialized
-        void* params = 0; // pointer to hardware specific parameters of driver
-
-        /** 
-         * Set phase voltages to the harware 
-         * 
-         * @param Ua phase A voltage
-         * @param Ub phase B voltage
-        */
-        virtual void setPwm(float Ua, float Ub) = 0;
+       /** Initialise hardware */
+virtual int init() = 0;
+/** Enable hardware */
+virtual void enable() = 0;
+/** Disable hardware */
+virtual void disable() = 0;
+
+long pwm_frequency; //!< pwm frequency value in hertz
+float voltage_power_supply; //!< power supply voltage 
+float voltage_limit; //!< limiting voltage set to the motor
+
+bool initialized = false; // true if driver was successfully initialized
+void* params = 0; // pointer to hardware specific parameters of driver
+
+/** 
+ * Set phase voltages to the hardware 
+ * 
+ * @param Ua phase A voltage
+ * @param Ub phase B voltage
+ */
+virtual void setPwm(float Ua, float Ub) = 0;
+
+
+
 };
 
 #endif

src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp

@@ -187,7 +187,7 @@ uint32_t _timerToRegularTRGO(HardwareTimer* timer){
 #endif
 #ifdef TIM8 // if defined timer 8
   else if(timer->getHandle()->Instance == TIM8) 
-    return ADC_EXTERNALTRIG_T7_TRGO;
+    return ADC_EXTERNALTRIG_T8_TRGO;
 #endif
 #ifdef TIM15 // if defined timer 15
   else if(timer->getHandle()->Instance == TIM15) 

src/drivers/StepperDriver8PWM.cpp

@@ -0,0 +1,98 @@
+#include "StepperDriver8PWM.h"
+
+StepperDriver8PWM::StepperDriver8PWM(int ph1A, int ph1B, int ph2A, int ph2B, int ph3A, int ph3B, int ph4A, int ph4B, int en1, int en2, int en3, int en4) {
+  // Pin initialization
+  pwm1A = ph1A;
+  pwm1B = ph1B;
+  pwm2A = ph2A;
+  pwm2B = ph2B;
+  pwm3A = ph3A;
+  pwm3B = ph3B;
+  pwm4A = ph4A;
+  pwm4B = ph4B;
+
+  // Enable pins
+  enable_pin1 = en1;
+  enable_pin2 = en2;
+  enable_pin3 = en3;
+  enable_pin4 = en4;
+
+  // Default values
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+
+  pinMode(enable_pin1, OUTPUT);
+  pinMode(enable_pin2, OUTPUT);
+  pinMode(enable_pin3, OUTPUT);
+  pinMode(enable_pin4, OUTPUT);
+  disable();
+
+    // dead zone initial - 2%
+  dead_zone = 0.02f;
+
+}
+
+// init hardware pins for 8PWM control
+int StepperDriver8PWM::init() {
+
+  // PWM pins
+  pinMode(pwm1A, OUTPUT);
+  pinMode(pwm1B, OUTPUT);
+  pinMode(pwm2A, OUTPUT);
+  pinMode(pwm2B, OUTPUT);
+  pinMode(pwm3A, OUTPUT);
+  pinMode(pwm3B, OUTPUT);
+  pinMode(pwm4A, OUTPUT);
+  pinMode(pwm4B, OUTPUT);
+
+  if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT);
+  if( _isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT);
+  if( _isset(enable_pin3) ) pinMode(enable_pin3, OUTPUT);
+  if( _isset(enable_pin4) ) pinMode(enable_pin4, OUTPUT);
+  // sanity check for the voltage limit configuration
+  if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
+
+  // Set the pwm frequency to the pins
+  // hardware specific function - depending on driver and mcu
+  params = _configure8PWM(pwm_frequency, dead_zone, pwm1A, pwm1B, pwm2A, pwm2B, pwm3A, pwm3B, pwm4A, pwm4B);
+  initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);  
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
+}
+
+
+// Set voltage to the pwm pin for 8PWM control
+void StepperDriver8PWM::setPwm(float Ualpha, float Ubeta) {
+  float duty_cycle1A_h1(0.0f),duty_cycle1A_h2(0.0f),duty_cycle1B_h1(0.0f),duty_cycle1B_h2(0.0f);
+  float duty_cycle2A_h1(0.0f),duty_cycle2A_h2(0.0f),duty_cycle2B_h1(0.0f),duty_cycle2B_h2(0.0f);
+
+  // limit the voltage in driver
+  Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit);
+  Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit);
+
+  // hardware specific writing
+  if( Ualpha > 0 ) {
+    duty_cycle1B_h1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f);
+    duty_cycle1B_h2 = 0.0f; // set second half-bridge duty cycle to 0
+  }
+  else {
+    duty_cycle1A_h1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f);
+    duty_cycle1A_h2 = 0.0f; // set second half-bridge duty cycle to 0
+  }
+
+  if( Ubeta > 0 ) {
+    duty_cycle2B_h1 = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
+    duty_cycle2B_h2 = 0.0f; // set second half-bridge duty cycle to 0
+  }
+  else {
+    duty_cycle2A_h1 = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
+    duty_cycle2A_h2 = 0.0f; // set second half-bridge duty cycle to 0
+  }
+
+  // write to hardware
+  _writeDutyCycle8PWM(duty_cycle1A_h1, duty_cycle1A_h2, duty_cycle1B_h1, duty_cycle1B_h2,
+                      duty_cycle2A_h1, duty_cycle2A_h2, duty_cycle2B_h1, duty_cycle2B_h2, params);
+}
+
+

src/drivers/StepperDriver8PWM.h

@@ -0,0 +1,68 @@
+#ifndef STEPPER_DRIVER_4PWM_h
+#define STEPPER_DRIVER_4PWM_h
+
+#include "../common/base_classes/StepperDriver.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "hardware_api.h"
+
+/**
+ 8 pwm stepper driver class
+*/
+class StepperDriver8PWM : public StepperDriver {
+  public:
+    /**
+      StepperMotor class constructor
+      @param ph1A 1A phase pwm pin
+      @param ph1B 1B phase pwm pin
+      @param ph2A 2A phase pwm pin
+      @param ph2B 2B phase pwm pin
+      @param ph3A 3A phase pwm pin
+      @param ph3B 3B phase pwm pin
+      @param ph4A 4A phase pwm pin
+      @param ph4B 4B phase pwm pin
+      @param en1 enable pin phase 1 (optional input)
+      @param en2 enable pin phase 2 (optional input)
+      @param en3 enable pin phase 3 (optional input)
+      @param en4 enable pin phase 4 (optional input)
+    */
+    StepperDriver8PWM(int ph1A, int ph1B, int ph2A, int ph2B, int ph3A, int ph3B, int ph4A, int ph4B, int en1 = NOT_SET, int en2 = NOT_SET, int en3 = NOT_SET, int en4 = NOT_SET);
+
+    /**  Motor hardware init function */
+    int init() override;
+    /** Motor disable function */
+    void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+     float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1]
+
+    // hardware variables
+    int pwm1A; //!< phase 1A pwm pin number
+    int pwm1B; //!< phase 1B pwm pin number
+    int pwm2A; //!< phase 2A pwm pin number
+    int pwm2B; //!< phase 2B pwm pin number
+    int pwm3A; //!< phase 3A pwm pin number
+    int pwm3B; //!< phase 3B pwm pin number
+    int pwm4A; //!< phase 4A pwm pin number
+    int pwm4B; //!< phase 4B pwm pin number
+    int enable_pin1; //!< enable pin number phase 1
+    int enable_pin2; //!< enable pin number phase 2
+    int enable_pin3; //!< enable pin number phase 3
+    int enable_pin4; //!< enable pin number phase 4
+
+      /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param Ua phase A voltage
+     * @param Ub phase B voltage
+    */
+    void setPwm(float Ua, float Ub) override;
+  private:
+
+};
+
+
+
+#endif