Signed-off-by: myperworld <myperworld@126.com>

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/BLDCDriver3PWM.cpp

@@ -0,0 +1,92 @@
+#include "BLDCDriver3PWM.h"
+
+BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int en3){
+  // Pin initialization
+  pwmA = phA;
+  pwmB = phB;
+  pwmC = phC;
+
+  // enable_pin pin
+  enableA_pin = en1;
+  enableB_pin = en2;
+  enableC_pin = en3;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+}
+
+// enable motor driver
+void  BLDCDriver3PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high);
+    if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high);
+    if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, enable_active_high);
+    // set zero to PWM
+    setPwm(0,0,0);
+}
+
+// disable motor driver
+void BLDCDriver3PWM::disable()
+{
+  // set zero to PWM
+  setPwm(0, 0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, !enable_active_high);
+  if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, !enable_active_high);
+  if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, !enable_active_high);
+
+}
+
+// init hardware pins
+int BLDCDriver3PWM::init() {
+  // PWM pins
+  pinMode(pwmA, OUTPUT);
+  pinMode(pwmB, OUTPUT);
+  pinMode(pwmC, OUTPUT);
+  if( _isset(enableA_pin)) pinMode(enableA_pin, OUTPUT);
+  if( _isset(enableB_pin)) pinMode(enableB_pin, OUTPUT);
+  if( _isset(enableC_pin)) pinMode(enableC_pin, 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
+  _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC);
+  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
+  return 0;
+}
+
+
+
+// Set voltage to the pwm pin
+void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) {
+  // disable if needed
+  if( _isset(enableA_pin) &&  _isset(enableB_pin)  && _isset(enableC_pin) ){
+    digitalWrite(enableA_pin, sa == _HIGH_IMPEDANCE ? LOW : HIGH);
+    digitalWrite(enableB_pin, sb == _HIGH_IMPEDANCE ? LOW : HIGH);
+    digitalWrite(enableC_pin, sc == _HIGH_IMPEDANCE ? LOW : HIGH);
+  }
+}
+
+// Set voltage to the pwm pin
+void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) {
+
+  // limit the voltage in driver
+  Ua = _constrain(Ua, 0.0f, voltage_limit);
+  Ub = _constrain(Ub, 0.0f, voltage_limit);
+  Uc = _constrain(Uc, 0.0f, voltage_limit);
+  // calculate duty cycle
+  // limited in [0,1]
+  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
+  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
+  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
+
+  // hardware specific writing
+  // hardware specific function - depending on driver and mcu
+  _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC);
+}

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/BLDCDriver3PWM.h

@@ -0,0 +1,65 @@
+#ifndef BLDCDriver3PWM_h
+#define BLDCDriver3PWM_h
+
+#include "../common/base_classes/BLDCDriver.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "hardware_api.h"
+
+/**
+ 3 pwm bldc driver class
+*/
+class BLDCDriver3PWM: public BLDCDriver
+{
+  public:
+    /**
+      BLDCDriver class constructor
+      @param phA A phase pwm pin
+      @param phB B phase pwm pin
+      @param phC C phase pwm pin
+      @param en1 enable pin (optional input)
+      @param en2 enable pin (optional input)
+      @param en3 enable pin (optional input)
+    */
+    BLDCDriver3PWM(int phA,int phB,int phC, int en1 = NOT_SET, int en2 = NOT_SET, int en3 = NOT_SET);
+    
+    /**  Motor hardware init function */
+  	int init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    // hardware variables
+  	int pwmA; //!< phase A pwm pin number
+  	int pwmB; //!< phase B pwm pin number
+  	int pwmC; //!< phase C pwm pin number
+    int enableA_pin; //!< enable pin number
+    int enableB_pin; //!< enable pin number
+    int enableC_pin; //!< enable pin number
+    bool enable_active_high = true;
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param Ua - phase A voltage
+     * @param Ub - phase B voltage
+     * @param Uc - phase C voltage
+    */
+    void setPwm(float Ua, float Ub, float Uc) override;
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param sc - phase A state : active / disabled ( high impedance )
+     * @param sb - phase B state : active / disabled ( high impedance )
+     * @param sa - phase C state : active / disabled ( high impedance )
+    */
+    virtual void setPhaseState(int sa, int sb, int sc) override;
+  private:
+        
+};
+
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/BLDCDriver6PWM.cpp

@@ -0,0 +1,86 @@
+#include "BLDCDriver6PWM.h"
+
+BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en){
+  // Pin initialization
+  pwmA_h = phA_h;
+  pwmB_h = phB_h;
+  pwmC_h = phC_h;
+  pwmA_l = phA_l;
+  pwmB_l = phB_l;
+  pwmC_l = phC_l;
+
+  // enable_pin pin
+  enable_pin = en;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+  // dead zone initial - 2%
+  dead_zone = 0.02f;
+
+}
+
+// enable motor driver
+void  BLDCDriver6PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high);
+    // set zero to PWM
+    setPwm(0, 0, 0);
+}
+
+// disable motor driver
+void BLDCDriver6PWM::disable()
+{
+  // set zero to PWM
+  setPwm(0, 0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( _isset(enable_pin) ) digitalWrite(enable_pin, !enable_active_high);
+
+}
+
+// init hardware pins
+int BLDCDriver6PWM::init() {
+
+  // PWM pins
+  pinMode(pwmA_h, OUTPUT);
+  pinMode(pwmB_h, OUTPUT);
+  pinMode(pwmC_h, OUTPUT);
+  pinMode(pwmA_l, OUTPUT);
+  pinMode(pwmB_l, OUTPUT);
+  pinMode(pwmC_l, OUTPUT);
+  if(_isset(enable_pin)) pinMode(enable_pin, OUTPUT);
+
+
+  // sanity check for the voltage limit configuration
+  if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
+
+  // configure 6pwm
+  // hardware specific function - depending on driver and mcu
+  int result = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
+  initialized = (result==0);
+  return result;
+}
+
+// Set voltage to the pwm pin
+void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) {
+  // limit the voltage in driver
+  Ua = _constrain(Ua, 0, voltage_limit);
+  Ub = _constrain(Ub, 0, voltage_limit);
+  Uc = _constrain(Uc, 0, voltage_limit);
+  // calculate duty cycle
+  // limited in [0,1]
+  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
+  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
+  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
+  // hardware specific writing
+  // hardware specific function - depending on driver and mcu
+  _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
+}
+
+
+// Set voltage to the pwm pin
+void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) {
+  // TODO implement disabling
+}

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/BLDCDriver6PWM.h

@@ -0,0 +1,67 @@
+#ifndef BLDCDriver6PWM_h
+#define BLDCDriver6PWM_h
+
+#include "../common/base_classes/BLDCDriver.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "hardware_api.h"
+
+/**
+ 6 pwm bldc driver class
+*/
+class BLDCDriver6PWM: public BLDCDriver
+{
+  public:
+    /**
+      BLDCDriver class constructor
+      @param phA_h A phase pwm pin
+      @param phA_l A phase pwm pin
+      @param phB_h B phase pwm pin
+      @param phB_l A phase pwm pin
+      @param phC_h C phase pwm pin
+      @param phC_l A phase pwm pin
+      @param en enable pin (optional input)
+    */
+    BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en = NOT_SET);
+    
+    /**  Motor hardware init function */
+  	int init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    // hardware variables
+  	int pwmA_h,pwmA_l; //!< phase A pwm pin number
+  	int pwmB_h,pwmB_l; //!< phase B pwm pin number
+  	int pwmC_h,pwmC_l; //!< phase C pwm pin number
+    int enable_pin; //!< enable pin number
+    bool enable_active_high = true;
+
+    float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1]
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param Ua - phase A voltage
+     * @param Ub - phase B voltage
+     * @param Uc - phase C voltage
+    */
+    void setPwm(float Ua, float Ub, float Uc) override;
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param sc - phase A state : active / disabled ( high impedance )
+     * @param sb - phase B state : active / disabled ( high impedance )
+     * @param sa - phase C state : active / disabled ( high impedance )
+    */
+    virtual void setPhaseState(int sa, int sb, int sc) override;
+
+  private:
+        
+};
+
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/StepperDriver2PWM.cpp

@@ -0,0 +1,107 @@
+#include "StepperDriver2PWM.h"
+
+StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, int en1, int en2){
+  // Pin initialization
+  pwm1 = _pwm1; // phase 1 pwm pin number
+  dir1a = _in1[0]; // phase 1 INA pin number
+  dir1b = _in1[1]; // phase 1 INB pin number
+  pwm2 = _pwm2; // phase 2 pwm pin number
+  dir2a = _in2[0]; // phase 2 INA pin number
+  dir2b = _in2[1]; // phase 2 INB pin number
+
+  // enable_pin pin
+  enable_pin1 = en1;
+  enable_pin2 = en2;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+}
+
+StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, int en1, int en2){
+  // Pin initialization
+  pwm1 = _pwm1; // phase 1 pwm pin number
+  dir1a = _dir1; // phase 1 direction pin
+  pwm2 = _pwm2; // phase 2 pwm pin number
+  dir2a = _dir2; // phase 2 direction pin
+  // these pins are not used
+  dir1b = NOT_SET;
+  dir2b = NOT_SET;
+
+  // enable_pin pin
+  enable_pin1 = en1;
+  enable_pin2 = en2;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+}
+
+// enable motor driver
+void  StepperDriver2PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH);
+    if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH);
+    // set zero to PWM
+    setPwm(0,0);
+}
+
+// disable motor driver
+void StepperDriver2PWM::disable()
+{
+  // set zero to PWM
+  setPwm(0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW);
+  if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW);
+
+}
+
+// init hardware pins
+int StepperDriver2PWM::init() {
+  // PWM pins
+  pinMode(pwm1, OUTPUT);
+  pinMode(pwm2, OUTPUT);
+  pinMode(dir1a, OUTPUT);
+  pinMode(dir2a, OUTPUT);
+  if( _isset(dir1b) ) pinMode(dir1b, OUTPUT);
+  if( _isset(dir2b) ) pinMode(dir2b, OUTPUT);
+
+  if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT);
+  if( _isset(enable_pin2) ) pinMode(enable_pin2, 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
+  _configure2PWM(pwm_frequency, pwm1, pwm2);
+  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
+  return 0;
+}
+
+
+// Set voltage to the pwm pin
+void StepperDriver2PWM::setPwm(float Ua, float Ub) {
+  float duty_cycle1(0.0f),duty_cycle2(0.0f);
+  // limit the voltage in driver
+  Ua = _constrain(Ua, -voltage_limit, voltage_limit);
+  Ub = _constrain(Ub, -voltage_limit, voltage_limit);
+  // hardware specific writing
+  duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f);
+  duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f);
+
+  // phase 1 direction
+  digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH);
+  if( _isset(dir1b) ) digitalWrite(dir1b, Ua >= 0 ? HIGH : LOW );
+  // phase 2 direction
+  digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH);
+  if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW );
+
+  // write to hardware
+  _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2);
+}

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/StepperDriver2PWM.h

@@ -0,0 +1,68 @@
+#ifndef STEPPER_DRIVER_2PWM_h
+#define STEPPER_DRIVER_2PWM_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"
+
+/**
+ 2 pwm stepper driver class
+*/
+class StepperDriver2PWM: public StepperDriver
+{
+  public:
+    /**
+      StepperMotor class constructor
+      @param pwm1  PWM1 phase pwm pin
+      @param in1   IN1A phase dir pin
+      @param pwm2  PWM2 phase pwm pin
+      @param in2   IN2A phase dir 
+      @param en1 enable pin phase 1 (optional input)
+      @param en2 enable pin phase 2 (optional input)
+    */
+    StepperDriver2PWM(int pwm1, int* in1, int pwm2, int* in2, int en1 = NOT_SET, int en2 = NOT_SET);
+    
+    /**
+      StepperMotor class constructor
+      @param pwm1  PWM1 phase pwm pin
+      @param dir1  DIR1 phase dir pin
+      @param pwm2  PWM2 phase pwm pin
+      @param dir2  DIR2 phase dir pin
+      @param en1 enable pin phase 1 (optional input)
+      @param en2 enable pin phase 2 (optional input)
+    */
+    StepperDriver2PWM(int pwm1, int dir1, int pwm2, int dir2, int en1 = NOT_SET, int en2 = NOT_SET);
+
+    /**  Motor hardware init function */
+  	int init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    // hardware variables
+    int pwm1; //!< phase 1 pwm pin number
+    int dir1a; //!< phase 1 INA pin number
+    int dir1b; //!< phase 1 INB pin number
+    int pwm2; //!< phase 2 pwm pin number
+    int dir2a; //!< phase 2 INA pin number
+    int dir2b; //!< phase 2 INB pin number
+    int enable_pin1; //!< enable pin number phase 1
+    int enable_pin2; //!< enable pin number phase 2
+
+    /** 
+     * 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

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/StepperDriver4PWM.cpp

@@ -0,0 +1,81 @@
+#include "StepperDriver4PWM.h"
+
+StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1, int en2){
+  // Pin initialization
+  pwm1A = ph1A;
+  pwm1B = ph1B;
+  pwm2A = ph2A;
+  pwm2B = ph2B;
+
+  // enable_pin pin
+  enable_pin1 = en1;
+  enable_pin2 = en2;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+}
+
+// enable motor driver
+void  StepperDriver4PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH);
+    if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH);
+    // set zero to PWM
+    setPwm(0,0);
+}
+
+// disable motor driver
+void StepperDriver4PWM::disable()
+{
+  // set zero to PWM
+  setPwm(0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW);
+  if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW);
+
+}
+
+// init hardware pins
+int StepperDriver4PWM::init() {
+
+  // PWM pins
+  pinMode(pwm1A, OUTPUT);
+  pinMode(pwm1B, OUTPUT);
+  pinMode(pwm2A, OUTPUT);
+  pinMode(pwm2B, OUTPUT);
+  if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT);
+  if( _isset(enable_pin2) ) pinMode(enable_pin2, 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
+  _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B);
+  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
+  return 0;
+}
+
+
+// Set voltage to the pwm pin
+void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) {
+  float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(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 = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f);
+  else
+    duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f);
+
+  if( Ubeta > 0 )
+    duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
+  else
+    duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
+  // write to hardware
+  _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B);
+}

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/StepperDriver4PWM.h

@@ -0,0 +1,55 @@
+#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"
+
+/**
+ 4 pwm stepper driver class
+*/
+class StepperDriver4PWM: 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 en1 enable pin phase 1 (optional input)
+      @param en2 enable pin phase 2 (optional input)
+    */
+    StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B, int en1 = NOT_SET, int en2 = NOT_SET);
+    
+    /**  Motor hardware init function */
+  	int init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    // 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 enable_pin1; //!< enable pin number phase 1
+    int enable_pin2; //!< enable pin number phase 2
+
+    /** 
+     * 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

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_api.h

@@ -0,0 +1,123 @@
+#ifndef HARDWARE_UTILS_DRIVER_H
+#define HARDWARE_UTILS_DRIVER_H
+
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+
+/** 
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 2PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pinA pinA bldc driver
+ * @param pinB pinB bldc driver
+ */
+void _configure2PWM(long pwm_frequency, const int pinA, const int pinB);
+
+/** 
+ * Configuring PWM frequency, resolution and alignment
+ * - BLDC driver - 3PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pinA pinA bldc driver
+ * @param pinB pinB bldc driver
+ * @param pinC pinC bldc driver
+ */
+void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC);
+
+/** 
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 4PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pin1A pin1A stepper driver
+ * @param pin1B pin1B stepper driver
+ * @param pin2A pin2A stepper driver
+ * @param pin2B pin2B stepper driver
+ */
+void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B);
+
+/** 
+ * Configuring PWM frequency, resolution and alignment
+ * - BLDC driver - 6PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param dead_zone  duty cycle protection zone [0, 1] - both low and high side low - if applicable
+ * @param pinA_h pinA high-side bldc driver 
+ * @param pinA_l pinA low-side bldc driver 
+ * @param pinB_h pinA high-side bldc driver 
+ * @param pinB_l pinA low-side bldc driver 
+ * @param pinC_h pinA high-side bldc driver 
+ * @param pinC_l pinA low-side bldc driver 
+ * 
+ * @return 0 if config good, -1 if failed
+ */
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l);
+
+/** 
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - Stepper driver - 2PWM setting
+ * - hardware specific
+ * 
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param pinA  phase A hardware pin number
+ * @param pinB  phase B hardware pin number
+ */ 
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB);
+
+/** 
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - BLDC driver - 3PWM setting
+ * - hardware specific
+ * 
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param dc_c  duty cycle phase C [0, 1]
+ * @param pinA  phase A hardware pin number
+ * @param pinB  phase B hardware pin number
+ * @param pinC  phase C hardware pin number
+ */ 
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC);
+
+/** 
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - Stepper driver - 4PWM setting
+ * - hardware specific
+ * 
+ * @param dc_1a  duty cycle phase 1A [0, 1]
+ * @param dc_1b  duty cycle phase 1B [0, 1]
+ * @param dc_2a  duty cycle phase 2A [0, 1]
+ * @param dc_2b  duty cycle phase 2B [0, 1]
+ * @param pin1A  phase 1A hardware pin number
+ * @param pin1B  phase 1B hardware pin number
+ * @param pin2A  phase 2A hardware pin number
+ * @param pin2B  phase 2B hardware pin number
+ */ 
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B);
+
+
+/** 
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - BLDC driver - 6PWM setting
+ * - hardware specific
+ * 
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param dc_c  duty cycle phase C [0, 1]
+ * @param dead_zone  duty cycle protection zone [0, 1] - both low and high side low
+ * @param pinA_h  phase A high-side hardware pin number
+ * @param pinA_l  phase A low-side hardware pin number
+ * @param pinB_h  phase B high-side hardware pin number
+ * @param pinB_l  phase B low-side hardware pin number
+ * @param pinC_h  phase C high-side hardware pin number
+ * @param pinC_l  phase C low-side hardware pin number
+ * 
+ */ 
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l);
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/atmega2560_mcu.cpp

@@ -0,0 +1,158 @@
+#include "../hardware_api.h"
+
+#if defined(__AVR_ATmega2560__)
+
+// set pwm frequency to 32KHz
+void _pinHighFrequency(const int pin){
+  //  High PWM frequency
+  //  https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
+  //  https://forum.arduino.cc/index.php?topic=72092.0
+  if (pin == 13 || pin == 4  ) {
+      TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+      TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
+  }
+  else if (pin == 12 || pin == 11 )
+      TCCR1B = ((TCCR1B & 0b11111000) | 0x01);     // set prescaler to 1
+  else if (pin == 10 || pin == 9 )
+      TCCR2B = ((TCCR2B & 0b11111000) | 0x01);     // set prescaler to 1
+  else if (pin == 5 || pin == 3 || pin == 2)
+      TCCR3B = ((TCCR2B & 0b11111000) | 0x01);    // set prescaler to 1
+  else if (pin == 8 || pin == 7 || pin == 6)
+      TCCR4B = ((TCCR4B & 0b11111000) | 0x01);    // set prescaler to 1
+  else if (pin == 44 || pin == 45 || pin == 46)
+      TCCR5B = ((TCCR5B & 0b11111000) | 0x01);    // set prescaler to 1
+
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+  _pinHighFrequency(pinC);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+  analogWrite(pinC, 255.0f*dc_c);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pin1A);
+  _pinHighFrequency(pin1B);
+  _pinHighFrequency(pin2A);
+  _pinHighFrequency(pin2B);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pin1A, 255.0f*dc_1a);
+  analogWrite(pin1B, 255.0f*dc_1b);
+  analogWrite(pin2A, 255.0f*dc_2a);
+  analogWrite(pin2B, 255.0f*dc_2b);
+}
+
+
+// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
+// supports Arudino/ATmega2560
+int _configureComplementaryPair(int pinH, int pinL) {
+  if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){
+    // configure the pwm phase-corrected mode
+    TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 13 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
+    else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
+    // set prescaler to 1 - 32kHz freq
+    TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
+  }else if( (pinH == 11 && pinL == 12 ) || (pinH == 12 && pinL == 11 ) ){
+    // set prescaler to 1 - 32kHz freq
+    TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 11 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
+    else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
+  }else if((pinH == 10 && pinL == 9 ) || (pinH == 9 && pinL == 10 ) ){
+    // set prescaler to 1 - 32kHz freq
+    TCCR2B = ((TCCR2B & 0b11111000) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 10 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ;
+    else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ;
+  }else{
+    return -1;
+  }
+  return 0;
+}
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+// supports Arudino/ATmega328
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+  //  High PWM frequency
+  // - always max 32kHz
+  int ret_flag = 0;
+  ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
+  ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
+  ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
+  return ret_flag; // returns -1 if not well configured
+}
+
+// function setting the
+void _setPwmPair(int pinH, int pinL, float val, int dead_time)
+{
+  int pwm_h = _constrain(val-dead_time/2,0,255);
+  int pwm_l = _constrain(val+dead_time/2,0,255);
+
+  analogWrite(pinH, pwm_h);
+  if(pwm_l == 255 || pwm_l == 0)
+    digitalWrite(pinL, pwm_l ? LOW : HIGH);
+  else
+    analogWrite(pinL, pwm_l);
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+//  - BLDC driver - 6PWM setting
+//  - hardware specific
+// supports Arudino/ATmega328
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+  _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0);
+  _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0);
+  _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0);
+}
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/atmega328_mcu.cpp

@@ -0,0 +1,156 @@
+#include "../hardware_api.h"
+
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)
+
+// set pwm frequency to 32KHz
+void _pinHighFrequency(const int pin){
+  //  High PWM frequency
+  //  https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328
+  if (pin == 5 || pin == 6  ) {
+      TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+      TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
+  }
+  if (pin == 9 || pin == 10 )
+      TCCR1B = ((TCCR1B & 0b11111000) | 0x01);     // set prescaler to 1
+  if (pin == 3 || pin == 11)
+      TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1
+
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+// supports Arudino/ATmega328
+void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+  _UNUSED(pwm_frequency);
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+// supports Arudino/ATmega328
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  _UNUSED(pwm_frequency);
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+  _pinHighFrequency(pinC);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+  analogWrite(pinC, 255.0f*dc_c);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+// supports Arudino/ATmega328
+void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pin1A);
+  _pinHighFrequency(pin1B);
+  _pinHighFrequency(pin2A);
+  _pinHighFrequency(pin2B);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pin1A, 255.0f*dc_1a);
+  analogWrite(pin1B, 255.0f*dc_1b);
+  analogWrite(pin2A, 255.0f*dc_2a);
+  analogWrite(pin2B, 255.0f*dc_2b);
+}
+
+
+// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
+int _configureComplementaryPair(int pinH, int pinL) {
+  if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){
+    // configure the pwm phase-corrected mode
+    TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
+    else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
+    // set prescaler to 1 - 32kHz freq
+    TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
+  }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){
+    // set prescaler to 1 - 32kHz freq
+    TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
+    else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
+  }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){
+    // set prescaler to 1 - 32kHz freq
+    TCCR2B = ((TCCR2B & 0b11111000) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ;
+    else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ;
+  }else{
+    return -1;
+  }
+  return 0;
+}
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+// supports Arudino/ATmega328
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+  _UNUSED(pwm_frequency);
+  _UNUSED(dead_zone);
+  //  High PWM frequency
+  // - always max 32kHz
+  int ret_flag = 0;
+  ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
+  ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
+  ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
+  return ret_flag; // returns -1 if not well configured
+}
+
+// function setting the
+void _setPwmPair(int pinH, int pinL, float val, int dead_time)
+{
+  int pwm_h = _constrain(val-dead_time/2,0,255);
+  int pwm_l = _constrain(val+dead_time/2,0,255);
+
+  analogWrite(pinH, pwm_h);
+  if(pwm_l == 255 || pwm_l == 0)
+    digitalWrite(pinL, pwm_l ? LOW : HIGH);
+  else
+    analogWrite(pinL, pwm_l);
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+//  - BLDC driver - 6PWM setting
+//  - hardware specific
+// supports Arudino/ATmega328
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+  _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0);
+  _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0);
+  _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0);
+}
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/atmega32u4_mcu.cpp

@@ -0,0 +1,172 @@
+
+#include "../hardware_api.h"
+
+#if defined(__AVR_ATmega32U4__)
+
+// set pwm frequency to 32KHz
+void _pinHighFrequency(const int pin){
+  //  High PWM frequency
+  // reference： http://r6500.blogspot.com/2014/12/fast-pwm-on-arduino-leonardo.html
+  if (pin == 3 || pin == 11 ) {
+    TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+    TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
+  }
+  else if (pin == 9 || pin == 10 )
+    TCCR1B = ((TCCR1B & 0b11111000) | 0x01);     // set prescaler to 1
+  else if (pin == 5 )
+    TCCR3B = ((TCCR3B & 0b11111000) | 0x01);     // set prescaler to 1
+  else if ( pin == 6 || pin == 13 ) {  // a bit more complicated 10 bit timer
+    // PLL Configuration
+    PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz
+    TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz
+    TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+    
+    if (pin == 6) TCCR4A = 0x82; // activate channel A - pin 13
+    else if (pin == 13) TCCR4C |= 0x09;  // Activate channel D - pin 6
+  }  
+
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+  _pinHighFrequency(pinC);
+}
+
+// function setting the pwm duty cycle to the hardware 
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0*dc_a);
+  analogWrite(pinB, 255.0*dc_b);
+}
+
+// function setting the pwm duty cycle to the hardware 
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0*dc_a);
+  analogWrite(pinB, 255.0*dc_b);
+  analogWrite(pinC, 255.0*dc_c);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pin1A);
+  _pinHighFrequency(pin1B);
+  _pinHighFrequency(pin2A);
+  _pinHighFrequency(pin2B); 
+}
+
+// function setting the pwm duty cycle to the hardware  
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pin1A, 255.0*dc_1a);
+  analogWrite(pin1B, 255.0*dc_1b);
+  analogWrite(pin2A, 255.0*dc_2a);
+  analogWrite(pin2B, 255.0*dc_2b);
+}
+
+
+
+
+// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
+int _configureComplementaryPair(int pinH, int pinL) {
+  if( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){
+    // configure the pwm phase-corrected mode
+    TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 11 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
+    else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
+    // set prescaler to 1 - 32kHz freq
+    TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
+  }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){
+    // set prescaler to 1 - 32kHz freq
+    TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
+    else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
+  }else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){
+    // PLL Configuration
+    PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz
+    TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz
+    TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+
+    // configure complementary pwm on low side
+    if(pinH == 13 ){
+      TCCR4A = 0x82; // activate channel A - pin 13
+      TCCR4C |= 0x0d;  // Activate complementary channel D - pin 6
+    }else {
+      TCCR4C |= 0x09;  // Activate channel D - pin 6
+      TCCR4A = 0xc2; // activate complementary channel A - pin 13
+    }
+  }else{
+    return -1;
+  }
+  return 0;
+}
+
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+  _UNUSED(pwm_frequency);
+  _UNUSED(dead_zone);
+  //  High PWM frequency
+  // - always max 32kHz
+  int ret_flag = 0;
+  ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
+  ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
+  ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
+  return ret_flag; // returns -1 if not well configured
+
+
+// function setting the 
+void _setPwmPair(int pinH, int pinL, float val, int dead_time)
+{
+  int pwm_h = _constrain(val-dead_time/2,0,255);
+  int pwm_l = _constrain(val+dead_time/2,0,255);
+  
+  analogWrite(pinH, pwm_h);
+  if(pwm_l == 255 || pwm_l == 0)
+    digitalWrite(pinL, pwm_l ? LOW : HIGH);
+  else
+    analogWrite(pinL, pwm_l);
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+//  - BLDC driver - 6PWM setting
+//  - hardware specific
+// supports Arudino/ATmega328 
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+  _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0);
+  _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0);
+  _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0);
+}
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/due_mcu.cpp

@@ -0,0 +1,406 @@
+#include "../hardware_api.h" 
+
+#if defined(__arm__) && defined(__SAM3X8E__)
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+#define _PWM_RES_MIN 255 // 50khz
+
+// pwm frequency and max duty cycle
+static unsigned long _pwm_frequency;
+static int _max_pwm_value = 1023;
+
+// array mapping the timer values to the interrupt handlers
+static IRQn_Type irq_type[] = {TC0_IRQn, TC0_IRQn, TC1_IRQn, TC1_IRQn, TC2_IRQn, TC2_IRQn, TC3_IRQn, TC3_IRQn, TC4_IRQn, TC4_IRQn, TC5_IRQn, TC5_IRQn, TC6_IRQn, TC6_IRQn, TC7_IRQn, TC7_IRQn, TC8_IRQn, TC8_IRQn};
+// current counter values 
+static volatile uint32_t pwm_counter_vals[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+
+// variables copied from wiring_analog.cpp for arduino due
+static uint8_t PWMEnabled = 0;
+static uint8_t TCChanEnabled[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+static const uint32_t channelToChNo[] = { 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2 };
+static const uint32_t channelToAB[]   = { 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0 };
+static Tc *channelToTC[] = {
+  TC0, TC0, TC0, TC0, TC0, TC0,
+  TC1, TC1, TC1, TC1, TC1, TC1,
+  TC2, TC2, TC2, TC2, TC2, TC2 };
+static const uint32_t channelToId[] = { 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8 };
+
+
+// function setting the CMR register
+static void TC_SetCMR_ChannelA(Tc *tc, uint32_t chan, uint32_t v){  tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xFFF0FFFF) | v;}
+static void TC_SetCMR_ChannelB(Tc *tc, uint32_t chan, uint32_t v){ tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xF0FFFFFF) | v; }
+
+
+// function which starts and syncs the timers 
+// if the pin is the true PWM pin this function does not do anything
+void syncTimers(uint32_t ulPin1,uint32_t ulPin2, uint32_t ulPin3 = -1, uint32_t ulPin4 = -1){
+  uint32_t chNo1,chNo2,chNo3,chNo4;
+  Tc *chTC1 = nullptr,*chTC2 = nullptr,*chTC3 = nullptr,*chTC4 = nullptr;
+
+  // configure timer channel for the first pin if it is a timer pin
+  uint32_t attr = g_APinDescription[ulPin1].ulPinAttribute;
+  if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
+    ETCChannel channel1 = g_APinDescription[ulPin1].ulTCChannel;
+    chNo1 = channelToChNo[channel1];
+    chTC1 = channelToTC[channel1];
+    TCChanEnabled[channelToId[channel1]] = 1;
+  }
+    
+  // configure timer channel for the first pin if it is a timer pin
+  attr = g_APinDescription[ulPin2].ulPinAttribute;
+  if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
+    ETCChannel channel2 = g_APinDescription[ulPin2].ulTCChannel;
+    chNo2 = channelToChNo[channel2];
+    chTC2 = channelToTC[channel2];
+    TCChanEnabled[channelToId[channel2]] = 1;
+  }
+  if(ulPin3 > 0 ){
+    // configure timer channel for the first pin if it is a timer pin
+    attr = g_APinDescription[ulPin3].ulPinAttribute;
+    if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
+      ETCChannel channel3 = g_APinDescription[ulPin3].ulTCChannel;
+      chNo3 = channelToChNo[channel3];
+      chTC3 = channelToTC[channel3];
+      TCChanEnabled[channelToId[channel3]] = 1;
+    }
+  }
+  if(ulPin4  > 0 ){
+    // configure timer channel for the first pin if it is a timer pin
+    attr = g_APinDescription[ulPin4].ulPinAttribute;
+    if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
+      ETCChannel channel4 = g_APinDescription[ulPin4].ulTCChannel;
+      chNo4 = channelToChNo[channel4];
+      chTC4 = channelToTC[channel4];
+      TCChanEnabled[channelToId[channel4]] = 1;
+    }
+  }
+  // start timers and make them synced
+  if(chTC1){ 
+    TC_Start(chTC1, chNo1);
+    chTC1->TC_BCR = TC_BCR_SYNC;
+  }
+  if(chTC2){  
+    TC_Start(chTC2, chNo2);
+    chTC2->TC_BCR = TC_BCR_SYNC;
+  }
+  if(chTC3 && ulPin3){ 
+    TC_Start(chTC3, chNo3);
+    chTC3->TC_BCR = TC_BCR_SYNC;
+  }
+  if(chTC4 && ulPin4){ 
+    TC_Start(chTC4, chNo4);
+    chTC4->TC_BCR = TC_BCR_SYNC;
+  }
+}
+
+// function configuring the pwm frequency for given pin
+// possible to supply the pwm pin and the timer pin 
+void initPWM(uint32_t ulPin, uint32_t pwm_freq){
+  // check which pin type
+  uint32_t attr = g_APinDescription[ulPin].ulPinAttribute;
+  if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm pin
+
+    if (!PWMEnabled) {
+      // PWM Startup code
+      pmc_enable_periph_clk(PWM_INTERFACE_ID);
+			// this function does not work too well - I'll rewrite it
+      // PWMC_ConfigureClocks(PWM_FREQUENCY * _max_pwm_value, 0, VARIANT_MCK);
+
+      // finding the divisors an prescalers form FindClockConfiguration function
+      uint32_t divisors[11] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024};
+      uint8_t divisor = 0;
+      uint32_t prescaler;
+
+      /* Find prescaler and divisor values */
+      prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value);
+      while ((prescaler > 255) && (divisor < 11)) {
+          divisor++;
+          prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value);
+      }
+      // update the divisor*prescaler value 
+      prescaler = prescaler | (divisor << 8);
+
+      // now calculate the real resolution timer period necessary (pwm resolution)
+      // pwm_res = bus_freq / (pwm_freq * (prescaler))
+      _max_pwm_value = (double)VARIANT_MCK / (double)pwm_freq / (double)(prescaler);
+      // set the prescaler value
+      PWM->PWM_CLK = prescaler;
+
+      PWMEnabled = 1;
+    }
+
+    uint32_t chan = g_APinDescription[ulPin].ulPWMChannel;
+    if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) {
+      // Setup PWM for this pin
+      PIO_Configure(g_APinDescription[ulPin].pPort,
+          g_APinDescription[ulPin].ulPinType,
+          g_APinDescription[ulPin].ulPin,
+          g_APinDescription[ulPin].ulPinConfiguration);
+      // PWM_CMR_CALG - center align
+      // PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, PWM_CMR_CALG, 0);
+      PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0);
+      PWMC_SetPeriod(PWM_INTERFACE, chan, _max_pwm_value);
+      PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0);
+      PWMC_EnableChannel(PWM_INTERFACE, chan);
+      g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM;
+    }
+    return;
+  }
+
+  if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin
+    // We use MCLK/2 as clock.
+    const uint32_t TC = VARIANT_MCK / 2 / pwm_freq ;
+    // Setup Timer for this pin
+    ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
+    uint32_t chNo = channelToChNo[channel];
+    uint32_t chA  = channelToAB[channel];
+    Tc *chTC = channelToTC[channel];
+    uint32_t interfaceID = channelToId[channel];
+
+      if (!TCChanEnabled[interfaceID]) {
+        pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID);
+        TC_Configure(chTC, chNo,
+          TC_CMR_TCCLKS_TIMER_CLOCK1 |
+          TC_CMR_WAVE |         // Waveform mode
+          TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC
+          TC_CMR_EEVT_XC0 |     // Set external events from XC0 (this setup TIOB as output)
+          TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR |
+          TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
+        TC_SetRC(chTC, chNo, TC);
+      } 
+
+    // disable the counter on start
+    if (chA){
+      TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
+    }else{
+      TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET);
+    }
+    // configure input-ouput structure
+    if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) {
+      PIO_Configure(g_APinDescription[ulPin].pPort,
+          g_APinDescription[ulPin].ulPinType,
+          g_APinDescription[ulPin].ulPin,
+          g_APinDescription[ulPin].ulPinConfiguration);
+      g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM;
+    }
+    // enable interrupts
+    chTC->TC_CHANNEL[chNo].TC_IER = TC_IER_CPAS       // interrupt on RA compare match
+                          | TC_IER_CPBS         // interrupt on RB compare match
+                          | TC_IER_CPCS;        // interrupt on RC compare match
+    chTC->TC_CHANNEL[chNo].TC_IDR = ~TC_IER_CPAS       // interrupt on RA compare match
+                            & ~TC_IER_CPBS         // interrupt on RB compare match
+                            & ~ TC_IER_CPCS;        // interrupt on RC compare match
+    // enable interrupts for this timer
+    NVIC_EnableIRQ(irq_type[channel]);                  
+    return;
+  }
+}
+
+// pwm setting function
+// it sets the duty cycle for pwm pin or timer pin
+void setPwm(uint32_t ulPin, uint32_t ulValue) {
+  // check pin type
+  uint32_t attr = g_APinDescription[ulPin].ulPinAttribute;
+  if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm
+    uint32_t chan = g_APinDescription[ulPin].ulPWMChannel;
+    PWMC_SetDutyCycle(PWM_INTERFACE, chan, ulValue);
+    return;
+  }
+
+  if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin
+    // get the timer variables 
+    ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
+    Tc *chTC = channelToTC[channel];
+    uint32_t chNo = channelToChNo[channel];
+    if(!ulValue) {
+      // if the value 0 disable counter
+      if (channelToAB[channel])
+        TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR);
+      else
+        TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
+    }else{
+      // if the value not zero
+      // calculate clock
+      const uint32_t TC = VARIANT_MCK / 2 / _pwm_frequency;
+      // Map value to Timer ranges 0..max_duty_cycle => 0..TC
+      // Setup Timer for this pin
+      ulValue = ulValue * TC ;
+      pwm_counter_vals[channel] = ulValue / _max_pwm_value;
+      // enable counter
+      if (channelToAB[channel])
+        TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
+      else
+        TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET);
+    }
+    
+    return;
+  }
+}
+
+// interrupt handlers for seamless pwm duty-cycle setting
+void TC0_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC0, 0);
+  // update the counters
+  if(pwm_counter_vals[0]) TC_SetRA(TC0, 0, pwm_counter_vals[0]);
+  if(pwm_counter_vals[1]) TC_SetRB(TC0, 0, pwm_counter_vals[1]);
+}
+
+void TC1_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC0, 1);
+  // update the counters
+  if(pwm_counter_vals[2]) TC_SetRA(TC0, 1, pwm_counter_vals[2]);
+  if(pwm_counter_vals[3]) TC_SetRB(TC0, 1, pwm_counter_vals[3]);
+}
+
+void TC2_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC0, 2);
+  // update the counters
+  if(pwm_counter_vals[4]) TC_SetRA(TC0, 2, pwm_counter_vals[4]);
+  if(pwm_counter_vals[5]) TC_SetRB(TC0, 2, pwm_counter_vals[5]);
+}
+void TC3_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC1, 0);
+  // update the counters
+  if(pwm_counter_vals[6]) TC_SetRA(TC1, 0, pwm_counter_vals[6]);
+  if(pwm_counter_vals[7]) TC_SetRB(TC1, 0, pwm_counter_vals[7]);
+}
+
+void TC4_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC1, 1);
+  // update the counters
+  if(pwm_counter_vals[8]) TC_SetRA(TC1, 1, pwm_counter_vals[8]);
+  if(pwm_counter_vals[9]) TC_SetRB(TC1, 1, pwm_counter_vals[9]);
+}
+
+void TC5_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC1, 2);
+  // update the counters
+  if(pwm_counter_vals[10]) TC_SetRA(TC1, 2, pwm_counter_vals[10]);
+  if(pwm_counter_vals[11]) TC_SetRB(TC1, 2, pwm_counter_vals[11]);
+}
+void TC6_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC2, 0);
+  // update the counters
+  if(pwm_counter_vals[12]) TC_SetRA(TC2, 0, pwm_counter_vals[12]);
+  if(pwm_counter_vals[13]) TC_SetRB(TC2, 0, pwm_counter_vals[13]);
+}
+
+void TC7_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC2, 1);
+  // update the counters
+  if(pwm_counter_vals[14]) TC_SetRA(TC2, 1, pwm_counter_vals[14]);
+  if(pwm_counter_vals[15]) TC_SetRB(TC2, 1, pwm_counter_vals[15]);
+}
+
+void TC8_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC2, 2);
+  // update the counters
+  if(pwm_counter_vals[16]) TC_SetRA(TC2, 2, pwm_counter_vals[16]);
+  if(pwm_counter_vals[17]) TC_SetRB(TC2, 2, pwm_counter_vals[17]);
+}
+
+// implementation of the hardware_api.cpp 
+// ---------------------------------------------------------------------------------------------------------------------------------
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware specific
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // save the pwm frequency
+  _pwm_frequency = pwm_frequency;
+  // cinfigure pwm pins
+  initPWM(pinA, _pwm_frequency);
+  initPWM(pinB, _pwm_frequency);
+  initPWM(pinC, _pwm_frequency);
+  // sync the timers if possible
+  syncTimers(pinA, pinB, pinC);
+}
+
+
+// Configuring PWM frequency, resolution and alignment
+//- Stepper driver - 2PWM setting
+// - hardware specific
+void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+  if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // save the pwm frequency
+  _pwm_frequency = pwm_frequency;
+  // cinfigure pwm pins
+  initPWM(pinA, _pwm_frequency);
+  initPWM(pinB, _pwm_frequency);
+  // sync the timers if possible
+  syncTimers(pinA, pinB);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // save the pwm frequency
+  _pwm_frequency = pwm_frequency;
+  // cinfigure pwm pins
+  initPWM(pinA, _pwm_frequency);
+  initPWM(pinB, _pwm_frequency);
+  initPWM(pinC, _pwm_frequency);
+  initPWM(pinD, _pwm_frequency);
+  // sync the timers if possible
+  syncTimers(pinA, pinB, pinC, pinD);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // transform duty cycle from [0,1] to [0,_max_pwm_value]
+  setPwm(pinA, _max_pwm_value*dc_a);
+  setPwm(pinB, _max_pwm_value*dc_b);
+  setPwm(pinC, _max_pwm_value*dc_c);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // transform duty cycle from [0,1] to [0,_max_pwm_value]
+  setPwm(pin1A, _max_pwm_value*dc_1a);
+  setPwm(pin1B, _max_pwm_value*dc_1b);
+  setPwm(pin2A, _max_pwm_value*dc_2a);
+  setPwm(pin2B, _max_pwm_value*dc_2b);
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - Stepper driver - 2PWM setting
+// - hardware specific
+void  _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // transform duty cycle from [0,1] to [0,_max_pwm_value]
+  setPwm(pinA, _max_pwm_value*dc_a);
+  setPwm(pinB, _max_pwm_value*dc_b);
+}
+
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/esp32_ledc_mcu.cpp

@@ -0,0 +1,204 @@
+#include "../hardware_api.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) 
+
+#include "driver/ledc.h"
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution
+#define _PWM_RES_BIT 10  // 10 bir resolution
+#define _PWM_RES 1023 // 2^10-1 = 1023-1
+
+
+// empty motor slot
+#define _EMPTY_SLOT -20
+#define _TAKEN_SLOT -21
+
+// figure out how many ledc channels are avaible
+// esp32   - 2x8=16
+// esp32s2 - 8
+// esp32c3 - 6
+#include "soc/soc_caps.h"
+#ifdef SOC_LEDC_SUPPORT_HS_MODE
+#define LEDC_CHANNELS           (SOC_LEDC_CHANNEL_NUM<<1)
+#else
+#define LEDC_CHANNELS           (SOC_LEDC_CHANNEL_NUM)
+#endif
+
+// current channel stack index
+// support for multiple motors
+// esp32 has 16 channels
+// esp32s2 has 8 channels
+// esp32c3 has 6 channels
+int channel_index = 0;
+
+// slot for the 3pwm bldc motors
+typedef struct {
+  int pinID;
+  int ch1;
+  int ch2;
+  int ch3;
+} bldc_3pwm_motor_slots_t;
+
+// slot for the 2pwm stepper motors
+typedef struct {
+  int pinID;
+  int ch1;
+  int ch2;
+} stepper_2pwm_motor_slots_t;
+
+// slot for the 4pwm stepper motors
+typedef struct {
+  int pinID;
+  int ch1;
+  int ch2;
+  int ch3;
+  int ch4;
+} stepper_4pwm_motor_slots_t;
+
+
+// define motor slots array
+bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = {
+  {_EMPTY_SLOT, 0,0,0}, // 1st motor
+  {_EMPTY_SLOT, 0,0,0}, // 2nd motor
+  {_EMPTY_SLOT, 0,0,0}, // 3st motor // esp32s2 & esp32
+  {_EMPTY_SLOT, 0,0,0}, // 4nd motor // esp32 only
+};
+stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4]={
+  {_EMPTY_SLOT, 0,0}, // 1st motor
+  {_EMPTY_SLOT, 0,0}, // 2nd motor
+  {_EMPTY_SLOT, 0,0}, // 3rd motor
+  {_EMPTY_SLOT, 0,0}  // 4th motor - esp32s2 and esp32
+};
+stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[4]={
+  {_EMPTY_SLOT, 0,0,0,0}, // 1st motor
+  {_EMPTY_SLOT, 0,0,0,0}, // 2nd motor - esp32s2 and esp32
+  {_EMPTY_SLOT, 0,0,0,0}, // 3st motor - only esp32
+  {_EMPTY_SLOT, 0,0,0,0}, // 4st motor - only esp32
+};
+
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin, const int channel){
+  ledcSetup(channel, freq, _PWM_RES_BIT );
+  ledcAttachPin(pin, channel);
+}
+
+void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+
+  // check if enough channels available
+  if ( channel_index + 2 >= LEDC_CHANNELS ) return;
+
+  stepper_2pwm_motor_slots_t m_slot = {};
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  for(int slot_num = 0; slot_num < 4; slot_num++){
+    if(esp32_stepper_2pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_stepper_2pwm_motor_slots[slot_num].pinID = pinA;
+      esp32_stepper_2pwm_motor_slots[slot_num].ch1 = channel_index++;
+      esp32_stepper_2pwm_motor_slots[slot_num].ch2 = channel_index++;
+      m_slot = esp32_stepper_2pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+
+  _setHighFrequency(pwm_frequency, pinA, m_slot.ch1);
+  _setHighFrequency(pwm_frequency, pinB, m_slot.ch2);
+}
+
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+
+  // check if enough channels available
+  if ( channel_index + 3 >= LEDC_CHANNELS ) return;
+
+  bldc_3pwm_motor_slots_t m_slot = {};
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  for(int slot_num = 0; slot_num < 4; slot_num++){
+    if(esp32_bldc_3pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_bldc_3pwm_motor_slots[slot_num].pinID = pinA;
+      esp32_bldc_3pwm_motor_slots[slot_num].ch1 = channel_index++;
+      esp32_bldc_3pwm_motor_slots[slot_num].ch2 = channel_index++;
+      esp32_bldc_3pwm_motor_slots[slot_num].ch3 = channel_index++;
+      m_slot = esp32_bldc_3pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+
+  _setHighFrequency(pwm_frequency, pinA, m_slot.ch1);
+  _setHighFrequency(pwm_frequency, pinB, m_slot.ch2);
+  _setHighFrequency(pwm_frequency, pinC, m_slot.ch3);
+}
+
+void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+
+  // check if enough channels available
+  if ( channel_index + 4 >= LEDC_CHANNELS ) return;
+
+
+  stepper_4pwm_motor_slots_t m_slot = {};
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  for(int slot_num = 0; slot_num < 4; slot_num++){
+    if(esp32_stepper_4pwm_motor_slots[slot_num].pinID == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_stepper_4pwm_motor_slots[slot_num].pinID = pinA;
+      esp32_stepper_4pwm_motor_slots[slot_num].ch1 = channel_index++;
+      esp32_stepper_4pwm_motor_slots[slot_num].ch2 = channel_index++;
+      esp32_stepper_4pwm_motor_slots[slot_num].ch3 = channel_index++;
+      esp32_stepper_4pwm_motor_slots[slot_num].ch4 = channel_index++;
+      m_slot = esp32_stepper_4pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+
+  _setHighFrequency(pwm_frequency, pinA, m_slot.ch1);
+  _setHighFrequency(pwm_frequency, pinB, m_slot.ch2);
+  _setHighFrequency(pwm_frequency, pinC, m_slot.ch3);
+  _setHighFrequency(pwm_frequency, pinD, m_slot.ch4);
+}
+
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+// determine which motor slot is the motor connected to
+  for(int i = 0; i < 4; i++){
+    if(esp32_stepper_2pwm_motor_slots[i].pinID == pinA){ // if motor slot found
+      ledcWrite(esp32_stepper_2pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_a, 0, _PWM_RES));
+      ledcWrite(esp32_stepper_2pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_b, 0, _PWM_RES));
+      break;
+    }
+  }
+}
+
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // determine which motor slot is the motor connected to
+  for(int i = 0; i < 4; i++){
+    if(esp32_bldc_3pwm_motor_slots[i].pinID == pinA){ // if motor slot found
+      ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_a, 0, _PWM_RES));
+      ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_b, 0, _PWM_RES));
+      ledcWrite(esp32_bldc_3pwm_motor_slots[i].ch3, _constrain(_PWM_RES*dc_c, 0, _PWM_RES));
+      break;
+    }
+  }
+}
+
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // determine which motor slot is the motor connected to
+  for(int i = 0; i < 4; i++){
+    if(esp32_stepper_4pwm_motor_slots[i].pinID == pin1A){ // if motor slot found
+      ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch1, _constrain(_PWM_RES*dc_1a, 0, _PWM_RES));
+      ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch2, _constrain(_PWM_RES*dc_1b, 0, _PWM_RES));
+      ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch3, _constrain(_PWM_RES*dc_2a, 0, _PWM_RES));
+      ledcWrite(esp32_stepper_4pwm_motor_slots[i].ch4, _constrain(_PWM_RES*dc_2b, 0, _PWM_RES));
+      break;
+    }
+  }
+}
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/esp32_mcu.cpp

@@ -0,0 +1,440 @@
+#include "../hardware_api.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) 
+
+#include "driver/mcpwm.h"
+#include "soc/mcpwm_reg.h"
+#include "soc/mcpwm_struct.h"
+
+// empty motor slot
+#define _EMPTY_SLOT -20
+#define _TAKEN_SLOT -21
+
+// ABI bus frequency - would be better to take it from somewhere
+// but I did nto find a good exposed variable
+#define _MCPWM_FREQ 160e6f
+
+// preferred pwm resolution default
+#define _PWM_RES_DEF 2048
+// min resolution
+#define _PWM_RES_MIN 1500
+// max resolution
+#define _PWM_RES_MAX 3000
+// pwm frequency
+#define _PWM_FREQUENCY 25000 // default
+#define _PWM_FREQUENCY_MAX 50000 // mqx
+
+// structure containing motor slot configuration
+// this library supports up to 4 motors
+typedef struct {
+  int pinA;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator;
+  mcpwm_io_signals_t mcpwm_a;
+  mcpwm_io_signals_t mcpwm_b;
+  mcpwm_io_signals_t mcpwm_c;
+} bldc_3pwm_motor_slots_t;
+typedef struct {
+  int pin1A;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator1;
+  mcpwm_operator_t mcpwm_operator2;
+  mcpwm_io_signals_t mcpwm_1a;
+  mcpwm_io_signals_t mcpwm_1b;
+  mcpwm_io_signals_t mcpwm_2a;
+  mcpwm_io_signals_t mcpwm_2b;
+} stepper_4pwm_motor_slots_t;
+typedef struct {
+  int pin1pwm;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator;
+  mcpwm_io_signals_t mcpwm_a;
+  mcpwm_io_signals_t mcpwm_b;
+} stepper_2pwm_motor_slots_t;
+
+typedef struct {
+  int pinAH;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator1;
+  mcpwm_operator_t mcpwm_operator2;
+  mcpwm_io_signals_t mcpwm_ah;
+  mcpwm_io_signals_t mcpwm_bh;
+  mcpwm_io_signals_t mcpwm_ch;
+  mcpwm_io_signals_t mcpwm_al;
+  mcpwm_io_signals_t mcpwm_bl;
+  mcpwm_io_signals_t mcpwm_cl;
+} bldc_6pwm_motor_slots_t;
+
+// define bldc motor slots array
+bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] =  {
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}  // 4th motor will be MCPWM1 channel B
+  };
+
+// define stepper motor slots array
+bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] =  {
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0
+  };
+
+// define 4pwm stepper motor slots array
+stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] =  {
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1
+  };
+
+// define 2pwm stepper motor slots array
+stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] =  {
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B}  // 4th motor will be MCPWM1 channel B
+  };
+
+// configuring high frequency pwm timer
+// a lot of help from this post from Paul Gauld
+// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller
+// a short tutorial for v2.0.1+
+// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html 
+//
+void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num,  mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){
+
+  mcpwm_config_t pwm_config;
+  pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave)
+  pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH
+  pwm_config.frequency  = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config);    //Configure PWM0A & PWM0B with above settings
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config);    //Configure PWM1A & PWM1B with above settings
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config);    //Configure PWM2A & PWM2B with above settings
+
+  if (_isset(dead_zone)){
+    // dead zone is configured
+    float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone;
+    mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
+    mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
+    mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
+  }
+  _delay(100);
+
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0);
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1);
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2);
+
+  // manual configuration due to the lack of config flexibility in mcpwm_init()
+  mcpwm_num->clk_cfg.clk_prescale = 0;
+  // calculate prescaler and period
+  // step 1: calculate the prescaler using the default pwm resolution
+  // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1
+  int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1;
+  // constrain prescaler
+  prescaler = _constrain(prescaler, 0, 128);
+  // now calculate the real resolution timer period necessary (pwm resolution)
+  // pwm_res = bus_freq / (pwm_freq * (prescaler + 1))
+  int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1);
+  // if pwm resolution too low lower the prescaler
+  if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 )
+    resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1);
+  resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX);
+
+  // set prescaler
+  mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler;
+  mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler;
+  mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler;
+  _delay(1);
+  //set period
+  mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected;
+  mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected;
+  mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected;
+  _delay(1);
+  mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0;
+  mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0;
+  mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0;
+  _delay(1);
+  // _delay(1);
+  //restart the timers
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_0);
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_1);
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_2);
+  _delay(1);
+
+  mcpwm_sync_config_t sync_conf = {
+    .sync_sig = MCPWM_SELECT_TIMER0_SYNC,
+    .timer_val = 0,
+    .count_direction = MCPWM_TIMER_DIRECTION_UP
+  };
+  mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf);
+  mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf);
+  mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf);
+
+  // Enable sync event for all timers to be the TEZ of timer0
+  mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+// supports Arudino/ATmega328, STM32 and ESP32
+void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
+
+  stepper_2pwm_motor_slots_t m_slot = {};
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  int slot_num;
+  for(slot_num = 0; slot_num < 4; slot_num++){
+    if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA;
+      m_slot = esp32_stepper_2pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+
+  // disable all the slots with the same MCPWM
+  // disable 3pwm bldc motor which would go in the same slot
+  esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
+  if( slot_num < 2 ){
+    // slot 0 of the 4pwm stepper
+    esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+  }else{
+    // slot 1 of the stepper
+    esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+  }
+  // configure pins
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
+
+  // configure the timer
+  _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit);
+
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+// supports Arudino/ATmega328, STM32 and ESP32
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
+
+  bldc_3pwm_motor_slots_t m_slot = {};
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  int slot_num;
+  for(slot_num = 0; slot_num < 4; slot_num++){
+    if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA;
+      m_slot = esp32_bldc_3pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+  // disable all the slots with the same MCPWM
+  // disable 2pwm steppr motor which would go in the same slot
+  esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT;
+  if( slot_num < 2 ){
+    // slot 0 of the 4pwm stepper
+    esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+  }else{
+    // slot 1 of the stepper
+    esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+  }
+  // configure pins
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC);
+
+  // configure the timer
+  _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit);
+
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
+  stepper_4pwm_motor_slots_t m_slot = {};
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  int slot_num;
+  for(slot_num = 0; slot_num < 2; slot_num++){
+    if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA;
+      m_slot = esp32_stepper_4pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+  // disable all the slots with the same MCPWM
+  if( slot_num == 0 ){
+    // slots 0 and 1 of the 3pwm bldc
+    esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
+    esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
+    // slots 0 and 1 of the 2pwm stepper taken
+    esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT;
+    esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+  }else{
+    // slots 2 and 3 of the 3pwm bldc
+    esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
+    esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
+    // slots 2 and 3 of the 2pwm stepper taken
+    esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT;
+    esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+  }
+
+  // configure pins
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD);
+
+  // configure the timer
+  _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+//  ESP32 uses MCPWM
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // determine which motor slot is the motor connected to
+  for(int i = 0; i < 4; i++){
+    if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found
+      // se the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,100]
+      mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0);
+      mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0);
+      break;
+    }
+  }
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+//  ESP32 uses MCPWM
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // determine which motor slot is the motor connected to
+  for(int i = 0; i < 4; i++){
+    if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found
+      // se the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,100]
+      mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0);
+      mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0);
+      mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0);
+      break;
+    }
+  }
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+//  ESP32 uses MCPWM
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // determine which motor slot is the motor connected to
+  for(int i = 0; i < 2; i++){
+    if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found
+      // se the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,100]
+      mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0);
+      mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0);
+      mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0);
+      mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0);
+      break;
+    }
+  }
+}
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency
+  bldc_6pwm_motor_slots_t m_slot = {};
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  int slot_num;
+  for(slot_num = 0; slot_num < 2; slot_num++){
+    if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h;
+      m_slot = esp32_bldc_6pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+  // if no slots available
+  if(slot_num >= 2) return -1;
+
+  // disable all the slots with the same MCPWM
+  if( slot_num == 0 ){
+    // slots 0 and 1 of the 3pwm bldc
+    esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
+    esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
+  }else{
+    // slots 2 and 3 of the 3pwm bldc
+    esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
+    esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
+  }
+
+  // configure pins
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l);
+
+  // configure the timer
+  _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit, dead_zone);
+  // return
+  return 0;
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - BLDC driver - 6PWM setting
+// - hardware specific
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+  // determine which motor slot is the motor connected to
+  for(int i = 0; i < 2; i++){
+    if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found
+      // se the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,100.0]
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0);
+      break;
+    }
+  }
+}
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/esp8266_mcu.cpp

@@ -0,0 +1,77 @@
+#include "../hardware_api.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266)
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin){
+  analogWrite(pin, 0);
+  analogWriteFreq(freq);
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+  _setHighFrequency(pwm_frequency, pinD);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+}
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+  analogWrite(pinC, 255.0f*dc_c);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pin1A, 255.0f*dc_1a);
+  analogWrite(pin1B, 255.0f*dc_1b);
+  analogWrite(pin2A, 255.0f*dc_2a);
+  analogWrite(pin2B, 255.0f*dc_2b);
+}
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/generic_mcu.cpp

@@ -0,0 +1,107 @@
+#include "../hardware_api.h"
+
+// if the mcu doen't have defiend analogWrite
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
+  __attribute__((weak)) void analogWrite(uint8_t pin, int value){ };
+#endif
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+// in generic case dont do anything
+__attribute__((weak)) void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+  _UNUSED(pwm_frequency);
+  _UNUSED(pinA);
+  _UNUSED(pinB);
+  return;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+// in generic case dont do anything
+__attribute__((weak)) void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  _UNUSED(pwm_frequency);
+  _UNUSED(pinA);
+  _UNUSED(pinB);
+  _UNUSED(pinC);
+  return;
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+// in generic case dont do anything
+__attribute__((weak)) void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+  _UNUSED(pwm_frequency);
+  _UNUSED(pin1A);
+  _UNUSED(pin1B);
+  _UNUSED(pin2A);
+  _UNUSED(pin2B);
+  return;
+}
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+__attribute__((weak)) int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+  _UNUSED(pwm_frequency);
+  _UNUSED(dead_zone);
+  _UNUSED(pinA_h);
+  _UNUSED(pinB_h);
+  _UNUSED(pinC_h);
+  _UNUSED(pinA_l);
+  _UNUSED(pinB_l);
+  _UNUSED(pinC_l);
+  return -1;
+}
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+  analogWrite(pinC, 255.0f*dc_c);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pin1A, 255.0f*dc_1a);
+  analogWrite(pin1B, 255.0f*dc_1b);
+  analogWrite(pin2A, 255.0f*dc_2a);
+  analogWrite(pin2B, 255.0f*dc_2b);
+}
+
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - BLDC driver - 6PWM setting
+// - hardware specific
+__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+  _UNUSED(dc_a);
+  _UNUSED(dc_b);
+  _UNUSED(dc_c);
+  _UNUSED(dead_zone);
+  _UNUSED(pinA_h);
+  _UNUSED(pinB_h);
+  _UNUSED(pinC_h);
+  _UNUSED(pinA_l);
+  _UNUSED(pinB_l);
+  _UNUSED(pinC_l);
+  return;
+}

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/nrf52_mcu.cpp

@@ -0,0 +1,355 @@
+
+#include "../hardware_api.h"
+
+#if defined(NRF52_SERIES)
+
+
+#define PWM_CLK (16000000)
+#define PWM_FREQ (40000)
+#define PWM_RESOLUTION (PWM_CLK/PWM_FREQ)
+#define PWM_MAX_FREQ (62500)
+#define DEAD_ZONE (250) // in ns
+#define DEAD_TIME (DEAD_ZONE  / (PWM_RESOLUTION * 0.25 * 62.5)) // 62.5ns resolution of PWM
+
+#ifdef NRF_PWM3
+#define PWM_COUNT 4
+#else
+#define PWM_COUNT 3
+#endif
+
+// empty motor slot 
+#define _EMPTY_SLOT (-0xAA)
+#define _TAKEN_SLOT (-0x55)
+
+int pwm_range;
+float dead_time;
+
+static NRF_PWM_Type* pwms[PWM_COUNT] = {
+  NRF_PWM0,
+  NRF_PWM1,
+  NRF_PWM2,
+  #ifdef  NRF_PWM3
+  NRF_PWM3
+  #endif
+};
+
+typedef struct {
+  int pinA;
+  NRF_PWM_Type* mcpwm;
+  uint16_t mcpwm_channel_sequence[4];
+} bldc_3pwm_motor_slots_t;
+
+typedef struct {
+  int pin1A;
+  NRF_PWM_Type* mcpwm;
+  uint16_t mcpwm_channel_sequence[4];
+} stepper_motor_slots_t;
+
+typedef struct {
+  int pinAH;
+  NRF_PWM_Type* mcpwm1;
+  NRF_PWM_Type* mcpwm2;
+  uint16_t mcpwm_channel_sequence[8];
+} bldc_6pwm_motor_slots_t;
+
+// define bldc motor slots array
+bldc_3pwm_motor_slots_t nrf52_bldc_3pwm_motor_slots[4] =  { 
+  {_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be PWM0 
+  {_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 2nd motor will be PWM1 
+  {_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 3rd motor will be PWM2 
+  {_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 4th motor will be PWM3 
+  };
+
+// define stepper motor slots array
+stepper_motor_slots_t nrf52_stepper_motor_slots[4] =  { 
+  {_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be on PWM0
+  {_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 1st motor will be on PWM1
+  {_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 1st motor will be on PWM2
+  {_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 1st motor will be on PWM3
+  };  
+
+// define BLDC motor slots array
+bldc_6pwm_motor_slots_t nrf52_bldc_6pwm_motor_slots[2] =  { 
+  {_EMPTY_SLOT, pwms[0], pwms[1], {0,0,0,0,0,0,0,0}},// 1st motor will be on PWM0 & PWM1
+  {_EMPTY_SLOT, pwms[2], pwms[3], {0,0,0,0,0,0,0,0}} // 2nd motor will be on PWM1 & PWM2
+  };  
+
+// configuring high frequency pwm timer
+void _configureHwPwm(NRF_PWM_Type* mcpwm1,  NRF_PWM_Type* mcpwm2){
+
+  mcpwm1->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos);
+  mcpwm1->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos);
+  mcpwm1->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos); 
+  mcpwm1->COUNTERTOP = pwm_range; //pwm freq.
+  mcpwm1->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos);
+  mcpwm1->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos);
+  mcpwm1->SEQ[0].REFRESH  = 0;
+  mcpwm1->SEQ[0].ENDDELAY = 0;
+
+  if(mcpwm1 != mcpwm2){
+    mcpwm2->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos);
+    mcpwm2->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos);
+    mcpwm2->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos); 
+    mcpwm2->COUNTERTOP = pwm_range; //pwm freq.
+    mcpwm2->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos);
+    mcpwm2->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos);
+    mcpwm2->SEQ[0].REFRESH  = 0;
+    mcpwm2->SEQ[0].ENDDELAY = 0;
+  }else{
+    mcpwm1->MODE = (PWM_MODE_UPDOWN_Up << PWM_MODE_UPDOWN_Pos);
+  }
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+
+  if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800
+  else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256  
+
+  pwm_range = (PWM_CLK / pwm_frequency);
+  
+  int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA];
+  int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB];
+  int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC];
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters 
+  int slot_num;
+  for(slot_num = 0; slot_num < 4; slot_num++){
+    if(nrf52_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      nrf52_bldc_3pwm_motor_slots[slot_num].pinA = pinA;
+      break;
+    }
+  }
+  // disable all the slots with the same MCPWM 
+  if(slot_num < 2){
+    // slot 0 of the stepper
+    nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+    //NRF_PPI->CHEN &= ~1UL;
+  }else{
+    // slot 1 of the stepper
+    nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+    //NRF_PPI->CHEN &= ~2UL;
+  }
+
+  // configure pwm outputs
+  
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA;
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB;
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC;
+   
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm_channel_sequence[0];
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4;
+
+  // configure the pwm
+  _configureHwPwm(nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm, nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+
+  if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800
+  else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256
+
+  pwm_range = (PWM_CLK / pwm_frequency);
+  
+  int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA];
+  int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB];
+  int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC];
+  int pD = digitalPinToPinName(pinD); //g_ADigitalPinMap[pinD];
+  
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters 
+  int slot_num;
+  for(slot_num = 0; slot_num < 4; slot_num++){
+    if(nrf52_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      nrf52_stepper_motor_slots[slot_num].pin1A = pinA;
+      break;
+    }
+  }
+  // disable all the slots with the same MCPWM 
+  if( slot_num < 2 ){
+    // slots 0 and 1 of the 3pwm bldc
+    nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+    //NRF_PPI->CHEN &= ~1UL;
+  }else{
+    // slots 2 and 3 of the 3pwm bldc
+    nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+    //NRF_PPI->CHEN &= ~2UL;
+  } 
+
+  // configure pwm outputs
+  
+  nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA;
+  nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB;
+  nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC;
+  nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[3] = pD;
+
+  nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_stepper_motor_slots[slot_num].mcpwm_channel_sequence[0];
+  nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4;
+
+  // configure the pwm
+  _configureHwPwm(nrf52_stepper_motor_slots[slot_num].mcpwm, nrf52_stepper_motor_slots[slot_num].mcpwm);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // determine which motor slot is the motor connected to 
+  for(int i = 0; i < 4; i++){
+    if(nrf52_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found
+      // se the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,range]
+      
+      nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_a * pwm_range) | 0x8000;
+      nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_b * pwm_range) | 0x8000;
+      nrf52_bldc_3pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_c * pwm_range) | 0x8000;
+  
+      nrf52_bldc_3pwm_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1;
+      break;
+    }
+  }
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A){
+  // determine which motor slot is the motor connected to 
+  for(int i = 0; i < 4; i++){
+    if(nrf52_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found
+      // se the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,range]
+
+      nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[0] = (int)(dc_1a * pwm_range) | 0x8000;
+      nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[1] = (int)(dc_1b * pwm_range) | 0x8000;
+      nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[2] = (int)(dc_2a * pwm_range) | 0x8000;
+      nrf52_stepper_motor_slots[i].mcpwm_channel_sequence[3] = (int)(dc_2b * pwm_range) | 0x8000;
+
+      nrf52_stepper_motor_slots[i].mcpwm->TASKS_SEQSTART[0] = 1;
+      break;
+    }
+  }
+}
+
+/* Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+*/
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+  
+  if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz - centered pwm has twice lower frequency for a resolution of 400
+  else pwm_frequency = _constrain(pwm_frequency*2, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max => 31.25kHz for a resolution of 256  
+
+  pwm_range = (PWM_CLK / pwm_frequency);
+  pwm_range /= 2; // scale the frequency (centered PWM)
+
+  if (dead_zone != NOT_SET){
+    dead_time = dead_zone/2;
+  }else{
+    dead_time = DEAD_TIME/2; 
+  }
+  
+  int pA_l = digitalPinToPinName(pinA_l); //g_ADigitalPinMap[pinA_l];
+  int pA_h = digitalPinToPinName(pinA_h); //g_ADigitalPinMap[pinA_h];
+  int pB_l = digitalPinToPinName(pinB_l); //g_ADigitalPinMap[pinB_l];
+  int pB_h = digitalPinToPinName(pinB_h); //g_ADigitalPinMap[pinB_h];
+  int pC_l = digitalPinToPinName(pinC_l); //g_ADigitalPinMap[pinC_l];
+  int pC_h = digitalPinToPinName(pinC_h); //g_ADigitalPinMap[pinC_h];
+
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters 
+  int slot_num;
+  for(slot_num = 0; slot_num < 2; slot_num++){
+    if(nrf52_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      nrf52_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h;
+      break;
+    }
+  }
+  // if no slots available
+  if(slot_num >= 2) return -1;
+
+  // disable all the slots with the same MCPWM 
+  if( slot_num == 0 ){
+    // slots 0 and 1 of the 3pwm bldc
+    nrf52_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
+    nrf52_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
+    // slot 0 and 1 of the stepper
+    nrf52_stepper_motor_slots[0].pin1A = _TAKEN_SLOT;
+    nrf52_stepper_motor_slots[1].pin1A = _TAKEN_SLOT;
+  }else{
+    // slots 2 and 3 of the 3pwm bldc
+    nrf52_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
+    nrf52_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
+    // slot 1 of the stepper
+    nrf52_stepper_motor_slots[2].pin1A = _TAKEN_SLOT;
+    nrf52_stepper_motor_slots[3].pin1A = _TAKEN_SLOT;
+  } 
+
+  // Configure pwm outputs
+  
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[0] = pA_h;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[1] = pA_l;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[2] = pB_h;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[3] = pB_l;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[0];
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].CNT = 4;
+
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[0] = pC_h;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[1] = pC_l;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[4];
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].CNT = 4;
+
+  // Initializing the PPI peripheral for sync the pwm slots
+
+  NRF_PPI->CH[slot_num].EEP = (uint32_t)&NRF_EGU0->EVENTS_TRIGGERED[0];
+  NRF_PPI->CH[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->TASKS_SEQSTART[0];
+  NRF_PPI->FORK[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->TASKS_SEQSTART[0];
+  NRF_PPI->CHEN =  1UL << slot_num;
+  
+  // configure the pwm type
+  _configureHwPwm(nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1, nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2);
+  
+  // return 
+  return 0;
+}
+
+/* Function setting the duty cycle to the pwm pin
+//  - BLDC driver - 6PWM setting
+//  - hardware specific
+*/
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, const int pinA_h, const int, const int, const int, const int, const int){
+  for(int i = 0; i < 2; i++){
+    if(nrf52_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found
+      // se the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,range]
+
+      nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[0] = (int)(_constrain(dc_a-dead_time,0,1)*pwm_range) | 0x8000;
+      nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[1] = (int)(_constrain(dc_a+dead_time,0,1)*pwm_range);
+      nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[2] = (int)(_constrain(dc_b-dead_time,0,1)*pwm_range) | 0x8000;
+      nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[3] = (int)(_constrain(dc_b+dead_time,0,1)*pwm_range);
+      nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[4] = (int)(_constrain(dc_c-dead_time,0,1)*pwm_range) | 0x8000;
+      nrf52_bldc_6pwm_motor_slots[i].mcpwm_channel_sequence[5] = (int)(_constrain(dc_c+dead_time,0,1)*pwm_range);
+     
+      NRF_EGU0->TASKS_TRIGGER[0] = 1;
+      break;
+    }
+  }
+}
+
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/portenta_h7_mcu.cpp

@@ -0,0 +1,539 @@
+
+#include "../hardware_api.h"
+
+#if defined(TARGET_PORTENTA_H7)
+
+#include "pwmout_api.h"
+#include "pinDefinitions.h"
+#include "platform/mbed_critical.h"
+#include "platform/mbed_power_mgmt.h"
+#include "platform/mbed_assert.h"
+#include "PeripheralPins.h"
+#include "pwmout_device.h"
+
+// default pwm parameters
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+
+// // 6pwm parameters
+// #define _HARDWARE_6PWM 1
+// #define _SOFTWARE_6PWM 0
+// #define _ERROR_6PWM -1
+
+typedef struct{
+  int id ;
+  pwmout_t pins[6];
+} portenta_h7_mcu_enty_s;
+
+portenta_h7_mcu_enty_s motor_slot[10];
+int slot_index = 0;
+
+
+/* Convert STM32 Cube HAL channel to LL channel */
+uint32_t _TIM_ChannelConvert_HAL2LL(uint32_t channel, pwmout_t *obj)
+{
+#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED)
+    if (obj->inverted) {
+        switch (channel) {
+            case TIM_CHANNEL_1  :
+                return LL_TIM_CHANNEL_CH1N;
+            case TIM_CHANNEL_2  :
+                return LL_TIM_CHANNEL_CH2N;
+            case TIM_CHANNEL_3  :
+                return LL_TIM_CHANNEL_CH3N;
+#if defined(LL_TIM_CHANNEL_CH4N)
+            case TIM_CHANNEL_4  :
+                return LL_TIM_CHANNEL_CH4N;
+#endif
+            default : /* Optional */
+                return 0;
+        }
+    } else
+#endif
+    {
+        switch (channel) {
+            case TIM_CHANNEL_1  :
+                return LL_TIM_CHANNEL_CH1;
+            case TIM_CHANNEL_2  :
+                return LL_TIM_CHANNEL_CH2;
+            case TIM_CHANNEL_3  :
+                return LL_TIM_CHANNEL_CH3;
+            case TIM_CHANNEL_4  :
+                return LL_TIM_CHANNEL_CH4;
+            default : /* Optional */
+                return 0;
+        }
+    }
+}
+
+
+
+// int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){
+//   return _pwm_init(obj, digitalPinToPinName(pin), frequency);
+// }
+
+int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){
+    int peripheral = (int)pinmap_peripheral(digitalPinToPinName(pin), PinMap_PWM);
+    int function = (int)pinmap_find_function(digitalPinToPinName(pin), PinMap_PWM);
+
+    const PinMap static_pinmap = {digitalPinToPinName(pin), peripheral, function};
+
+    pwmout_init_direct(obj, &static_pinmap);
+
+    TIM_HandleTypeDef TimHandle;
+    TimHandle.Instance = (TIM_TypeDef *)(obj->pwm);
+    RCC_ClkInitTypeDef RCC_ClkInitStruct;
+    uint32_t PclkFreq = 0;
+    uint32_t APBxCLKDivider = RCC_HCLK_DIV1;
+    uint8_t i = 0;
+
+
+    __HAL_TIM_DISABLE(&TimHandle);
+
+    // Get clock configuration
+    // Note: PclkFreq contains here the Latency (not used after)
+    HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &PclkFreq);
+
+    /*  Parse the pwm / apb mapping table to find the right entry */
+    while (pwm_apb_map_table[i].pwm != obj->pwm) i++;
+    // sanity check
+    if (pwm_apb_map_table[i].pwm == 0) return -1;
+    
+
+    if (pwm_apb_map_table[i].pwmoutApb == PWMOUT_ON_APB1) {
+        PclkFreq = HAL_RCC_GetPCLK1Freq();
+        APBxCLKDivider = RCC_ClkInitStruct.APB1CLKDivider;
+    } else {
+#if !defined(PWMOUT_APB2_NOT_SUPPORTED)
+        PclkFreq = HAL_RCC_GetPCLK2Freq();
+        APBxCLKDivider = RCC_ClkInitStruct.APB2CLKDivider;
+#endif
+    }
+
+    long period_us = 500000.0/((float)frequency);
+    /* By default use, 1us as SW pre-scaler */
+    obj->prescaler = 1;
+    // TIMxCLK = PCLKx when the APB prescaler = 1 else TIMxCLK = 2 * PCLKx
+    if (APBxCLKDivider == RCC_HCLK_DIV1) {
+        TimHandle.Init.Prescaler = (((PclkFreq) / 1000000)) - 1; // 1 us tick
+    } else {
+        TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000)) - 1; // 1 us tick
+    }
+    TimHandle.Init.Period = (period_us - 1);
+
+    /*  In case period or pre-scalers are out of range, loop-in to get valid values */
+    while ((TimHandle.Init.Period > 0xFFFF) || (TimHandle.Init.Prescaler > 0xFFFF)) {
+        obj->prescaler = obj->prescaler * 2;
+        if (APBxCLKDivider == RCC_HCLK_DIV1) {
+            TimHandle.Init.Prescaler = (((PclkFreq) / 1000000) * obj->prescaler) - 1;
+        } else {
+            TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000) * obj->prescaler) - 1;
+        }
+        TimHandle.Init.Period = (period_us - 1) / obj->prescaler;
+        /*  Period decreases and prescaler increases over loops, so check for
+         *  possible out of range cases */
+        if ((TimHandle.Init.Period < 0xFFFF) && (TimHandle.Init.Prescaler > 0xFFFF)) {
+            break;
+        }
+    }
+
+    TimHandle.Init.ClockDivision = 0;
+    TimHandle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; // center aligned
+
+    if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) {
+        return -1;
+    }
+    
+    TIM_OC_InitTypeDef sConfig;
+    // Configure channels
+    sConfig.OCMode       = TIM_OCMODE_PWM1;
+    sConfig.Pulse        = obj->pulse / obj->prescaler;
+    sConfig.OCPolarity   = TIM_OCPOLARITY_HIGH;
+    sConfig.OCFastMode   = TIM_OCFAST_DISABLE;
+#if defined(TIM_OCIDLESTATE_RESET)
+    sConfig.OCIdleState  = TIM_OCIDLESTATE_RESET;
+#endif
+#if defined(TIM_OCNIDLESTATE_RESET)
+    sConfig.OCNPolarity  = TIM_OCNPOLARITY_HIGH;
+    sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+#endif
+
+    int channel = 0;
+    switch (obj->channel) {
+        case 1:
+            channel = TIM_CHANNEL_1;
+            break;
+        case 2:
+            channel = TIM_CHANNEL_2;
+            break;
+        case 3:
+            channel = TIM_CHANNEL_3;
+            break;
+        case 4:
+            channel = TIM_CHANNEL_4;
+            break;
+        default:
+            return -1;
+    }
+    
+    if (LL_TIM_CC_IsEnabledChannel(TimHandle.Instance, _TIM_ChannelConvert_HAL2LL(channel, obj)) == 0) {
+        // If channel is not enabled, proceed to channel configuration
+        if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, channel) != HAL_OK) {
+            return -1;
+        }
+    } 
+
+    // Save for future use
+    obj->period = period_us;
+#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED)
+    if (obj->inverted) {
+        HAL_TIMEx_PWMN_Start(&TimHandle, channel);
+    } else
+#endif
+    {
+        HAL_TIM_PWM_Start(&TimHandle, channel);
+    }
+    
+    return 0;
+}
+
+// setting pwm to hardware pin - instead analogWrite()
+void _pwm_write(pwmout_t *obj, float value){
+  TIM_HandleTypeDef TimHandle;
+  int channel = 0;
+
+  TimHandle.Instance = (TIM_TypeDef *)(obj->pwm);
+  
+  if (value < (float)0.0) {
+      value = 0.0;
+  } else if (value > (float)1.0) {
+      value = 1.0;
+  }
+
+  obj->pulse = (uint32_t)((float)obj->period * value + 0.5);
+
+  switch (obj->channel) {
+      case 1:
+          channel = TIM_CHANNEL_1;
+          break;
+      case 2:
+          channel = TIM_CHANNEL_2;
+          break;
+      case 3:
+          channel = TIM_CHANNEL_3;
+          break;
+      case 4:
+          channel = TIM_CHANNEL_4;
+          break;
+      default:
+          return;
+  }
+  
+  // If channel already enabled, only update compare value to avoid glitch
+  __HAL_TIM_SET_COMPARE(&TimHandle, channel, obj->pulse / obj->prescaler);
+} 
+
+// init low side pin
+// HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin)
+// {
+//   PinName pin = digitalPinToPinName(ulPin);
+//   TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
+//   uint32_t index = get_timer_index(Instance);
+
+//   if (HardwareTimer_Handle[index] == NULL) {
+//     HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM));
+//     HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+//     HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+//     TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef();
+//     sConfigOC.OCMode = TIM_OCMODE_PWM2;
+//     sConfigOC.Pulse = 100;
+//     sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
+//     sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+//     sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
+//     sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET;
+//     sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+//     uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
+//     HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel);
+//   }
+//   HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+//   uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
+//   HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin);
+//   HT->setOverflow(PWM_freq, HERTZ_FORMAT);
+//   HT->pause();
+//   HT->refresh();
+//   return HT;
+// }
+
+
+// align the timers to end the init
+void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2){
+    TIM_HandleTypeDef TimHandle1, TimHandle2;
+    TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
+    TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
+    __HAL_TIM_DISABLE(&TimHandle1);
+    __HAL_TIM_DISABLE(&TimHandle2);
+    __HAL_TIM_ENABLE(&TimHandle1); 
+    __HAL_TIM_ENABLE(&TimHandle2); 
+}
+
+// align the timers to end the init
+void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3){
+    TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3;
+    TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
+    TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
+    TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm);
+    __HAL_TIM_DISABLE(&TimHandle1);
+    __HAL_TIM_DISABLE(&TimHandle2);
+    __HAL_TIM_DISABLE(&TimHandle3);
+    __HAL_TIM_ENABLE(&TimHandle1); 
+    __HAL_TIM_ENABLE(&TimHandle2); 
+    __HAL_TIM_ENABLE(&TimHandle3); 
+}
+
+// align the timers to end the init
+void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){
+    TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3, TimHandle4;
+    TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
+    TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
+    TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm);
+    TimHandle4.Instance = (TIM_TypeDef *)(t4->pwm);
+    __HAL_TIM_DISABLE(&TimHandle1);
+    __HAL_TIM_DISABLE(&TimHandle2);
+    __HAL_TIM_DISABLE(&TimHandle3);
+    __HAL_TIM_DISABLE(&TimHandle4);
+    __HAL_TIM_ENABLE(&TimHandle1); 
+    __HAL_TIM_ENABLE(&TimHandle2); 
+    __HAL_TIM_ENABLE(&TimHandle3);  
+    __HAL_TIM_ENABLE(&TimHandle4); 
+}
+
+// // configure hardware 6pwm interface only one timer with inverted channels
+// HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l)
+// {
+//   PinName uhPinName = digitalPinToPinName(pinA_h);
+//   PinName ulPinName = digitalPinToPinName(pinA_l);
+//   PinName vhPinName = digitalPinToPinName(pinB_h);
+//   PinName vlPinName = digitalPinToPinName(pinB_l);
+//   PinName whPinName = digitalPinToPinName(pinC_h);
+//   PinName wlPinName = digitalPinToPinName(pinC_l);
+
+//   TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM);
+
+//   uint32_t index = get_timer_index(Instance);
+
+//   if (HardwareTimer_Handle[index] == NULL) {
+//     HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM));
+//     HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+//     HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+//     ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT);
+//   }
+//   HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName);
+
+//   // dead time is set in nanoseconds
+//   uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone;
+//   uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns);
+//   LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear!
+//   LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N);
+
+//   HT->pause();
+//   HT->refresh();
+//   HT->resume();
+//   return HT;
+// }
+
+
+// // returns 0 if each pair of pwm channels has same channel
+// // returns 1 all the channels belong to the same timer - hardware inverted channels
+// // returns -1 if neither - avoid configuring - error!!!
+// int _interfaceType(const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+//   PinName nameAH = digitalPinToPinName(pinA_h);
+//   PinName nameAL = digitalPinToPinName(pinA_l);
+//   PinName nameBH = digitalPinToPinName(pinB_h);
+//   PinName nameBL = digitalPinToPinName(pinB_l);
+//   PinName nameCH = digitalPinToPinName(pinC_h);
+//   PinName nameCL = digitalPinToPinName(pinC_l);
+//   int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM));
+//   int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM));
+//   int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM));
+//   int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM));
+//   int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM));
+//   int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM));
+//   if(tim1 == tim2 && tim2==tim3 && tim3==tim4  && tim4==tim5 && tim5==tim6)
+//     return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer
+//   else if(tim1 == tim2 && tim3==tim4  && tim5==tim6)
+//     return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer
+//   else
+//     return _ERROR_6PWM; // might be error avoid configuration
+// }
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+  if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  
+  motor_slot[slot_index].id = pinA;
+
+  core_util_critical_section_enter();
+  _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency);
+  _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency);
+  // allign the timers
+  _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]));
+  core_util_critical_section_exit();
+  slot_index++;
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+
+  motor_slot[slot_index].id = pinA;
+  core_util_critical_section_enter();
+  _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency);
+  _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency);
+  _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency);
+  // allign the timers
+  _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]));
+  core_util_critical_section_exit();
+  slot_index++;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+
+  motor_slot[slot_index].id = pinA;
+  core_util_critical_section_enter();
+  _pwm_init(&(motor_slot[slot_index].pins[0]), pinA, (long)pwm_frequency);
+  _pwm_init(&(motor_slot[slot_index].pins[1]), pinB, (long)pwm_frequency);
+  _pwm_init(&(motor_slot[slot_index].pins[2]), pinC, (long)pwm_frequency);
+  _pwm_init(&(motor_slot[slot_index].pins[3]), pinD, (long)pwm_frequency);
+  // allign the timers
+  _alignPWMTimers(&(motor_slot[slot_index].pins[0]), &(motor_slot[slot_index].pins[1]), &(motor_slot[slot_index].pins[2]), &(motor_slot[slot_index].pins[3]));
+  core_util_critical_section_exit();
+  slot_index++;
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+//- hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+for(int i=0; i<slot_index; i++){
+     if(motor_slot[i].id == pinA){
+      core_util_critical_section_enter();
+      _pwm_write(&(motor_slot[i].pins[0]), (float)dc_a);
+      _pwm_write(&(motor_slot[i].pins[1]), (float)dc_b);
+      core_util_critical_section_exit();
+    }
+  }
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+//- hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  for(int i=0; i<slot_index; i++){
+     if(motor_slot[i].id == pinA){
+      core_util_critical_section_enter();
+      _pwm_write(&(motor_slot[i].pins[0]), (float)dc_a);
+      _pwm_write(&(motor_slot[i].pins[1]), (float)dc_b);
+      _pwm_write(&(motor_slot[i].pins[2]), (float)dc_c);
+      core_util_critical_section_exit();
+    }
+  }
+}
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+//- hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  for(int i=0; i<slot_index; i++){
+     if(motor_slot[i].id == pin1A){
+      core_util_critical_section_enter();
+      _pwm_write(&(motor_slot[i].pins[0]), (float)dc_1a);
+      _pwm_write(&(motor_slot[i].pins[1]), (float)dc_1b);
+      _pwm_write(&(motor_slot[i].pins[2]), (float)dc_2a);
+      _pwm_write(&(motor_slot[i].pins[3]), (float)dc_2b);
+      core_util_critical_section_exit();
+    }
+  }
+}
+
+
+
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+  // if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  // else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max
+  // // center-aligned frequency is uses two periods
+  // pwm_frequency *=2;
+
+  // // find configuration
+  // int config = _interfaceType(pinA_h, pinA_l,  pinB_h, pinB_l, pinC_h, pinC_l);
+  // // configure accordingly
+  // switch(config){
+  //   case _ERROR_6PWM:
+  //     return -1; // fail
+  //     break;
+  //   case _HARDWARE_6PWM:
+  //     _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
+  //     break;
+  //   case _SOFTWARE_6PWM:
+  //     HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h);
+  //     _initPinPWMLow(pwm_frequency, pinA_l);
+  //     HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h);
+  //     _initPinPWMLow(pwm_frequency, pinB_l);
+  //     HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h);
+  //     _initPinPWMLow(pwm_frequency, pinC_l);
+  //     _alignPWMTimers(HT1, HT2, HT3);
+  //     break;
+  // }
+  return -1; // success
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - BLDC driver - 6PWM setting
+// - hardware specific
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+  // // find configuration
+  // int config = _interfaceType(pinA_h, pinA_l,  pinB_h, pinB_l, pinC_h, pinC_l);
+  // // set pwm accordingly
+  // switch(config){
+  //   case _HARDWARE_6PWM:
+  //     _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+  //     _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
+  //     _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
+  //     break;
+  //   case _SOFTWARE_6PWM:
+  //     _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     break;
+  // }
+}
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/rp2040_mcu.cpp

@@ -0,0 +1,163 @@
+
+/**
+ * Support for the RP2040 MCU, as found on the Raspberry Pi Pico.
+ */
+#if defined(TARGET_RP2040)
+
+#define SIMPLEFOC_DEBUG_RP2040
+
+
+#ifdef SIMPLEFOC_DEBUG_RP2040
+
+#ifndef SIMPLEFOC_RP2040_DEBUG_SERIAL
+#define SIMPLEFOC_RP2040_DEBUG_SERIAL Serial
+#endif
+
+#endif
+
+#include "Arduino.h"
+
+
+
+
+// until I can figure out if this can be quickly read from some register, keep it here.
+// it also serves as a marker for what slices are already used.
+uint16_t wrapvalues[NUM_PWM_SLICES];
+
+
+// TODO add checks which channels are already used...
+
+void setupPWM(int pin, long pwm_frequency, bool invert = false) {
+	gpio_set_function(pin, GPIO_FUNC_PWM);
+	uint slice = pwm_gpio_to_slice_num(pin);
+	uint chan = pwm_gpio_to_channel(pin);
+	pwm_set_clkdiv_int_frac(slice, 1, 0); // fastest pwm we can get
+	pwm_set_phase_correct(slice, true);
+	uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L;
+	if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000
+	if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300
+#ifdef SIMPLEFOC_DEBUG_RP2040
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print("Configuring pin ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pin);
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" slice ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(slice);
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" channel ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(chan);
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" frequency ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pwm_frequency);
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" top value ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.println(wrapvalue);
+#endif
+	pwm_set_wrap(slice, wrapvalue);
+	wrapvalues[slice] = wrapvalue;
+	if (invert) {
+		if (chan==0)
+			hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_A_INV_LSB, PWM_CH0_CSR_A_INV_BITS);
+		else
+			hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_B_INV_LSB, PWM_CH0_CSR_B_INV_BITS);
+	}
+	pwm_set_chan_level(slice, chan, 0); // switch off initially
+}
+
+
+void syncSlices() {
+	for (int i=0;i<NUM_PWM_SLICES;i++) {
+		pwm_set_enabled(i, false);
+		pwm_set_counter(i, 0);
+	}
+	// enable all slices
+	pwm_set_mask_enabled(0x7F);
+}
+
+
+void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+	setupPWM(pinA, pwm_frequency);
+	setupPWM(pinB, pwm_frequency);
+	syncSlices();
+}
+
+
+
+void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) {
+	setupPWM(pinA, pwm_frequency);
+	setupPWM(pinB, pwm_frequency);
+	setupPWM(pinC, pwm_frequency);
+	syncSlices();
+}
+
+
+
+
+void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+	setupPWM(pin1A, pwm_frequency);
+	setupPWM(pin1B, pwm_frequency);
+	setupPWM(pin2A, pwm_frequency);
+	setupPWM(pin2B, pwm_frequency);
+	syncSlices();
+}
+
+
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+	// non-PIO solution...
+	setupPWM(pinA_h, pwm_frequency);
+	setupPWM(pinB_h, pwm_frequency);
+	setupPWM(pinC_h, pwm_frequency);
+	setupPWM(pinA_l, pwm_frequency, true);
+	setupPWM(pinB_l, pwm_frequency, true);
+	setupPWM(pinC_l, pwm_frequency, true);
+	syncSlices();
+	return 0;
+}
+
+
+
+
+
+void writeDutyCycle(float val, int pin) {
+	uint slice = pwm_gpio_to_slice_num(pin);
+	uint chan = pwm_gpio_to_channel(pin);
+	pwm_set_chan_level(slice, chan, (wrapvalues[slice]+1) * val);
+}
+
+
+
+
+
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB) {
+	writeDutyCycle(dc_a, pinA);
+	writeDutyCycle(dc_b, pinB);
+}
+
+
+
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC) {
+	writeDutyCycle(dc_a, pinA);
+	writeDutyCycle(dc_b, pinB);
+	writeDutyCycle(dc_c, pinC);
+}
+
+
+
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B) {
+	writeDutyCycle(dc_1a, pin1A);
+	writeDutyCycle(dc_1b, pin1B);
+	writeDutyCycle(dc_2a, pin2A);
+	writeDutyCycle(dc_2b, pin2B);
+}
+
+inline float swDti(float val, float dt) {
+	float ret = dt+val;
+	if (ret>1.0) ret = 1.0f;
+	return ret;
+}
+
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) {
+	writeDutyCycle(dc_a, pinA_h);
+	writeDutyCycle(swDti(dc_a, dead_zone), pinA_l);
+	writeDutyCycle(dc_b, pinB_h);
+	writeDutyCycle(swDti(dc_b,dead_zone), pinB_l);
+	writeDutyCycle(dc_c, pinC_h);
+	writeDutyCycle(swDti(dc_c,dead_zone), pinC_l);
+}
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/samd21_mcu.cpp

@@ -0,0 +1,348 @@
+
+
+
+#include "./samd_mcu.h"
+
+
+#ifdef _SAMD21_
+
+
+
+#ifndef TCC3_CH0
+#define TCC3_CH0 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH1
+#define TCC3_CH1 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH2
+#define TCC3_CH2 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH3
+#define TCC3_CH3 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH4
+#define TCC3_CH4 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH5
+#define TCC3_CH5 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH6
+#define TCC3_CH6 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH7
+#define TCC3_CH7 NOT_ON_TIMER
+#endif
+#ifndef TC6_CH0
+#define TC6_CH0 NOT_ON_TIMER
+#endif
+#ifndef TC6_CH1
+#define TC6_CH1 NOT_ON_TIMER
+#endif
+#ifndef TC7_CH0
+#define TC7_CH0 NOT_ON_TIMER
+#endif
+#ifndef TC7_CH1
+#define TC7_CH1 NOT_ON_TIMER
+#endif
+
+
+
+#define NUM_WO_ASSOCIATIONS 48
+
+/*
+ * For SAM D21 A/B/C/D Variant Devices and SAM DA1 A/B Variant Devices
+ * Good for SAMD2xE, SAMD2xG and SAMD2xJ devices. Other SAMD21s currently not supported in arduino anyway?
+ *
+ * Note: only the pins which have timers associated are listed in this table.
+ * You can use the values from g_APinDescription.ulPort and g_APinDescription.ulPin to find the correct row in the table.
+ *
+ * See Microchip Technology datasheet DS40001882F-page 30
+ */
+struct wo_association WO_associations[] = {
+
+		{ PORTA,  0, TCC2_CH0, 		0, NOT_ON_TIMER, 	0},
+		{ PORTA,  1, TCC2_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTA,  2, NOT_ON_TIMER, 	0, TCC3_CH0, 		0},
+		{ PORTA,  3, NOT_ON_TIMER,	0, TCC3_CH1, 		1},
+		// PB04, PB05, PB06, PB07 - no timers
+		{ PORTB,  8, TC4_CH0, 		0, TCC3_CH6, 		6},
+		{ PORTB,  9, TC4_CH1, 		1, TCC3_CH7, 		7},
+		{ PORTA,  4, TCC0_CH0, 		0, TCC3_CH2, 		2},
+		{ PORTA,  5, TCC0_CH1, 		1, TCC3_CH3, 		3},
+		{ PORTA,  6, TCC1_CH0, 		0, TCC3_CH4, 		4},
+		{ PORTA,  7, TCC1_CH1, 		1, TCC3_CH5, 		5},
+		{ PORTA,  8, TCC0_CH0, 		0, TCC1_CH2, 		2},
+		{ PORTA,  9, TCC0_CH1, 		1, TCC1_CH3, 		3},
+		{ PORTA, 10, TCC1_CH0, 		0, TCC0_CH2, 		2},
+		{ PORTA, 11, TCC1_CH1, 		1, TCC0_CH3, 		3},
+		{ PORTB, 10, TC5_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTB, 11, TC5_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTB, 12, TC4_CH0, 		0, TCC0_CH6, 		6},
+		{ PORTB, 13, TC4_CH1, 		1, TCC0_CH7, 		7},
+		{ PORTB, 14, TC5_CH0, 		0, NOT_ON_TIMER,	0},
+		{ PORTB, 15, TC5_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTA, 12, TCC2_CH0, 		0, TCC0_CH6, 		6},
+		{ PORTA, 13, TCC2_CH1, 		1, TCC0_CH7, 		7},
+		{ PORTA, 14, TC3_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTA, 15, TC3_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTA, 16, TCC2_CH0, 		0, TCC0_CH6, 		6},
+		{ PORTA, 17, TCC2_CH1, 		1, TCC0_CH7, 		7},
+		{ PORTA, 18, TC3_CH0, 		0, TCC0_CH2, 		2},
+		{ PORTA, 19, TC3_CH1, 		1, TCC0_CH3, 		3},
+		{ PORTB, 16, TC6_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTB, 17, TC6_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTA, 20, TC7_CH0, 		0, TCC0_CH6, 		6},
+		{ PORTA, 21, TC7_CH1, 		1, TCC0_CH7, 		7},
+		{ PORTA, 22, TC4_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTA, 23, TC4_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTA, 24, TC5_CH0, 		0, TCC1_CH2, 		2},
+		{ PORTA, 25, TC5_CH1, 		1, TCC1_CH3, 		3},
+		{ PORTB, 22, TC7_CH0, 		0, TCC3_CH0, 		0},
+		{ PORTB, 23, TC7_CH1, 		1, TCC3_CH1, 		1},
+		{ PORTA, 27, NOT_ON_TIMER, 	0, TCC3_CH6, 		6},
+		{ PORTA, 28, NOT_ON_TIMER, 	0, TCC3_CH7, 		7},
+		{ PORTA, 30, TCC1_CH0, 		0, TCC3_CH4, 		4},
+		{ PORTA, 31, TCC1_CH1, 		1, TCC3_CH5, 		5},
+		{ PORTB, 30, TCC0_CH0, 		0, TCC1_CH2, 		2},
+		{ PORTB, 31, TCC0_CH1, 		1, TCC1_CH3, 		3},
+		{ PORTB,  0, TC7_CH0, 		0, NOT_ON_TIMER, 	0},
+		{ PORTB,  1, TC7_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTB,  2, TC6_CH0, 		0, TCC3_CH2, 		2},
+		{ PORTB,  3, TC6_CH1, 		1, TCC3_CH3, 		3}
+};
+wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0};
+
+
+
+struct wo_association& getWOAssociation(EPortType port, uint32_t pin) {
+	for (int i=0;i<NUM_WO_ASSOCIATIONS;i++) {
+		if (WO_associations[i].port==port && WO_associations[i].pin==pin)
+			return WO_associations[i];
+	}
+	return ASSOCIATION_NOT_FOUND;
+};
+
+
+
+EPioType getPeripheralOfPermutation(int permutation, int pin_position) {
+	return ((permutation>>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER;
+}
+
+
+
+
+
+void syncTCC(Tcc* TCCx) {
+  while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);       // Wait for synchronization of registers between the clock domains
+}
+
+
+
+/**
+ * Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that
+ * any compatible pin combination can be used without having to worry about configuring different
+ * clocks.
+ */
+void configureSAMDClock() {
+
+	// TODO investigate using the FDPLL96M clock to get 96MHz timer clocks... this
+	//      would enable 48KHz PWM clocks, and setting the frequency between 24Khz with resolution 2000, to 48KHz with resolution 1000
+
+	if (!SAMDClockConfigured) {
+		SAMDClockConfigured = true;						// mark clock as configured
+		for (int i=0;i<TCC_INST_NUM;i++)				// mark all the TCCs as not yet configured
+			tccConfigured[i] = false;
+		REG_GCLK_GENDIV = GCLK_GENDIV_DIV(1) |          // Divide the 48MHz clock source by divisor N=1: 48MHz/1=48MHz
+						GCLK_GENDIV_ID(4);            	// Select Generic Clock (GCLK) 4
+		while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization
+
+		REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC |           // Set the duty cycle to 50/50 HIGH/LOW
+						 GCLK_GENCTRL_GENEN |         	// Enable GCLK4
+						 GCLK_GENCTRL_SRC_DFLL48M |   	// Set the 48MHz clock source
+						// GCLK_GENCTRL_SRC_FDPLL |   	// Set the 96MHz clock source
+						 GCLK_GENCTRL_ID(4);          	// Select GCLK4
+		while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock...");
+#endif
+	}
+}
+
+
+
+
+/**
+ * Configure a TCC unit
+ * pwm_frequency is fixed at 24kHz for now. We could go slower, but going
+ * faster won't be possible without sacrificing resolution.
+ */
+void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) {
+
+	long pwm_resolution = (24000000) / pwm_frequency;
+	if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) 
+		pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION;
+	if (pwm_resolution<SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION) 
+		pwm_resolution = SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION;
+	// store for later use
+	tccConfig.pwm_res = pwm_resolution;
+
+	// TODO for the moment we ignore the frequency...
+	if (!tccConfigured[tccConfig.tcc.tccn]) {
+		uint32_t GCLK_CLKCTRL_ID_ofthistcc = -1;
+		switch (tccConfig.tcc.tccn>>1) {
+		case 0: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC0_TCC1); break; //GCLK_CLKCTRL_ID_TCC0_TCC1;
+		case 1: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC2_TC3); break;  //GCLK_CLKCTRL_ID_TCC2_TC3;
+		case 2: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC4_TC5); break;   //GCLK_CLKCTRL_ID_TC4_TC5;
+		case 3: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC6_TC7); break;
+		default: return;
+		}
+
+		// Feed GCLK4 to TCC
+		REG_GCLK_CLKCTRL = (uint16_t)  GCLK_CLKCTRL_CLKEN |         // Enable GCLK4
+									   GCLK_CLKCTRL_GEN_GCLK4 |     // Select GCLK4
+									   GCLK_CLKCTRL_ID_ofthistcc;   // Feed GCLK4 to tcc
+		while (GCLK->STATUS.bit.SYNCBUSY);              			// Wait for synchronization
+
+		tccConfigured[tccConfig.tcc.tccn] = true;
+
+		if (tccConfig.tcc.tccn>=TCC_INST_NUM) {
+			Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo);
+			// disable
+			tc->COUNT8.CTRLA.bit.ENABLE = 0;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// unfortunately we need the 8-bit counter mode to use the PER register...
+			tc->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM ;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// meaning prescaler of 8, since TC-Unit has no up/down mode, and has resolution of 250 rather than 1000...
+			tc->COUNT8.CTRLA.bit.PRESCALER = TC_CTRLA_PRESCALER_DIV8_Val ;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// period is 250, period cannot be higher than 256!
+			tc->COUNT8.PER.reg = SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// initial duty cycle is 0
+			tc->COUNT8.CC[tccConfig.tcc.chan].reg = 0;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// enable
+			tc->COUNT8.CTRLA.bit.ENABLE = 1;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+#ifdef SIMPLEFOC_SAMD_DEBUG
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC ");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn);
+#endif
+		}
+		else {
+			Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
+
+			uint8_t invenMask = ~(1<<tccConfig.tcc.chan);	// negate (invert) the signal if needed
+			uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
+			tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
+			syncTCC(tcc); // wait for sync
+
+			tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTH;   // Set wave form configuration
+			while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync
+
+			if (hw6pwm>0.0) {
+				tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
+				tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
+				tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
+				syncTCC(tcc); // wait for sync
+			}
+
+			tcc->PER.reg = pwm_resolution - 1;                 // Set counter Top using the PER register
+			while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync
+
+			// set all channels to 0%
+			uint8_t chanCount = (tccConfig.tcc.tccn==1||tccConfig.tcc.tccn==2)?2:4;
+			for (int i=0;i<chanCount;i++) {
+				tcc->CC[i].reg = 0;					// start off at 0% duty cycle
+				uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+i);
+				while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
+			}
+
+			// Enable TC
+			tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz
+			while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("    Initialized TCC ");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn);
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan);
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo);
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("]  pwm res ");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution);
+#endif
+		}
+	}
+	else if (tccConfig.tcc.tccn<TCC_INST_NUM) {
+		// set invert bit for TCC channels in SW 6-PWM...
+		Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
+
+		tcc->CTRLA.bit.ENABLE = 0;
+		while ( tcc->SYNCBUSY.bit.ENABLE == 1 );
+
+		uint8_t invenMask = ~(1<<tccConfig.tcc.chan);	// negate (invert) the signal if needed
+		uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
+		tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
+		syncTCC(tcc); // wait for sync
+
+		if (hw6pwm>0.0) {
+			tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
+			tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
+			tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
+			syncTCC(tcc); // wait for sync
+		}
+
+		tcc->CTRLA.bit.ENABLE = 1;
+		while ( tcc->SYNCBUSY.bit.ENABLE == 1 );
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC ");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("]  pwm res ");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution);
+#endif
+	}
+
+
+}
+
+
+
+
+
+void writeSAMDDutyCycle(tccConfiguration* info, float dc) {
+	uint8_t tccn = GetTCNumber(info->tcc.chaninfo);
+	uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo);
+	if (tccn<TCC_INST_NUM) {
+		Tcc* tcc = (Tcc*)GetTC(info->tcc.chaninfo);
+		// set via CC
+//		tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc);
+//		uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan);
+//		while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
+		// set via CCB
+		//while ( (tcc->SYNCBUSY.vec.CC & (0x1<<chan)) > 0 );
+		tcc->CCB[chan].reg = (uint32_t)((info->pwm_res-1) * dc);
+//		while ( (tcc->SYNCBUSY.vec.CCB & (0x1<<chan)) > 0 );
+//		tcc->STATUS.vec.CCBV |= (0x1<<chan);
+//		while ( tcc->SYNCBUSY.bit.STATUS > 0 );
+//		tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val);
+//		while ( tcc->SYNCBUSY.bit.CTRLB > 0 );
+	}
+	else {
+		Tc* tc = (Tc*)GetTC(info->tcc.chaninfo);
+		tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc);
+		while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+	}
+}
+
+
+
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/samd51_mcu.cpp

@@ -0,0 +1,311 @@
+
+
+#include "./samd_mcu.h"
+
+
+#if defined(_SAMD51_)||defined(_SAME51_)
+
+
+
+// expected frequency on DPLL, since we don't configure it ourselves. Typically this is the CPU frequency.
+// for custom boards or overclockers you can override it using this define.
+#ifndef SIMPLEFOC_SAMD51_DPLL_FREQ
+#define SIMPLEFOC_SAMD51_DPLL_FREQ 120000000
+#endif
+
+
+
+
+
+//	TCC#   Channels   WO_NUM   Counter size   Fault   Dithering   Output matrix   DTI   SWAP   Pattern generation
+//	0         6         8         24-bit       Yes       Yes       Yes            Yes   Yes    Yes
+//	1         4         8         24-bit       Yes       Yes       Yes            Yes   Yes    Yes
+//	2         3         3         16-bit       Yes       -         Yes            -     -      -
+//	3         2         2         16-bit       Yes       -         -              -     -      -
+//	4         2         2         16-bit       Yes       -         -              -     -      -
+
+
+#define NUM_WO_ASSOCIATIONS 72
+
+struct wo_association WO_associations[] = {
+
+		{ PORTB,   9, TC4_CH1, 		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTA,   4, TC0_CH0, 		0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTA,   5, TC0_CH1, 		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTA,   6, TC1_CH0, 		0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTA,   7, TC1_CH1, 		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTC,   4, NOT_ON_TIMER, 0, TCC0_CH0, 	    0, NOT_ON_TIMER, 	0},
+		// PC05, PC06, PC07 -> no timers
+		{ PORTA,   8, TC0_CH0, 		0, TCC0_CH0,     	0, TCC1_CH0, 		4},
+		{ PORTA,   9, TC0_CH1, 		1, TCC0_CH1,     	1, TCC1_CH1, 		5},
+		{ PORTA,  10, TC1_CH0, 		0, TCC0_CH2,     	2, TCC1_CH2, 		6},
+		{ PORTA,  11, TC1_CH1, 		1, TCC0_CH3,     	3, TCC1_CH3, 		7},
+		{ PORTB,  10, TC5_CH0, 		0, TCC0_CH4, 		4, TCC1_CH0, 		0},
+		{ PORTB,  11, TC5_CH1, 		1, TCC0_CH5,  		5, TCC1_CH1, 		1},
+		{ PORTB,  12, TC4_CH0, 		0, TCC3_CH0, 		0, TCC0_CH0, 		0},
+		{ PORTB,  13, TC4_CH1, 		1, TCC3_CH1, 		1, TCC0_CH1, 		1},
+		{ PORTB,  14, TC5_CH0, 		0, TCC4_CH0, 		0, TCC0_CH2, 		2},
+		{ PORTB,  15, TC5_CH1, 		1, TCC4_CH1, 		1, TCC0_CH3, 		3},
+		{ PORTD,   8, NOT_ON_TIMER,	0, TCC0_CH1,     	1, NOT_ON_TIMER, 	0},
+		{ PORTD,   9, NOT_ON_TIMER,	0, TCC0_CH2,     	2, NOT_ON_TIMER, 	0},
+		{ PORTD,  10, NOT_ON_TIMER,	0, TCC0_CH3,     	3, NOT_ON_TIMER, 	0},
+		{ PORTD,  11, NOT_ON_TIMER,	0, TCC0_CH4, 		4, NOT_ON_TIMER, 	0},
+		{ PORTD,  12, NOT_ON_TIMER,	0, TCC0_CH5,  		5, NOT_ON_TIMER, 	0},
+		{ PORTC,  10, NOT_ON_TIMER,	0, TCC0_CH0,     	0, TCC1_CH0, 		4},
+		{ PORTC,  11, NOT_ON_TIMER,	0, TCC0_CH1,     	1, TCC1_CH1, 		5},
+		{ PORTC,  12, NOT_ON_TIMER,	0, TCC0_CH2, 		2, TCC1_CH2, 		6},
+		{ PORTC,  13, NOT_ON_TIMER,	0, TCC0_CH3, 		3, TCC1_CH3, 		7},
+		{ PORTC,  14, NOT_ON_TIMER,	0, TCC0_CH4, 		4, TCC1_CH0, 		0},
+		{ PORTC,  15, NOT_ON_TIMER,	0, TCC0_CH5, 		5, TCC1_CH1, 		1},
+		{ PORTA,  12, TC2_CH0,		0, TCC0_CH0, 		6, TCC1_CH2, 		2},
+		{ PORTA,  13, TC2_CH1,		1, TCC0_CH1, 		7, TCC1_CH3, 		3},
+		{ PORTA,  14, TC3_CH0,		0, TCC2_CH0, 		0, TCC1_CH2, 		2},
+		{ PORTA,  15, TC3_CH1,		1, TCC1_CH1, 		1, TCC1_CH3, 		3},
+		{ PORTA,  16, TC2_CH0,		0, TCC1_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTA,  17, TC2_CH1,		1, TCC1_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTA,  18, TC3_CH0,		0, TCC1_CH2, 		2, TCC0_CH0, 		6},
+		{ PORTA,  19, TC3_CH1,		1, TCC1_CH3, 		3, TCC0_CH1, 		7},
+		{ PORTC,  16, NOT_ON_TIMER,	0, TCC0_CH0, 		0, NOT_ON_TIMER, 	0}, // PDEC0
+		{ PORTC,  17, NOT_ON_TIMER,	0, TCC0_CH1, 		1, NOT_ON_TIMER, 	0}, // PDEC1
+		{ PORTC,  18, NOT_ON_TIMER,	0, TCC0_CH2, 		2, NOT_ON_TIMER, 	0}, // PDEC2
+		{ PORTC,  19, NOT_ON_TIMER,	0, TCC0_CH3, 		3, NOT_ON_TIMER, 	0},
+		{ PORTC,  20, NOT_ON_TIMER,	0, TCC0_CH4, 		4, NOT_ON_TIMER, 	0},
+		{ PORTC,  21, NOT_ON_TIMER,	0, TCC0_CH5, 		5, NOT_ON_TIMER, 	0},
+		{ PORTC,  22, NOT_ON_TIMER,	0, TCC0_CH0, 		6, NOT_ON_TIMER, 	0},
+		{ PORTC,  23, NOT_ON_TIMER,	0, TCC0_CH1, 		7, NOT_ON_TIMER, 	0},
+		{ PORTD,  20, NOT_ON_TIMER,	0, TCC1_CH0, 		0, NOT_ON_TIMER, 	0},
+		{ PORTD,  21, NOT_ON_TIMER,	0, TCC1_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTB,  16, TC6_CH0,		0, TCC3_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTB,  17, TC6_CH1,		1, TCC3_CH1, 		1, TCC0_CH5,	 	5},
+		{ PORTB,  18, NOT_ON_TIMER,	0, TCC1_CH0, 		0, NOT_ON_TIMER, 	0}, // PDEC0
+		{ PORTB,  19, NOT_ON_TIMER,	0, TCC1_CH1, 		1, NOT_ON_TIMER, 	0}, // PDEC1
+		{ PORTB,  20, NOT_ON_TIMER,	0, TCC1_CH2, 		2, NOT_ON_TIMER, 	0}, // PDEC2
+		{ PORTB,  21, NOT_ON_TIMER,	0, TCC1_CH3, 		3, NOT_ON_TIMER, 	0},
+		{ PORTA,  20, TC7_CH0,		0, TCC1_CH0, 		4, TCC0_CH0, 		0},
+		{ PORTA,  21, TC7_CH1,		1, TCC1_CH1, 		5, TCC0_CH1, 		1},
+		{ PORTA,  22, TC4_CH0,		0, TCC1_CH2, 		6, TCC0_CH2, 		2},
+		{ PORTA,  23, TC4_CH1,		1, TCC1_CH3, 		7, TCC0_CH3, 		3},
+		{ PORTA,  24, TC5_CH0,		0, TCC2_CH2, 		2, NOT_ON_TIMER, 	0}, // PDEC0
+		{ PORTA,  25, TC5_CH1,		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0}, // PDEC1
+		{ PORTB,  22, TC7_CH0,		0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0}, // PDEC2
+		{ PORTB,  23, TC7_CH1,		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER,	0}, // PDEC0
+		{ PORTB,  24, NOT_ON_TIMER,	0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0}, // PDEC1
+		{ PORTB,  25, NOT_ON_TIMER,	0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0}, // PDEC2
+		{ PORTB,  26, NOT_ON_TIMER,	0, TCC1_CH2, 		2, NOT_ON_TIMER, 	0},
+		{ PORTB,  27, NOT_ON_TIMER,	0, TCC1_CH3, 		3, NOT_ON_TIMER, 	0},
+		{ PORTB,  28, NOT_ON_TIMER,	0, TCC1_CH0, 		4, NOT_ON_TIMER, 	0},
+		{ PORTB,  29, NOT_ON_TIMER,	1, TCC1_CH1, 		5, NOT_ON_TIMER,	0},
+		// PC24-PC28, PA27, RESET -> no TC/TCC peripherals
+		{ PORTA,  30, TC6_CH0,		0, TCC2_CH0, 		0, NOT_ON_TIMER, 	0},
+		{ PORTA,  31, TC6_CH1,		1, TCC2_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTB,  30, TC0_CH0,		0, TCC4_CH0, 		0, TCC0_CH0, 		6},
+		{ PORTB,  31, TC0_CH1,		1, TCC4_CH1, 		1, TCC0_CH1, 		7},
+		// PC30, PC31 -> no TC/TCC peripherals
+		{ PORTB,   0, TC7_CH0,		0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTB,   1, TC7_CH1,		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER,	0},
+		{ PORTB,   2, TC6_CH0,		0, TCC2_CH2, 		2, NOT_ON_TIMER,	0},
+
+};
+
+wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0};
+
+uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM, TCC3_CC_NUM, TCC4_CC_NUM };
+
+struct wo_association& getWOAssociation(EPortType port, uint32_t pin) {
+	for (int i=0;i<NUM_WO_ASSOCIATIONS;i++) {
+		if (WO_associations[i].port==port && WO_associations[i].pin==pin)
+			return WO_associations[i];
+	}
+	return ASSOCIATION_NOT_FOUND;
+};
+
+
+EPioType getPeripheralOfPermutation(int permutation, int pin_position) {
+	return ((permutation>>pin_position)&0x01)==0x1?PIO_TCC_PDEC:PIO_TIMER_ALT;
+}
+
+
+
+void syncTCC(Tcc* TCCx) {
+	while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
+}
+
+
+
+
+void writeSAMDDutyCycle(tccConfiguration* info, float dc) {
+	uint8_t tccn = GetTCNumber(info->tcc.chaninfo);
+	uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo);
+	if (tccn<TCC_INST_NUM) {
+		Tcc* tcc = (Tcc*)GetTC(info->tcc.chaninfo);
+		// set via CCBUF
+//		while ( (tcc->SYNCBUSY.vec.CC & (0x1<<chan)) > 0 );
+		tcc->CCBUF[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // TODO pwm frequency!
+	}
+	else { 
+		// we don't support the TC channels on SAMD51, isn't worth it.
+	}
+}
+
+
+#define DPLL_CLOCK_NUM 2  	// use GCLK2
+#define PWM_CLOCK_NUM 3  	// use GCLK3
+
+
+/**
+ * Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that
+ * any compatible pin combination can be used without having to worry about configuring different
+ * clocks.
+ */
+void configureSAMDClock() {
+	if (!SAMDClockConfigured) {
+		SAMDClockConfigured = true;						// mark clock as configured
+		for (int i=0;i<TCC_INST_NUM;i++)				// mark all the TCCs as not yet configured
+			tccConfigured[i] = false;
+
+		// GCLK->GENCTRL[DPLL_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1)
+		// 								  | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val);
+		// while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<DPLL_CLOCK_NUM));
+
+		// GCLK->PCHCTRL[1].reg = GCLK_PCHCTRL_GEN(DPLL_CLOCK_NUM)|GCLK_PCHCTRL_CHEN;
+		// while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<DPLL_CLOCK_NUM));
+
+		// OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = 0;
+		// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
+
+		// OSCCTRL->Dpll[0].DPLLCTRLB.bit.REFCLK = OSCCTRL_DPLLCTRLB_REFCLK_GCLK_Val;
+		// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
+		// OSCCTRL->Dpll[0].DPLLRATIO.reg = 3;
+		// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
+
+		// OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = 1;
+		// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
+
+		GCLK->GENCTRL[PWM_CLOCK_NUM].bit.GENEN = 0;
+		while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
+
+		GCLK->GENCTRL[PWM_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) | GCLK_GENCTRL_IDC
+										 //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val);
+	 	 	 	 	 	 	 	 	 	 | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val);
+		while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock...");
+#endif
+	}
+}
+
+
+
+
+
+/**
+ * Configure a TCC unit
+ * pwm_frequency is fixed at 24kHz for now. We could go slower, but going
+ * faster won't be possible without sacrificing resolution.
+ */
+void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) {
+	// TODO for the moment we ignore the frequency...
+	if (!tccConfigured[tccConfig.tcc.tccn]) {
+		uint32_t GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_IDs[tccConfig.tcc.tccn];
+		GCLK->PCHCTRL[GCLK_CLKCTRL_ID_ofthistcc].reg = GCLK_PCHCTRL_GEN(PWM_CLOCK_NUM)|GCLK_PCHCTRL_CHEN;
+		while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
+	}
+
+	if (tccConfig.tcc.tccn<TCC_INST_NUM) {
+		Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
+
+		tcc->CTRLA.bit.ENABLE = 0; //switch off tcc
+		while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
+
+		uint8_t invenMask = ~(1<<tccConfig.tcc.chan);	// negate (invert) the signal if needed
+		uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
+		tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
+		syncTCC(tcc); // wait for sync
+
+		// work out pwm resolution for desired frequency and constrain to max/min values
+		long pwm_resolution = (SIMPLEFOC_SAMD51_DPLL_FREQ/2) / pwm_frequency;
+		if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) 
+			pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION;
+		if (pwm_resolution<SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION) 
+			pwm_resolution = SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION;
+		// store for later use
+		tccConfig.pwm_res = pwm_resolution;
+
+		if (hw6pwm>0.0) {
+			tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
+			tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
+			tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
+			syncTCC(tcc); // wait for sync
+		}
+
+		if (!tccConfigured[tccConfig.tcc.tccn]) {
+			tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVE_WAVEGEN_DSTOP;   // Set wave form configuration - TODO check this... why set like this?
+			while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync
+
+			tcc->PER.reg = pwm_resolution - 1;                 // Set counter Top using the PER register
+			while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync
+
+			// set all channels to 0%
+			for (int i=0;i<TCC_CHANNEL_COUNT[tccConfig.tcc.tccn];i++) {
+				tcc->CC[i].reg = 0;					// start off at 0% duty cycle
+				uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+i);
+				while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
+			}
+		}
+
+		// Enable TCC
+		tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz
+		while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC ");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("]  pwm res ");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution);		
+#endif
+	}
+	else if (tccConfig.tcc.tccn>=TCC_INST_NUM) {
+		//Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo);
+
+		// disable
+		// tc->COUNT8.CTRLA.bit.ENABLE = 0;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // unfortunately we need the 8-bit counter mode to use the PER register...
+		// tc->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM ;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // meaning prescaler of 8, since TC-Unit has no up/down mode, and has resolution of 250 rather than 1000...
+		// tc->COUNT8.CTRLA.bit.PRESCALER = TC_CTRLA_PRESCALER_DIV8_Val ;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // period is 250, period cannot be higher than 256!
+		// tc->COUNT8.PER.reg = SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // initial duty cycle is 0
+		// tc->COUNT8.CC[tccConfig.tcc.chan].reg = 0;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // enable
+		// tc->COUNT8.CTRLA.bit.ENABLE = 1;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+
+	#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Not initialized: TC ");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TC units not supported on SAMD51");
+	#endif
+	}
+
+	// set as configured
+	tccConfigured[tccConfig.tcc.tccn] = true;
+
+
+}
+
+
+
+
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/samd_mcu.cpp

@@ -0,0 +1,903 @@
+
+
+
+#include "./samd_mcu.h"
+
+#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)
+
+
+
+/**
+ * Global state
+ */
+tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS];
+uint8_t numTccPinConfigurations = 0;
+bool SAMDClockConfigured = false;
+bool tccConfigured[TCC_INST_NUM+TC_INST_NUM];
+
+
+
+
+
+/**
+ * Attach the TCC to the pin
+ */
+bool attachTCC(tccConfiguration& tccConfig) {
+	if (numTccPinConfigurations>=SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS)
+		return false;
+    pinMode(tccConfig.pin, OUTPUT);
+
+    pinPeripheral(tccConfig.pin, tccConfig.peripheral);
+    tccPinConfigurations[numTccPinConfigurations++] = tccConfig;
+    return true;
+}
+
+
+
+
+
+int getPermutationNumber(int pins) {
+	int num = 1;
+	for (int i=0;i<pins;i++)
+		num *= NUM_PIO_TIMER_PERIPHERALS;
+	return num;
+}
+
+
+
+
+/**
+ * Check if the configuration is in use already.
+ */
+bool inUse(tccConfiguration& tccConfig) {
+	for (int i=0;i<numTccPinConfigurations;i++) {
+		if (tccPinConfigurations[i].tcc.chaninfo==tccConfig.tcc.chaninfo)
+			return true;
+	}
+	return false;
+}
+
+
+tccConfiguration* getTccPinConfiguration(uint8_t pin) {
+	for (int i=0;i<numTccPinConfigurations;i++)
+		if (tccPinConfigurations[i].pin==pin)
+			return &tccPinConfigurations[i];
+	return NULL;
+}
+
+
+
+
+
+/**
+ * Get the TCC channel associated with that pin
+ */
+tccConfiguration getTCCChannelNr(int pin, EPioType peripheral) {
+	tccConfiguration result;
+	result.pin = pin;
+	result.peripheral  = peripheral;
+	result.tcc.tccn = -2;
+	result.tcc.chan = -2;
+	const PinDescription& pinDesc = g_APinDescription[pin];
+	struct wo_association& association = getWOAssociation(pinDesc.ulPort, pinDesc.ulPin);
+	if (association.port==NOT_A_PORT)
+		return result; // could not find the port/pin
+	if (peripheral==PIO_TIMER) {
+		result.tcc.chaninfo = association.tccE;
+		result.wo = association.woE;
+	}
+	else if (peripheral==PIO_TIMER_ALT) {
+		result.tcc.chaninfo = association.tccF;
+		result.wo = association.woF;
+	}
+#if defined(_SAMD51_)||defined(_SAME51_)
+	else if (peripheral==PIO_TCC_PDEC) {
+		result.tcc.chaninfo = association.tccG;
+		result.wo = association.woG;
+	}
+#endif
+	return result;
+}
+
+
+
+
+
+bool checkPeripheralPermutationSameTCC6(tccConfiguration& pinAh, tccConfiguration& pinAl, tccConfiguration& pinBh, tccConfiguration& pinBl, tccConfiguration& pinCh, tccConfiguration& pinCl) {
+	if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl))
+		return false;
+
+	if (pinAh.tcc.tccn<0 || pinAh.tcc.tccn!=pinAl.tcc.tccn || pinAh.tcc.tccn!=pinBh.tcc.tccn || pinAh.tcc.tccn!=pinBl.tcc.tccn
+		|| pinAh.tcc.tccn!=pinCh.tcc.tccn || pinAh.tcc.tccn!=pinCl.tcc.tccn || pinAh.tcc.tccn>=TCC_INST_NUM)
+		return false;
+
+	if (pinAh.tcc.chan==pinBh.tcc.chan || pinAh.tcc.chan==pinBl.tcc.chan || pinAh.tcc.chan==pinCh.tcc.chan || pinAh.tcc.chan==pinCl.tcc.chan)
+		return false;
+	if (pinBh.tcc.chan==pinCh.tcc.chan || pinBh.tcc.chan==pinCl.tcc.chan)
+		return false;
+	if (pinAl.tcc.chan==pinBh.tcc.chan || pinAl.tcc.chan==pinBl.tcc.chan || pinAl.tcc.chan==pinCh.tcc.chan || pinAl.tcc.chan==pinCl.tcc.chan)
+		return false;
+	if (pinBl.tcc.chan==pinCh.tcc.chan || pinBl.tcc.chan==pinCl.tcc.chan)
+		return false;
+
+	if (pinAh.tcc.chan!=pinAl.tcc.chan || pinBh.tcc.chan!=pinBl.tcc.chan || pinCh.tcc.chan!=pinCl.tcc.chan)
+		return false;
+	if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo!=pinCl.wo)
+		return false;
+
+	return true;
+}
+
+
+
+
+bool checkPeripheralPermutationCompatible(tccConfiguration pins[], uint8_t num) {
+	for (int i=0;i<num;i++)
+		if (pins[i].tcc.tccn<0)
+			return false;
+	for (int i=0;i<num-1;i++)
+		for (int j=i+1;j<num;j++)
+			if (pins[i].tcc.chaninfo==pins[j].tcc.chaninfo)
+				return false;
+	for (int i=0;i<num;i++)
+		if (inUse(pins[i]))
+			return false;
+	return true;
+}
+
+
+
+
+
+bool checkPeripheralPermutationCompatible6(tccConfiguration& pinAh, tccConfiguration& pinAl, tccConfiguration& pinBh, tccConfiguration& pinBl, tccConfiguration& pinCh, tccConfiguration& pinCl) {
+	// check we're valid PWM pins
+	if (pinAh.tcc.tccn<0 || pinAl.tcc.tccn<0 || pinBh.tcc.tccn<0 || pinBl.tcc.tccn<0 || pinCh.tcc.tccn<0 || pinCl.tcc.tccn<0)
+		return false;
+	// only TCC units for 6-PWM
+	if (pinAh.tcc.tccn>=TCC_INST_NUM || pinAl.tcc.tccn>=TCC_INST_NUM || pinBh.tcc.tccn>=TCC_INST_NUM
+			|| pinBl.tcc.tccn>=TCC_INST_NUM || pinCh.tcc.tccn>=TCC_INST_NUM || pinCl.tcc.tccn>=TCC_INST_NUM)
+		return false;
+
+	// check we're not in use
+	if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl))
+		return false;
+
+	// check pins are all different tccs/channels
+	if (pinAh.tcc.chaninfo==pinBh.tcc.chaninfo || pinAh.tcc.chaninfo==pinBl.tcc.chaninfo || pinAh.tcc.chaninfo==pinCh.tcc.chaninfo || pinAh.tcc.chaninfo==pinCl.tcc.chaninfo)
+		return false;
+	if (pinAl.tcc.chaninfo==pinBh.tcc.chaninfo || pinAl.tcc.chaninfo==pinBl.tcc.chaninfo || pinAl.tcc.chaninfo==pinCh.tcc.chaninfo || pinAl.tcc.chaninfo==pinCl.tcc.chaninfo)
+		return false;
+	if (pinBh.tcc.chaninfo==pinCh.tcc.chaninfo || pinBh.tcc.chaninfo==pinCl.tcc.chaninfo)
+		return false;
+	if (pinBl.tcc.chaninfo==pinCh.tcc.chaninfo || pinBl.tcc.chaninfo==pinCl.tcc.chaninfo)
+		return false;
+
+	// check H/L pins are on same timer
+	if (pinAh.tcc.tccn!=pinAl.tcc.tccn || pinBh.tcc.tccn!=pinBl.tcc.tccn || pinCh.tcc.tccn!=pinCl.tcc.tccn)
+		return false;
+
+	// check H/L pins aren't on both the same timer and wo
+	if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo==pinCl.wo)
+		return false;
+
+	return true;
+}
+
+
+
+
+
+int checkHardware6PWM(const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+	for (int i=0;i<64;i++) {
+		tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0));
+		tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1));
+		tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2));
+		tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3));
+		tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4));
+		tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5));
+		if (checkPeripheralPermutationSameTCC6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl))
+			return i;
+	}
+	return -1;
+}
+
+
+
+
+int checkSoftware6PWM(const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+	for (int i=0;i<64;i++) {
+		tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0));
+		tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1));
+		tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2));
+		tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3));
+		tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4));
+		tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5));
+		if (checkPeripheralPermutationCompatible6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl))
+			return i;
+	}
+	return -1;
+}
+
+
+
+int scorePermutation(tccConfiguration pins[], uint8_t num) {
+	uint32_t usedtccs = 0;
+	for (int i=0;i<num;i++)
+		usedtccs |= (1<<pins[i].tcc.tccn);
+	int score = 0;
+	for (int i=0;i<TCC_INST_NUM;i++){
+		if (usedtccs&0x1)
+			score+=1;
+		usedtccs = usedtccs>>1;
+	}
+	for (int i=0;i<TC_INST_NUM;i++){
+		if (usedtccs&0x1)
+			score+=(num+1);
+		usedtccs = usedtccs>>1;
+	}
+	return score;
+}
+
+
+
+
+
+
+
+
+int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguration[], uint8_t) ) {
+	tccConfiguration tccConfs[num];
+	int best = -1;
+	int bestscore = 1000000;
+	for (int i=0;i<(0x1<<num);i++) {
+		for (int j=0;j<num;j++)
+			tccConfs[j] = getTCCChannelNr(pins[j], getPeripheralOfPermutation(i, j));
+		if (checkFunc(tccConfs, num)) {
+			int score = scorePermutation(tccConfs, num);
+			if (score<bestscore) {
+				bestscore = score;
+				best = i;
+			}
+		}
+	}
+	return best;
+}
+
+
+
+
+
+
+
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 2PWM setting
+ * - hardware specific
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pinA pinA bldc driver
+ * @param pinB pinB bldc driver
+ */
+void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	printAllPinInfos();
+#endif
+	int pins[2] = { pinA, pinB };
+	int compatibility = checkPermutations(2, pins, checkPeripheralPermutationCompatible);
+	if (compatibility<0) {
+		// no result!
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
+#endif
+		return;
+	}
+
+	tccConfiguration tccConfs[2] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)),
+								     getTCCChannelNr(pinB, getPeripheralOfPermutation(compatibility, 1)) };
+
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score=");
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2));
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")");
+	printTCCConfiguration(tccConfs[0]);
+	printTCCConfiguration(tccConfs[1]);
+#endif
+
+	// attach pins to timer peripherals
+	attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
+	attachTCC(tccConfs[1]);
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins...");
+#endif
+
+	// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
+	// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
+	configureSAMDClock();
+
+	if (pwm_frequency==NOT_SET) {
+		// use default frequency
+		pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
+	}
+
+	// configure the TCC (waveform, top-value, pre-scaler = frequency)
+	configureTCC(tccConfs[0], pwm_frequency);
+	configureTCC(tccConfs[1], pwm_frequency);
+	getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res;
+	getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
+#endif
+
+	return; // Someone with a stepper-setup who can test it?
+}
+
+
+
+
+
+
+
+
+
+
+
+
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - BLDC driver - 3PWM setting
+ * - hardware specific
+ *
+ * SAMD21 will support up to 2 BLDC motors in 3-PWM:
+ *  one on TCC0 using 3 of the channels 0,1,2 or 3
+ *  one on TCC3 using 3 of the channels 0,1,2 or 3
+ * i.e. 8 different pins can be used, but only 4 different signals (WO[x]) on those 8 pins
+ *  WO[0] and WO[4] are the same
+ *  WO[1] and WO[5] are the same
+ *  WO[2] and WO[6] are the same
+ *  WO[3] and WO[7] are the same
+ *
+ * If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x]
+ * signal is on which pin of the Nano. You can drive one motor on TCC0. For other boards, consult their documentation.
+ *
+ * Note:
+ * That's if we want to keep the signals strictly in sync.
+ *
+ * If we can accept out-of-sync PWMs on the different phases, we could drive up to 4 BLDCs in 3-PWM mode,
+ * using all the TCC channels. (TCC0 & TCC3 - 4 channels each, TCC1 & TCC2 - 2 channels each)
+ *
+ * All channels will use the same resolution, prescaler and clock, but they will have different start-times leading
+ * to misaligned signals.
+ *
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pinA pinA bldc driver
+ * @param pinB pinB bldc driver
+ * @param pinC pinC bldc driver
+ */
+void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) {
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	printAllPinInfos();
+#endif
+	int pins[3] = { pinA, pinB, pinC };
+	int compatibility = checkPermutations(3, pins, checkPeripheralPermutationCompatible);
+	if (compatibility<0) {
+		// no result!
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
+#endif
+		return;
+	}
+
+	tccConfiguration tccConfs[3] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)),
+									 getTCCChannelNr(pinB, getPeripheralOfPermutation(compatibility, 1)),
+								     getTCCChannelNr(pinC, getPeripheralOfPermutation(compatibility, 2)) };
+
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score=");
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3));
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")");
+	printTCCConfiguration(tccConfs[0]);
+	printTCCConfiguration(tccConfs[1]);
+	printTCCConfiguration(tccConfs[2]);
+#endif
+
+	// attach pins to timer peripherals
+	attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
+	attachTCC(tccConfs[1]);
+	attachTCC(tccConfs[2]);
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins...");
+#endif
+
+	// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
+	// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
+	configureSAMDClock();
+
+	if (pwm_frequency==NOT_SET) {
+		// use default frequency
+		pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
+	}
+
+	// configure the TCC (waveform, top-value, pre-scaler = frequency)
+	configureTCC(tccConfs[0], pwm_frequency);
+	configureTCC(tccConfs[1], pwm_frequency);
+	configureTCC(tccConfs[2], pwm_frequency);
+	getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res;
+	getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res;
+	getTccPinConfiguration(pinC)->pwm_res = tccConfs[2].pwm_res;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
+#endif
+
+}
+
+
+
+
+
+
+
+
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 4PWM setting
+ * - hardware specific
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pin1A pin1A stepper driver
+ * @param pin1B pin1B stepper driver
+ * @param pin2A pin2A stepper driver
+ * @param pin2B pin2B stepper driver
+ */
+void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	printAllPinInfos();
+#endif
+	int pins[4] = { pin1A, pin1B, pin2A, pin2B };
+	int compatibility = checkPermutations(4, pins, checkPeripheralPermutationCompatible);
+	if (compatibility<0) {
+		// no result!
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
+#endif
+		return;
+	}
+
+	tccConfiguration tccConfs[4] = { getTCCChannelNr(pin1A, getPeripheralOfPermutation(compatibility, 0)),
+									 getTCCChannelNr(pin1B, getPeripheralOfPermutation(compatibility, 1)),
+									 getTCCChannelNr(pin2A, getPeripheralOfPermutation(compatibility, 2)),
+									 getTCCChannelNr(pin2B, getPeripheralOfPermutation(compatibility, 3)) };
+
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score=");
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4));
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")");
+	printTCCConfiguration(tccConfs[0]);
+	printTCCConfiguration(tccConfs[1]);
+	printTCCConfiguration(tccConfs[2]);
+	printTCCConfiguration(tccConfs[3]);
+#endif
+
+	// attach pins to timer peripherals
+	attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
+	attachTCC(tccConfs[1]);
+	attachTCC(tccConfs[2]);
+	attachTCC(tccConfs[3]);
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins...");
+#endif
+
+	// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
+	// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
+	configureSAMDClock();
+
+	if (pwm_frequency==NOT_SET) {
+		// use default frequency
+		pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
+	}
+
+	// configure the TCC (waveform, top-value, pre-scaler = frequency)
+	configureTCC(tccConfs[0], pwm_frequency);
+	configureTCC(tccConfs[1], pwm_frequency);
+	configureTCC(tccConfs[2], pwm_frequency);
+	configureTCC(tccConfs[3], pwm_frequency);
+	getTccPinConfiguration(pin1A)->pwm_res = tccConfs[0].pwm_res;
+	getTccPinConfiguration(pin2A)->pwm_res = tccConfs[1].pwm_res;
+	getTccPinConfiguration(pin1B)->pwm_res = tccConfs[2].pwm_res;
+	getTccPinConfiguration(pin2B)->pwm_res = tccConfs[3].pwm_res;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
+#endif
+
+	return; // Someone with a stepper-setup who can test it?
+}
+
+
+
+
+
+
+
+
+
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - BLDC driver - 6PWM setting
+ * - hardware specific
+ *
+ * SAMD21 will support up to 2 BLDC motors in 6-PWM:
+ *  one on TCC0 using 3 of the channels 0,1,2 or 3
+ *  one on TCC3 using 3 of the channels 0,1,2 or 3
+ * i.e. 6 out of 8 pins must be used, in the following high/low side pairs:
+ *  WO[0] & WO[4]  (high side & low side)
+ *  WO[1] & WO[5]
+ *  WO[2] & WO[6]
+ *  WO[3] & WO[7]
+ *
+ * If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x]
+ * signal is on which pin of the Nano. You can drive 1 BLDC on TCC0. For other boards, consult their documentation.
+ *
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param dead_zone  duty cycle protection zone [0, 1] - both low and high side low - if applicable
+ * @param pinA_h pinA high-side bldc driver
+ * @param pinA_l pinA low-side bldc driver
+ * @param pinB_h pinA high-side bldc driver
+ * @param pinB_l pinA low-side bldc driver
+ * @param pinC_h pinA high-side bldc driver
+ * @param pinC_l pinA low-side bldc driver
+ *
+ * @return 0 if config good, -1 if failed
+ */
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+	// we want to use a TCC channel with 1 non-inverted and 1 inverted output for each phase, with dead-time insertion
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	printAllPinInfos();
+#endif
+	int compatibility = checkHardware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
+	if (compatibility<0) {
+		compatibility = checkSoftware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
+		if (compatibility<0) {
+			// no result!
+#ifdef SIMPLEFOC_SAMD_DEBUG
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!");
+#endif
+			return -1;
+		}
+	}
+
+	tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(compatibility, 0));
+	tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(compatibility, 1));
+	tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(compatibility, 2));
+	tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(compatibility, 3));
+	tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(compatibility, 4));
+	tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5));
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: ");
+	printTCCConfiguration(pinAh);
+	printTCCConfiguration(pinAl);
+	printTCCConfiguration(pinBh);
+	printTCCConfiguration(pinBl);
+	printTCCConfiguration(pinCh);
+	printTCCConfiguration(pinCl);
+#endif
+
+	// attach pins to timer peripherals
+	bool allAttached = true;
+	allAttached |= attachTCC(pinAh); // in theory this can fail, but there is no way to signal it...
+	allAttached |= attachTCC(pinAl);
+	allAttached |= attachTCC(pinBh);
+	allAttached |= attachTCC(pinBl);
+	allAttached |= attachTCC(pinCh);
+	allAttached |= attachTCC(pinCl);
+	if (!allAttached)
+		return -1;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins...");
+#endif
+	// set up clock - if we did this right it should be possible to get all TCC units synchronized?
+	// e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API
+	configureSAMDClock();
+
+	if (pwm_frequency==NOT_SET) {
+		// use default frequency
+		pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
+	}
+
+	// configure the TCC(s)
+	configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1);
+	if ((pinAh.tcc.chaninfo!=pinAl.tcc.chaninfo))
+		configureTCC(pinAl, pwm_frequency, true, -1.0);
+	configureTCC(pinBh, pwm_frequency, false, (pinBh.tcc.chaninfo==pinBl.tcc.chaninfo)?dead_zone:-1);
+	if ((pinBh.tcc.chaninfo!=pinBl.tcc.chaninfo))
+		configureTCC(pinBl, pwm_frequency, true, -1.0);
+	configureTCC(pinCh, pwm_frequency, false, (pinCh.tcc.chaninfo==pinCl.tcc.chaninfo)?dead_zone:-1);
+	if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo))
+		configureTCC(pinCl, pwm_frequency, true, -1.0);
+	getTccPinConfiguration(pinA_h)->pwm_res = pinAh.pwm_res;
+	getTccPinConfiguration(pinA_l)->pwm_res = pinAh.pwm_res; // use the high phase resolution, in case we didn't set it
+	getTccPinConfiguration(pinB_h)->pwm_res = pinBh.pwm_res;
+	getTccPinConfiguration(pinB_l)->pwm_res = pinBh.pwm_res;
+	getTccPinConfiguration(pinC_h)->pwm_res = pinCh.pwm_res;
+	getTccPinConfiguration(pinC_l)->pwm_res = pinCh.pwm_res;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs...");
+#endif
+
+	return 0;
+}
+
+
+
+
+/**
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - Stepper driver - 2PWM setting
+ * - hardware specific
+ *
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param pinA  phase A hardware pin number
+ * @param pinB  phase B hardware pin number
+ */
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB) {
+	tccConfiguration* tccI = getTccPinConfiguration(pinA);
+	writeSAMDDutyCycle(tccI, dc_a);
+	tccI = getTccPinConfiguration(pinB);
+	writeSAMDDutyCycle(tccI, dc_b);
+	return;
+}
+
+
+
+
+
+/**
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - BLDC driver - 3PWM setting
+ * - hardware specific
+ *
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param dc_c  duty cycle phase C [0, 1]
+ * @param pinA  phase A hardware pin number
+ * @param pinB  phase B hardware pin number
+ * @param pinC  phase C hardware pin number
+ */
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC) {
+	tccConfiguration* tccI = getTccPinConfiguration(pinA);
+	writeSAMDDutyCycle(tccI, dc_a);
+	tccI = getTccPinConfiguration(pinB);
+	writeSAMDDutyCycle(tccI, dc_b);
+	tccI = getTccPinConfiguration(pinC);
+	writeSAMDDutyCycle(tccI, dc_c);
+	return;
+}
+
+
+
+
+/**
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - Stepper driver - 4PWM setting
+ * - hardware specific
+ *
+ * @param dc_1a  duty cycle phase 1A [0, 1]
+ * @param dc_1b  duty cycle phase 1B [0, 1]
+ * @param dc_2a  duty cycle phase 2A [0, 1]
+ * @param dc_2b  duty cycle phase 2B [0, 1]
+ * @param pin1A  phase 1A hardware pin number
+ * @param pin1B  phase 1B hardware pin number
+ * @param pin2A  phase 2A hardware pin number
+ * @param pin2B  phase 2B hardware pin number
+ */
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+	tccConfiguration* tccI = getTccPinConfiguration(pin1A);
+	writeSAMDDutyCycle(tccI, dc_1a);
+	tccI = getTccPinConfiguration(pin2A);
+	writeSAMDDutyCycle(tccI, dc_2a);
+	tccI = getTccPinConfiguration(pin1B);
+	writeSAMDDutyCycle(tccI, dc_1b);
+	tccI = getTccPinConfiguration(pin2B);
+	writeSAMDDutyCycle(tccI, dc_2b);
+	return;
+}
+
+
+
+
+/**
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - BLDC driver - 6PWM setting
+ * - hardware specific
+ *
+ * Note: dead-time must be setup in advance, so parameter "dead_zone" is ignored
+ *       the low side pins are automatically driven by the SAMD DTI module, so it is enough to set the high-side
+ *       duty cycle.
+ *       No sanity checks are perfomed to ensure the pinA, pinB, pinC are the same pins you used in configure method...
+ *       so use appropriately.
+ *
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param dc_c  duty cycle phase C [0, 1]
+ * @param dead_zone  duty cycle protection zone [0, 1] - both low and high side low
+ * @param pinA_h  phase A high-side hardware pin number
+ * @param pinA_l  phase A low-side hardware pin number
+ * @param pinB_h  phase B high-side hardware pin number
+ * @param pinB_l  phase B low-side hardware pin number
+ * @param pinC_h  phase C high-side hardware pin number
+ * @param pinC_l  phase C low-side hardware pin number
+ *
+ */
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+	tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h);
+	tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l);
+	if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
+		// low-side on a different pin of same TCC - do dead-time in software...
+		float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1));
+		if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
+		writeSAMDDutyCycle(tcc1, dc_a);
+		writeSAMDDutyCycle(tcc2, ls);
+	}
+	else
+		writeSAMDDutyCycle(tcc1, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly
+
+	tcc1 = getTccPinConfiguration(pinB_h);
+	tcc2 = getTccPinConfiguration(pinB_l);
+	if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
+		float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1));
+		if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
+		writeSAMDDutyCycle(tcc1, dc_b);
+		writeSAMDDutyCycle(tcc2, ls);
+	}
+	else
+		writeSAMDDutyCycle(tcc1, dc_b);
+
+	tcc1 = getTccPinConfiguration(pinC_h);
+	tcc2 = getTccPinConfiguration(pinC_l);
+	if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
+		float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1));
+		if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
+		writeSAMDDutyCycle(tcc1, dc_c);
+		writeSAMDDutyCycle(tcc2, ls);
+	}
+	else
+		writeSAMDDutyCycle(tcc1, dc_c);
+	return;
+}
+
+
+
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+
+/**
+ * Prints a table of pin assignments for your SAMD MCU. Very useful since the
+ * board pinout descriptions and variant.cpp are usually quite wrong, and this
+ * saves you hours of cross-referencing with the datasheet.
+ */
+void printAllPinInfos() {
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println();
+	for (uint8_t pin=0;pin<PINS_COUNT;pin++) {
+		const PinDescription& pinDesc = g_APinDescription[pin];
+		wo_association& association = getWOAssociation(pinDesc.ulPort, pinDesc.ulPin);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Pin ");
+		if (pin<10) SIMPLEFOC_SAMD_DEBUG_SERIAL.print("0");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(pin);
+		switch (pinDesc.ulPort) {
+			case NOT_A_PORT: SIMPLEFOC_SAMD_DEBUG_SERIAL.print("    "); break;
+			case PORTA: SIMPLEFOC_SAMD_DEBUG_SERIAL.print("  PA"); break;
+			case PORTB: SIMPLEFOC_SAMD_DEBUG_SERIAL.print("  PB"); break;
+			case PORTC: SIMPLEFOC_SAMD_DEBUG_SERIAL.print("  PC"); break;
+#if defined(_SAMD51_)||defined(_SAME51_)
+			case PORTD: SIMPLEFOC_SAMD_DEBUG_SERIAL.print("  PD"); break;
+#endif
+		}
+		if (pinDesc.ulPin <10) SIMPLEFOC_SAMD_DEBUG_SERIAL.print("0");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(pinDesc.ulPin);
+
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("  E=");
+		if (association.tccE>=0) {
+			int tcn = GetTCNumber(association.tccE);
+			if (tcn>=TCC_INST_NUM)
+				SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" TC");
+			else
+				SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TCC");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tcn);
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.tccE));
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.woE));
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("]");
+			if (tcn<10)
+				SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ");
+		}
+		else
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("  None    ");
+
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F=");
+		if (association.tccF>=0) {
+			int tcn = GetTCNumber(association.tccF);
+			if (tcn>=TCC_INST_NUM)
+				SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" TC");
+			else
+				SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TCC");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tcn);
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.tccF));
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.woF));
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("]");
+			if (tcn<10)
+				SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ");
+		}
+		else
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("  None    ");
+
+#if defined(_SAMD51_)||defined(_SAME51_)
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G=");
+		if (association.tccG>=0) {
+			int tcn = GetTCNumber(association.tccG);
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TCC");
+			if (tcn<10)
+				SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tcn);
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.tccG));
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.print(GetTCChannelNumber(association.woG));
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]");
+		}
+		else
+			SIMPLEFOC_SAMD_DEBUG_SERIAL.println("  None ");
+#else
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println("");
+#endif
+
+	}
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.println();
+}
+
+
+
+
+
+void printTCCConfiguration(tccConfiguration& info) {
+	SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.pin);
+	if (info.tcc.tccn>=TCC_INST_NUM)
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(":  TC Peripheral");
+	else
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral");
+	switch (info.peripheral) {
+	case PIO_TIMER:
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break;
+	case PIO_TIMER_ALT:
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break;
+#if defined(_SAMD51_)||defined(_SAME51_)
+	case PIO_TCC_PDEC:
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break;
+#endif
+	default:
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break;
+	}
+	if (info.tcc.tccn>=0) {
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print("[");
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo);
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]");
+	}
+	else
+		SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None");
+}
+
+
+
+#endif
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/samd_mcu.h

@@ -0,0 +1,120 @@
+
+#ifndef SAMD_MCU_H
+#define SAMD_MCU_H
+
+
+// uncomment to enable debug output to Serial port
+// #define SIMPLEFOC_SAMD_DEBUG
+#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL)
+#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial
+#endif
+
+#include "../hardware_api.h"
+
+
+#if defined(__SAME51J19A__) || defined(__ATSAME51J19A__)
+#ifndef _SAME51_
+#define _SAME51_
+#endif
+#endif
+
+
+#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)
+
+
+#include "Arduino.h"
+#include "variant.h"
+#include "wiring_private.h"
+
+
+#ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION
+#define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000
+#endif
+
+#define SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ 24000
+// arbitrary maximum. On SAMD51 with 120MHz clock this means 2kHz minimum pwm frequency
+#define SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION 30000
+// lets not go too low - 400 with clock speed of 120MHz on SAMD51 means 150kHz maximum PWM frequency...
+//						 400 with 48MHz clock on SAMD21 means 60kHz maximum PWM frequency...
+#define SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION 400 
+// this is the most we can support on the TC units
+#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250
+
+#ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS
+#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24
+#endif
+
+
+
+struct tccConfiguration {
+	uint8_t pin;
+	EPioType peripheral; // 1=true, 0=false
+	uint8_t wo;
+	union tccChanInfo {
+		struct {
+			int8_t chan;
+			int8_t tccn;
+		};
+		uint16_t chaninfo;
+	} tcc;
+	uint16_t pwm_res;
+};
+
+
+
+
+
+
+struct wo_association {
+	EPortType port;
+	uint32_t pin;
+	ETCChannel tccE;
+	uint8_t woE;
+	ETCChannel tccF;
+	uint8_t woF;
+#if defined(_SAMD51_)||defined(_SAME51_)
+	ETCChannel tccG;
+	uint8_t woG;
+#endif
+};
+
+
+
+#if defined(_SAMD21_)
+#define NUM_PIO_TIMER_PERIPHERALS 2
+#elif defined(_SAMD51_)||defined(_SAME51_)
+#define NUM_PIO_TIMER_PERIPHERALS 3
+#endif
+
+
+
+/**
+ * Global state
+ */
+extern struct wo_association WO_associations[];
+extern uint8_t TCC_CHANNEL_COUNT[];
+extern tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS];
+extern uint8_t numTccPinConfigurations;
+extern bool SAMDClockConfigured;
+extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM];
+
+
+
+struct wo_association& getWOAssociation(EPortType port, uint32_t pin);
+void writeSAMDDutyCycle(tccConfiguration* info, float dc);
+void configureSAMDClock();
+void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1);
+__inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused));
+EPioType getPeripheralOfPermutation(int permutation, int pin_position);
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+void printTCCConfiguration(tccConfiguration& info);
+void printAllPinInfos();
+#endif
+
+
+
+#endif
+
+
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/stm32_mcu.cpp

@@ -0,0 +1,355 @@
+
+#include "../hardware_api.h"
+
+#if defined(_STM32_DEF_)
+// default pwm parameters
+#define _PWM_RESOLUTION 12 // 12bit
+#define _PWM_RANGE 4095.0 // 2^12 -1 = 4095
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+// 6pwm parameters
+#define _HARDWARE_6PWM 1
+#define _SOFTWARE_6PWM 0
+#define _ERROR_6PWM -1
+
+
+// setting pwm to hardware pin - instead analogWrite()
+void _setPwm(int ulPin, uint32_t value, int resolution)
+{
+  PinName pin = digitalPinToPinName(ulPin);
+  TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
+  uint32_t index = get_timer_index(Instance);
+  HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+
+  uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
+  HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution);
+}
+
+
+// init pin pwm
+HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin)
+{
+  PinName pin = digitalPinToPinName(ulPin);
+  TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
+
+  uint32_t index = get_timer_index(Instance);
+  if (HardwareTimer_Handle[index] == NULL) {
+    HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM));
+    HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+    HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+  }
+  HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+  uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
+  HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin);
+  HT->setOverflow(PWM_freq, HERTZ_FORMAT);
+  HT->pause();
+  HT->refresh();
+  return HT;
+}
+
+
+// init high side pin
+HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin)
+{
+  return _initPinPWM(PWM_freq, ulPin);
+}
+
+// init low side pin
+HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin)
+{
+  PinName pin = digitalPinToPinName(ulPin);
+  TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
+  uint32_t index = get_timer_index(Instance);
+
+  if (HardwareTimer_Handle[index] == NULL) {
+    HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM));
+    HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+    HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+    TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef();
+    sConfigOC.OCMode = TIM_OCMODE_PWM2;
+    sConfigOC.Pulse = 100;
+    sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
+    sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+#if defined(TIM_OCIDLESTATE_SET)
+    sConfigOC.OCIdleState  = TIM_OCIDLESTATE_SET;
+#endif
+#if defined(TIM_OCNIDLESTATE_RESET)
+    sConfigOC.OCNPolarity  = TIM_OCNPOLARITY_HIGH;
+    sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+#endif
+    uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
+    HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel);
+  }
+  HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+  uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
+  HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin);
+  HT->setOverflow(PWM_freq, HERTZ_FORMAT);
+  HT->pause();
+  HT->refresh();
+  return HT;
+}
+
+
+// align the timers to end the init
+void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3)
+{
+  HT1->pause();
+  HT1->refresh();
+  HT2->pause();
+  HT2->refresh();
+  HT3->pause();
+  HT3->refresh();
+  HT1->resume();
+  HT2->resume();
+  HT3->resume();
+}
+
+// align the timers to end the init
+void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4)
+{
+  HT1->pause();
+  HT1->refresh();
+  HT2->pause();
+  HT2->refresh();
+  HT3->pause();
+  HT3->refresh();
+  HT4->pause();
+  HT4->refresh();
+  HT1->resume();
+  HT2->resume();
+  HT3->resume();
+  HT4->resume();
+}
+
+// configure hardware 6pwm interface only one timer with inverted channels
+HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l)
+{
+  
+#if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface 
+  PinName uhPinName = digitalPinToPinName(pinA_h);
+  PinName ulPinName = digitalPinToPinName(pinA_l);
+  PinName vhPinName = digitalPinToPinName(pinB_h);
+  PinName vlPinName = digitalPinToPinName(pinB_l);
+  PinName whPinName = digitalPinToPinName(pinC_h);
+  PinName wlPinName = digitalPinToPinName(pinC_l);
+
+  TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM);
+
+  uint32_t index = get_timer_index(Instance);
+
+  if (HardwareTimer_Handle[index] == NULL) {
+    HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM));
+    HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+    HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1;
+    HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+    ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT);
+  }
+  HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+
+  HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName);
+  HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName);
+  HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName);
+  HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName);
+  HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName);
+  HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName);
+
+  // dead time is set in nanoseconds
+  uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone;
+  uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns);
+  LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear!
+  LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N);
+
+  // Set Trigger out for DMA transfer
+  LL_TIM_SetTriggerOutput(HT->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
+
+  HT->pause();
+  HT->refresh();
+
+  // adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs.
+  HT->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
+  HT->getHandle()->Instance->CNT = TIM1->ARR;
+
+  HT->resume();
+  return HT;
+#else 
+  return nullptr; // return nothing
+#endif
+}
+
+
+// returns 0 if each pair of pwm channels has same channel
+// returns 1 all the channels belong to the same timer - hardware inverted channels
+// returns -1 if neither - avoid configuring - error!!!
+int _interfaceType(const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+  PinName nameAH = digitalPinToPinName(pinA_h);
+  PinName nameAL = digitalPinToPinName(pinA_l);
+  PinName nameBH = digitalPinToPinName(pinB_h);
+  PinName nameBL = digitalPinToPinName(pinB_l);
+  PinName nameCH = digitalPinToPinName(pinC_h);
+  PinName nameCL = digitalPinToPinName(pinC_l);
+  int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM));
+  int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM));
+  int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM));
+  int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM));
+  int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM));
+  int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM));
+
+#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface 
+
+  if(tim1 == tim2 && tim3==tim4  && tim5==tim6)
+    return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer
+  else
+    return _ERROR_6PWM; // might be error avoid configuration
+#else // the rest of stm32 boards
+
+  if(tim1 == tim2 && tim2==tim3 && tim3==tim4  && tim4==tim5 && tim5==tim6)
+    return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer
+  else if(tim1 == tim2 && tim3==tim4  && tim5==tim6)
+    return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer
+  else
+    return _ERROR_6PWM; // might be error avoid configuration
+#endif
+}
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+  if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA);
+  HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB);
+  // allign the timers
+  _alignPWMTimers(HT1, HT2, HT2);
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA);
+  HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB);
+  HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC);
+  // allign the timers
+  _alignPWMTimers(HT1, HT2, HT3);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA);
+  HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB);
+  HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC);
+  HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD);
+  // allign the timers
+  _alignPWMTimers(HT1, HT2, HT3, HT4);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+//- hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // transform duty cycle from [0,1] to [0,4095]
+  _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+  _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+//- hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // transform duty cycle from [0,1] to [0,4095]
+  _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+  _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
+  _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
+}
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+//- hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // transform duty cycle from [0,1] to [0,4095]
+  _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION);
+  _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION);
+  _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION);
+  _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION);
+}
+
+
+
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+  if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  // find configuration
+  int config = _interfaceType(pinA_h, pinA_l,  pinB_h, pinB_l, pinC_h, pinC_l);
+  // configure accordingly
+  switch(config){
+    case _ERROR_6PWM:
+      return -1; // fail
+      break;
+    case _HARDWARE_6PWM:
+      _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
+      break;
+    case _SOFTWARE_6PWM:
+      HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h);
+      _initPinPWMLow(pwm_frequency, pinA_l);
+      HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h);
+      _initPinPWMLow(pwm_frequency, pinB_l);
+      HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h);
+      _initPinPWMLow(pwm_frequency, pinC_l);
+      _alignPWMTimers(HT1, HT2, HT3);
+      break;
+  }
+  return 0; // success
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - BLDC driver - 6PWM setting
+// - hardware specific
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
+  // find configuration
+  int config = _interfaceType(pinA_h, pinA_l,  pinB_h, pinB_l, pinC_h, pinC_l);
+  // set pwm accordingly
+  switch(config){
+    case _HARDWARE_6PWM:
+      _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+      _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
+      _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
+      break;
+    case _SOFTWARE_6PWM:
+      _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+      _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+      _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+      _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+      _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+      _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+      break;
+  }
+}
+#endif

V1.0/Shibo-main-TOMMY/lib/Simple_FOC/src/drivers/hardware_specific/teensy_mcu.cpp

@@ -0,0 +1,77 @@
+#include "../hardware_api.h"
+
+#if defined(__arm__) && defined(CORE_TEENSY)
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin){
+  analogWrite(pin, 0);
+  analogWriteFrequency(pin, freq);
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+  _setHighFrequency(pwm_frequency, pinD);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+}
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pinA, 255.0f*dc_a);
+  analogWrite(pinB, 255.0f*dc_b);
+  analogWrite(pinC, 255.0f*dc_c);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(pin1A, 255.0f*dc_1a);
+  analogWrite(pin1B, 255.0f*dc_1b);
+  analogWrite(pin2A, 255.0f*dc_2a);
+  analogWrite(pin2B, 255.0f*dc_2b);
+}
+
+#endif