v1.3增加一个简单的越障模式

v1.3/lib/Simple_FOC/src/BLDCMotor.cpp

@@ -0,0 +1,670 @@
+#include "BLDCMotor.h"
+
+// BLDCMotor( int pp , float R)
+// - pp            - pole pair number
+// - R             - motor phase resistance
+BLDCMotor::BLDCMotor(int pp, float _R)
+: FOCMotor()
+{
+  // save pole pairs number
+  pole_pairs = pp;
+  // save phase resistance number
+  phase_resistance = _R;
+  // torque control type is voltage by default
+  torque_controller = TorqueControlType::voltage;
+}
+
+
+/**
+	Link the driver which controls the motor
+*/
+void BLDCMotor::linkDriver(BLDCDriver* _driver) {
+  driver = _driver;
+}
+
+// init hardware pins
+void BLDCMotor::init() {
+  if (!driver || !driver->initialized) {
+    motor_status = FOCMotorStatus::motor_init_failed;
+    if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
+  }
+  motor_status = FOCMotorStatus::motor_initializing;
+  if(monitor_port) monitor_port->println(F("MOT: Init"));
+
+  // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit
+  if( !current_sense && _isset(phase_resistance)) {
+    float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case
+    // use it if it is less then voltage_limit set by the user
+    voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit;
+  }
+  // sanity check for the voltage limit configuration
+  if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
+  // constrain voltage for sensor alignment
+  if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
+
+  // update the controller limits
+  if(current_sense){
+    // current control loop controls voltage
+    PID_current_q.limit = voltage_limit;
+    PID_current_d.limit = voltage_limit;
+  }
+  if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){
+    // velocity control loop controls current
+    PID_velocity.limit = current_limit;
+  }else{
+    // velocity control loop controls the voltage
+    PID_velocity.limit = voltage_limit;
+  }
+  P_angle.limit = velocity_limit;
+
+  _delay(500);
+  // enable motor
+  if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
+  enable();
+  _delay(500);
+  motor_status = FOCMotorStatus::motor_uncalibrated;
+}
+
+
+// disable motor driver
+void BLDCMotor::disable()
+{
+  // set zero to PWM
+  driver->setPwm(0, 0, 0);
+  // disable the driver
+  driver->disable();
+  // motor status update
+  enabled = 0;
+}
+// enable motor driver
+void BLDCMotor::enable()
+{
+  // enable the driver
+  driver->enable();
+  // set zero to PWM
+  driver->setPwm(0, 0, 0);
+  // motor status update
+  enabled = 1;
+}
+
+/**
+  FOC functions
+*/
+// FOC initialization function
+int  BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) {
+  int exit_flag = 1;
+
+  motor_status = FOCMotorStatus::motor_calibrating;
+
+  // align motor if necessary
+  // alignment necessary for encoders!
+  if(_isset(zero_electric_offset)){
+    // abosolute zero offset provided - no need to align
+    zero_electric_angle = zero_electric_offset;
+    // set the sensor direction - default CW
+    sensor_direction = _sensor_direction;
+  }
+
+  // sensor and motor alignment - can be skipped
+  // by setting motor.sensor_direction and motor.zero_electric_angle
+  _delay(500);
+  if(sensor){
+    exit_flag *= alignSensor();
+    // added the shaft_angle update
+    sensor->update();
+    shaft_angle = shaftAngle();
+  }else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
+
+  // aligning the current sensor - can be skipped
+  // checks if driver phases are the same as current sense phases
+  // and checks the direction of measuremnt.
+  _delay(500);
+  if(exit_flag){
+    if(current_sense) exit_flag *= alignCurrentSense();
+    else if(monitor_port) monitor_port->println(F("MOT: No current sense."));
+  }
+
+  if(exit_flag){
+    if(monitor_port) monitor_port->println(F("MOT: Ready."));
+    motor_status = FOCMotorStatus::motor_ready;
+  }else{
+    if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
+    disable();
+    motor_status = FOCMotorStatus::motor_calib_failed;
+  }
+
+  return exit_flag;
+}
+
+// Calibarthe the motor and current sense phases
+int BLDCMotor::alignCurrentSense() {
+  int exit_flag = 1; // success
+
+  if(monitor_port) monitor_port->println(F("MOT: Align current sense."));
+
+  // align current sense and the driver
+  exit_flag = current_sense->driverAlign(driver, voltage_sensor_align);
+  if(!exit_flag){
+    // error in current sense - phase either not measured or bad connection
+    if(monitor_port) monitor_port->println(F("MOT: Align error!"));
+    exit_flag = 0;
+  }else{
+    // output the alignment status flag
+    if(monitor_port) monitor_port->print(F("MOT: Success: "));
+    if(monitor_port) monitor_port->println(exit_flag);
+  }
+
+  return exit_flag > 0;
+}
+
+// Encoder alignment to electrical 0 angle
+int BLDCMotor::alignSensor() {
+  int exit_flag = 1; //success
+  if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
+
+  // if unknown natural direction
+  if(!_isset(sensor_direction)){
+    // check if sensor needs zero search
+    if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
+    // stop init if not found index
+    if(!exit_flag) return exit_flag;
+
+    // find natural direction
+    // move one electrical revolution forward
+    for (int i = 0; i <=500; i++ ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f;
+      setPhaseVoltage(voltage_sensor_align, 0,  angle);
+      _delay(2);
+    }
+    // take and angle in the middle
+    sensor->update();
+    float mid_angle = sensor->getAngle();
+    // move one electrical revolution backwards
+    for (int i = 500; i >=0; i-- ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f ;
+      setPhaseVoltage(voltage_sensor_align, 0,  angle);
+      _delay(2);
+    }
+    sensor->update();
+    float end_angle = sensor->getAngle();
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+    // determine the direction the sensor moved
+    if (mid_angle == end_angle) {
+      if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
+      return 0; // failed calibration
+    } else if (mid_angle < end_angle) {
+      if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
+      sensor_direction = Direction::CCW;
+    } else{
+      if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
+      sensor_direction = Direction::CW;
+    }
+    // check pole pair number
+    if(monitor_port) monitor_port->print(F("MOT: PP check: "));
+    float moved =  fabs(mid_angle - end_angle);
+    if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
+      if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
+      if(monitor_port) monitor_port->println(_2PI/moved,4);
+    }else if(monitor_port) monitor_port->println(F("OK!"));
+
+  }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib."));
+
+  // zero electric angle not known
+  if(!_isset(zero_electric_angle)){
+    // align the electrical phases of the motor and sensor
+    // set angle -90(270 = 3PI/2) degrees
+    setPhaseVoltage(voltage_sensor_align, 0,  _3PI_2);
+    _delay(700);
+    // read the sensor
+    sensor->update();
+    // get the current zero electric angle
+    zero_electric_angle = 0;
+    zero_electric_angle = electricalAngle();
+    //zero_electric_angle =  _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
+    _delay(20);
+    if(monitor_port){
+      monitor_port->print(F("MOT: Zero elec. angle: "));
+      monitor_port->println(zero_electric_angle);
+    }
+    // stop everything
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+  }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
+  return exit_flag;
+}
+
+// Encoder alignment the absolute zero angle
+// - to the index
+int BLDCMotor::absoluteZeroSearch() {
+  // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
+  //                    of float is sufficient.
+  if(monitor_port) monitor_port->println(F("MOT: Index search..."));
+  // search the absolute zero with small velocity
+  float limit_vel = velocity_limit;
+  float limit_volt = voltage_limit;
+  velocity_limit = velocity_index_search;
+  voltage_limit = voltage_sensor_align;
+  shaft_angle = 0;
+  while(sensor->needsSearch() && shaft_angle < _2PI){
+    angleOpenloop(1.5f*_2PI);
+    // call important for some sensors not to loose count
+    // not needed for the search
+    sensor->update();
+  }
+  // disable motor
+  setPhaseVoltage(0, 0, 0);
+  // reinit the limits
+  velocity_limit = limit_vel;
+  voltage_limit = limit_volt;
+  // check if the zero found
+  if(monitor_port){
+    if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
+    else monitor_port->println(F("MOT: Success!"));
+  }
+  return !sensor->needsSearch();
+}
+
+// Iterative function looping FOC algorithm, setting Uq on the Motor
+// The faster it can be run the better
+void BLDCMotor::loopFOC() {
+  // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
+  //                 of full rotations otherwise.
+  if (sensor) sensor->update();
+
+  // if open-loop do nothing
+  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
+  
+  // if disabled do nothing
+  if(!enabled) return;
+
+  // Needs the update() to be called first
+  // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() 
+  // which is in range 0-2PI
+  electrical_angle = electricalAngle();
+  switch (torque_controller) {
+    case TorqueControlType::voltage:
+      // no need to do anything really
+      break;
+    case TorqueControlType::dc_current:
+      if(!current_sense) return;
+      // read overall current magnitude
+      current.q = current_sense->getDCCurrent(electrical_angle);
+      // filter the value values
+      current.q = LPF_current_q(current.q);
+      // calculate the phase voltage
+      voltage.q = PID_current_q(current_sp - current.q);
+      voltage.d = 0;
+      break;
+    case TorqueControlType::foc_current:
+      if(!current_sense) return;
+      // read dq currents
+      current = current_sense->getFOCCurrents(electrical_angle);
+      // filter values
+      current.q = LPF_current_q(current.q);
+      current.d = LPF_current_d(current.d);
+      // calculate the phase voltages
+      voltage.q = PID_current_q(current_sp - current.q);
+      voltage.d = PID_current_d(-current.d);
+      break;
+    default:
+      // no torque control selected
+      if(monitor_port) monitor_port->println(F("MOT: no torque control selected!"));
+      break;
+  }
+
+  // set the phase voltage - FOC heart function :)
+  setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
+}
+
+// Iterative function running outer loop of the FOC algorithm
+// Behavior of this function is determined by the motor.controller variable
+// It runs either angle, velocity or torque loop
+// - needs to be called iteratively it is asynchronous function
+// - if target is not set it uses motor.target value
+void BLDCMotor::move(float new_target) {
+
+  // downsampling (optional)
+  if(motion_cnt++ < motion_downsample) return;
+  motion_cnt = 0;
+
+  // shaft angle/velocity need the update() to be called first
+  // get shaft angle
+  // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
+  //                        For this reason it is NOT precise when the angles become large.
+  //                        Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
+  //                        when switching to a 2-component representation.
+  if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) 
+    shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
+  // get angular velocity 
+  shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
+
+  // if disabled do nothing
+  if(!enabled) return;
+  // set internal target variable
+  if(_isset(new_target)) target = new_target;
+
+  switch (controller) {
+    case MotionControlType::torque:
+      if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
+        if(!_isset(phase_resistance))  voltage.q = target;
+        else voltage.q =  target*phase_resistance;
+        voltage.d = 0;
+      }else{
+        current_sp = target; // if current/foc_current torque control
+      }
+      break;
+    case MotionControlType::angle:
+      // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
+      //                        the angles are large. This results in not being able to command small changes at high position values.
+      //                        to solve this, the delta-angle has to be calculated in a numerically precise way.
+      // angle set point
+      shaft_angle_sp = target;
+      // calculate velocity set point
+      shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
+      // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
+      current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
+      // if torque controlled through voltage
+      if(torque_controller == TorqueControlType::voltage){
+        // use voltage if phase-resistance not provided
+        if(!_isset(phase_resistance))  voltage.q = current_sp;
+        else  voltage.q = current_sp*phase_resistance;
+        voltage.d = 0;
+      }
+      break;
+    case MotionControlType::velocity:
+      // velocity set point - sensor precision: this calculation is numerically precise.
+      shaft_velocity_sp = target;
+      // calculate the torque command
+      current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
+      // if torque controlled through voltage control
+      if(torque_controller == TorqueControlType::voltage){
+        // use voltage if phase-resistance not provided
+        if(!_isset(phase_resistance))  voltage.q = current_sp;
+        else  voltage.q = current_sp*phase_resistance;
+        voltage.d = 0;
+      }
+      break;
+    case MotionControlType::velocity_openloop:
+      // velocity control in open loop - sensor precision: this calculation is numerically precise.
+      shaft_velocity_sp = target;
+      voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
+      voltage.d = 0;
+      break;
+    case MotionControlType::angle_openloop:
+      // angle control in open loop - 
+      // TODO sensor precision: this calculation NOT numerically precise, and subject
+      //                        to the same problems in small set-point changes at high angles 
+      //                        as the closed loop version.
+      shaft_angle_sp = target;
+      voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
+      voltage.d = 0;
+      break;
+  }
+}
+
+
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Space Vector PWM and Sine PWM algorithms
+//
+// Function using sine approximation
+// regular sin + cos ~300us    (no memory usaage)
+// approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
+void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
+
+  float center;
+  int sector;
+  float _ca,_sa;
+
+  switch (foc_modulation)
+  {
+    case FOCModulationType::Trapezoid_120 :
+      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
+      static int trap_120_map[6][3] = {
+        {_HIGH_IMPEDANCE,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,_HIGH_IMPEDANCE,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,_HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+      };
+      // static int trap_120_state = 0;
+      sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
+      // centering the voltages around either
+      // modulation_centered == true > driver.volage_limit/2
+      // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
+      center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+
+      if(trap_120_map[sector][0]  == _HIGH_IMPEDANCE){
+        Ua= center;
+        Ub = trap_120_map[sector][1] * Uq + center;
+        Uc = trap_120_map[sector][2] * Uq + center;
+        driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible
+      }else if(trap_120_map[sector][1]  == _HIGH_IMPEDANCE){
+        Ua = trap_120_map[sector][0] * Uq + center;
+        Ub = center;
+        Uc = trap_120_map[sector][2] * Uq + center;
+        driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible
+      }else{
+        Ua = trap_120_map[sector][0] * Uq + center;
+        Ub = trap_120_map[sector][1] * Uq + center;
+        Uc = center;
+        driver->setPhaseState(_ACTIVE,_ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible
+      }
+
+    break;
+
+    case FOCModulationType::Trapezoid_150 :
+      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
+      static int trap_150_map[12][3] = {
+        {_HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,1,1},{-1,_HIGH_IMPEDANCE,1},{-1,-1,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,-1,-1},{1,_HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+      };
+      // static int trap_150_state = 0;
+      sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
+      // centering the voltages around either
+      // modulation_centered == true > driver.volage_limit/2
+      // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
+      center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+
+      if(trap_150_map[sector][0]  == _HIGH_IMPEDANCE){
+        Ua= center;
+        Ub = trap_150_map[sector][1] * Uq + center;
+        Uc = trap_150_map[sector][2] * Uq + center;
+        driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible
+      }else if(trap_150_map[sector][1]  == _HIGH_IMPEDANCE){
+        Ua = trap_150_map[sector][0] * Uq + center;
+        Ub = center;
+        Uc = trap_150_map[sector][2] * Uq + center;
+        driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible
+      }else{
+        Ua = trap_150_map[sector][0] * Uq + center;
+        Ub = trap_150_map[sector][1] * Uq + center;
+        Uc = center;
+        driver->setPhaseState(_ACTIVE, _ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible
+      }
+
+    break;
+
+    case FOCModulationType::SinePWM :
+      // Sinusoidal PWM modulation
+      // Inverse Park + Clarke transformation
+
+      // angle normalization in between 0 and 2pi
+      // only necessary if using _sin and _cos - approximation functions
+      angle_el = _normalizeAngle(angle_el);
+      _ca = _cos(angle_el);
+      _sa = _sin(angle_el);
+      // Inverse park transform
+      Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
+      Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;
+
+      // center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+      center = driver->voltage_limit/2;
+      // Clarke transform
+      Ua = Ualpha + center;
+      Ub = -0.5f * Ualpha  + _SQRT3_2 * Ubeta + center;
+      Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center;
+
+      if (!modulation_centered) {
+        float Umin = min(Ua, min(Ub, Uc));
+        Ua -= Umin;
+        Ub -= Umin;
+        Uc -= Umin;
+      }
+
+      break;
+
+    case FOCModulationType::SpaceVectorPWM :
+      // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
+      // https://www.youtube.com/watch?v=QMSWUMEAejg
+
+      // the algorithm goes
+      // 1) Ualpha, Ubeta
+      // 2) Uout = sqrt(Ualpha^2 + Ubeta^2)
+      // 3) angle_el = atan2(Ubeta, Ualpha)
+      //
+      // equivalent to 2)  because the magnitude does not change is:
+      // Uout = sqrt(Ud^2 + Uq^2)
+      // equivalent to 3) is
+      // angle_el = angle_el + atan2(Uq,Ud)
+
+      float Uout;
+      // a bit of optitmisation
+      if(Ud){ // only if Ud and Uq set
+        // _sqrt is an approx of sqrt (3-4% error)
+        Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit;
+        // angle normalisation in between 0 and 2pi
+        // only necessary if using _sin and _cos - approximation functions
+        angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
+      }else{// only Uq available - no need for atan2 and sqrt
+        Uout = Uq / driver->voltage_limit;
+        // angle normalisation in between 0 and 2pi
+        // only necessary if using _sin and _cos - approximation functions
+        angle_el = _normalizeAngle(angle_el + _PI_2);
+      }
+      // find the sector we are in currently
+      sector = floor(angle_el / _PI_3) + 1;
+      // calculate the duty cycles
+      float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
+      float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout;
+      // two versions possible
+      float T0 = 0; // pulled to 0 - better for low power supply voltage
+      if (modulation_centered) {
+        T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
+      }
+
+      // calculate the duty cycles(times)
+      float Ta,Tb,Tc;
+      switch(sector){
+        case 1:
+          Ta = T1 + T2 + T0/2;
+          Tb = T2 + T0/2;
+          Tc = T0/2;
+          break;
+        case 2:
+          Ta = T1 +  T0/2;
+          Tb = T1 + T2 + T0/2;
+          Tc = T0/2;
+          break;
+        case 3:
+          Ta = T0/2;
+          Tb = T1 + T2 + T0/2;
+          Tc = T2 + T0/2;
+          break;
+        case 4:
+          Ta = T0/2;
+          Tb = T1+ T0/2;
+          Tc = T1 + T2 + T0/2;
+          break;
+        case 5:
+          Ta = T2 + T0/2;
+          Tb = T0/2;
+          Tc = T1 + T2 + T0/2;
+          break;
+        case 6:
+          Ta = T1 + T2 + T0/2;
+          Tb = T0/2;
+          Tc = T1 + T0/2;
+          break;
+        default:
+         // possible error state
+          Ta = 0;
+          Tb = 0;
+          Tc = 0;
+      }
+
+      // calculate the phase voltages and center
+      Ua = Ta*driver->voltage_limit;
+      Ub = Tb*driver->voltage_limit;
+      Uc = Tc*driver->voltage_limit;
+      break;
+
+  }
+
+  // set the voltages in driver
+  driver->setPwm(Ua, Ub, Uc);
+}
+
+
+
+// Function (iterative) generating open loop movement for target velocity
+// - target_velocity - rad/s
+// it uses voltage_limit variable
+float BLDCMotor::velocityOpenloop(float target_velocity){
+  // get current timestamp
+  unsigned long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // calculate the necessary angle to achieve target velocity
+  shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
+  // for display purposes
+  shaft_velocity = target_velocity;
+
+  // use voltage limit or current limit
+  float Uq = voltage_limit;
+  if(_isset(phase_resistance)) Uq =  current_limit*phase_resistance;
+
+  // set the maximal allowed voltage (voltage_limit) with the necessary angle
+  setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, pole_pairs));
+
+  // save timestamp for next call
+  open_loop_timestamp = now_us;
+
+  return Uq;
+}
+
+// Function (iterative) generating open loop movement towards the target angle
+// - target_angle - rad
+// it uses voltage_limit and velocity_limit variables
+float BLDCMotor::angleOpenloop(float target_angle){
+  // get current timestamp
+  unsigned long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // calculate the necessary angle to move from current position towards target angle
+  // with maximal velocity (velocity_limit)
+  // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point
+  //                        where small position changes are no longer captured by the precision of floats
+  //                        when the total position is large.
+  if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){
+    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
+    shaft_velocity = velocity_limit;
+  }else{
+    shaft_angle = target_angle;
+    shaft_velocity = 0;
+  }
+
+
+  // use voltage limit or current limit
+  float Uq = voltage_limit;
+  if(_isset(phase_resistance)) Uq =  current_limit*phase_resistance;
+  // set the maximal allowed voltage (voltage_limit) with the necessary angle
+  // sensor precision: this calculation is OK due to the normalisation
+  setPhaseVoltage(Uq,  0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs));
+
+  // save timestamp for next call
+  open_loop_timestamp = now_us;
+
+  return Uq;
+}

v1.3/lib/Simple_FOC/src/BLDCMotor.h

@@ -0,0 +1,112 @@
+#ifndef BLDCMotor_h
+#define BLDCMotor_h
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/Sensor.h"
+#include "common/base_classes/BLDCDriver.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "common/defaults.h"
+
+/**
+ BLDC motor class
+*/
+class BLDCMotor: public FOCMotor
+{
+  public:
+    /**
+     BLDCMotor class constructor
+     @param pp pole pairs number
+     @param R  motor phase resistance
+     */ 
+    BLDCMotor(int pp,  float R = NOT_SET);
+    
+    /**
+     * Function linking a motor and a foc driver 
+     * 
+     * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting
+     */
+    void linkDriver(BLDCDriver* driver);
+
+    /** 
+      * BLDCDriver link:
+      * - 3PWM 
+      * - 6PWM 
+    */
+    BLDCDriver* driver; 
+    
+    /**  Motor hardware init function */
+  	void init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    /**
+     * Function initializing FOC algorithm
+     * and aligning sensor's and motors' zero position 
+     */  
+    int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override;
+    /**
+     * Function running FOC algorithm in real-time
+     * it calculates the gets motor angle and sets the appropriate voltages 
+     * to the phase pwm signals
+     * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us
+     */ 
+    void loopFOC() override;
+
+    /**
+     * Function executing the control loops set by the controller parameter of the BLDCMotor.
+     * 
+     * @param target  Either voltage, angle or velocity based on the motor.controller
+     *                If it is not set the motor will use the target set in its variable motor.target
+     * 
+     * This function doesn't need to be run upon each loop execution - depends of the use case
+     */
+    void move(float target = NOT_SET) override;
+    
+    float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor
+    float	Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform
+
+
+  private:
+    // FOC methods 
+    /**
+    * Method using FOC to set Uq to the motor at the optimal angle
+    * Heart of the FOC algorithm
+    * 
+    * @param Uq Current voltage in q axis to set to the motor
+    * @param Ud Current voltage in d axis to set to the motor
+    * @param angle_el current electrical angle of the motor
+    */
+    void setPhaseVoltage(float Uq, float Ud, float angle_el);
+    /** Sensor alignment to electrical 0 angle of the motor */
+    int alignSensor();
+    /** Current sense and motor phase alignment */
+    int alignCurrentSense();
+    /** Motor and sensor alignment to the sensors absolute 0 angle  */
+    int absoluteZeroSearch();
+
+        
+    // Open loop motion control    
+    /**
+     * Function (iterative) generating open loop movement for target velocity
+     * it uses voltage_limit variable
+     * 
+     * @param target_velocity - rad/s
+     */
+    float velocityOpenloop(float target_velocity);
+    /**
+     * Function (iterative) generating open loop movement towards the target angle
+     * it uses voltage_limit and velocity_limit variables
+     * 
+     * @param target_angle - rad
+     */
+    float angleOpenloop(float target_angle);
+    // open loop variables
+    long open_loop_timestamp;
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/SimpleFOC.h

@@ -0,0 +1,118 @@
+/*!
+ * @file SimpleFOC.h
+ *
+ * @mainpage Simple Field Oriented Control BLDC motor control library
+ *
+ * @section intro_sec Introduction
+ *
+ * Proper low-cost and low-power FOC supporting boards are very hard to find these days and even may not exist.<br> Even harder to find is a stable and simple FOC algorithm code capable of running on Arduino devices. Therefore this is an attempt to:
+ * - Demystify FOC algorithm and make a robust but simple Arduino library: Arduino SimpleFOC library
+ * - Develop a modular BLDC driver board: Arduino SimpleFOC shield.
+ *
+ * @section features Features
+ *  - Arduino compatible: Arduino library code
+ *  - Easy to setup and configure:
+ *     - Easy hardware configuration
+ *     - Easy tuning the control loops
+ *  - Modular:
+ *     - Supports as many sensors , BLDC motors and driver boards as possible
+ *     - Supports as many application requirements as possible
+ *  - Plug & play: Arduino SimpleFOC shield
+ * 
+ * @section dependencies Supported Hardware
+ *  - Motors 
+ *    - BLDC motors
+ *    - Stepper motors
+ * - Drivers 
+ *    - BLDC drivers
+ *    - Gimbal drivers
+ *    - Stepper drivers
+ * - Position sensors 
+ *    - Encoders
+ *    - Magnetic sensors
+ *    - Hall sensors
+ *    - Open-loop control
+ * - Microcontrollers 
+ *    - Arduino
+ *    - STM32
+ *    - ESP32
+ *    - Teensy
+ * 
+ * @section example_code Example code
+ * @code
+#include <SimpleFOC.h>
+
+//  BLDCMotor( pole_pairs )
+BLDCMotor motor = BLDCMotor(11);
+//  BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) )
+BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8);
+//  Encoder(pin_A, pin_B, CPR)
+Encoder encoder = Encoder(2, 3, 2048);
+// channel A and B callbacks
+void doA(){encoder.handleA();}
+void doB(){encoder.handleB();}
+
+
+void setup() {  
+  // initialize encoder hardware
+  encoder.init();
+  // hardware interrupt enable
+  encoder.enableInterrupts(doA, doB);
+  // link the motor to the sensor
+  motor.linkSensor(&encoder);
+  
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  // initialise driver hardware
+  driver.init();
+  // link driver
+  motor.linkDriver(&driver);
+
+  // set control loop type to be used
+  motor.controller = MotionControlType::velocity;
+  // initialize motor
+  motor.init();
+  
+  // align encoder and start FOC
+  motor.initFOC();
+}
+
+void loop() {
+  // FOC algorithm function
+  motor.loopFOC();
+
+  // velocity control loop function
+  // setting the target velocity or 2rad/s
+  motor.move(2);
+}
+ * @endcode 
+ *
+ * @section license License
+ *
+ * MIT license, all text here must be included in any redistribution.
+ *
+ */
+
+#ifndef SIMPLEFOC_H
+#define SIMPLEFOC_H
+
+#include "BLDCMotor.h"
+#include "StepperMotor.h"
+#include "sensors/Encoder.h"
+#include "sensors/MagneticSensorSPI.h"
+#include "sensors/MagneticSensorI2C.h"
+#include "sensors/MagneticSensorAnalog.h"
+#include "sensors/MagneticSensorPWM.h"
+#include "sensors/HallSensor.h"
+#include "sensors/GenericSensor.h"
+#include "drivers/BLDCDriver3PWM.h"
+#include "drivers/BLDCDriver6PWM.h"
+#include "drivers/StepperDriver4PWM.h"
+#include "drivers/StepperDriver2PWM.h"
+#include "current_sense/InlineCurrentSense.h"
+#include "current_sense/LowsideCurrentSense.h"
+#include "current_sense/GenericCurrentSense.h"
+#include "communication/Commander.h"
+#include "communication/StepDirListener.h"
+
+#endif

v1.3/lib/Simple_FOC/src/StepperMotor.cpp

@@ -0,0 +1,417 @@
+#include "StepperMotor.h"
+
+// StepperMotor(int pp)
+// - pp            - pole pair number
+// - R             - motor phase resistance
+StepperMotor::StepperMotor(int pp, float _R)
+: FOCMotor()
+{
+  // number od pole pairs
+  pole_pairs = pp;
+  // save phase resistance number
+  phase_resistance = _R;
+
+  // torque control type is voltage by default
+  // current and foc_current not supported yet
+  torque_controller = TorqueControlType::voltage;
+}
+
+/**
+	Link the driver which controls the motor
+*/
+void StepperMotor::linkDriver(StepperDriver* _driver) {
+  driver = _driver;
+}
+
+// init hardware pins
+void StepperMotor::init() {
+  if (!driver || !driver->initialized) {
+    motor_status = FOCMotorStatus::motor_init_failed;
+    if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
+  }
+  motor_status = FOCMotorStatus::motor_initializing;
+  if(monitor_port) monitor_port->println(F("MOT: Init"));
+
+  // if set the phase resistance of the motor use current limit to calculate the voltage limit
+  if(_isset(phase_resistance)) {
+    float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case
+    // use it if it is less then voltage_limit set by the user
+    voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit;
+  }
+  // sanity check for the voltage limit configuration
+  if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
+  // constrain voltage for sensor alignment
+  if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
+
+  // update the controller limits
+  if(_isset(phase_resistance)){
+    // velocity control loop controls current
+    PID_velocity.limit = current_limit;
+  }else{
+    PID_velocity.limit = voltage_limit;
+  }
+  P_angle.limit = velocity_limit;
+
+  _delay(500);
+  // enable motor
+  if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
+  enable();
+  _delay(500);
+
+  motor_status = FOCMotorStatus::motor_uncalibrated;
+}
+
+
+// disable motor driver
+void StepperMotor::disable()
+{
+  // set zero to PWM
+  driver->setPwm(0, 0);
+  // disable driver
+  driver->disable();
+  // motor status update
+  enabled = 0;
+}
+// enable motor driver
+void StepperMotor::enable()
+{
+  // disable enable
+  driver->enable();
+  // set zero to PWM
+  driver->setPwm(0, 0);
+  // motor status update
+  enabled = 1;
+}
+
+
+/**
+  FOC functions
+*/
+// FOC initialization function
+int  StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) {
+  int exit_flag = 1;
+  
+  motor_status = FOCMotorStatus::motor_calibrating;
+
+  // align motor if necessary
+  // alignment necessary for encoders!
+  if(_isset(zero_electric_offset)){
+    // abosolute zero offset provided - no need to align
+    zero_electric_angle = zero_electric_offset;
+    // set the sensor direction - default CW
+    sensor_direction = _sensor_direction;
+  }
+
+  // sensor and motor alignment - can be skipped
+  // by setting motor.sensor_direction and motor.zero_electric_angle
+  _delay(500);
+  if(sensor){
+    exit_flag *= alignSensor();
+    // added the shaft_angle update
+    sensor->update();
+    shaft_angle = sensor->getAngle();
+  }else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
+
+  if(exit_flag){
+    if(monitor_port) monitor_port->println(F("MOT: Ready."));
+    motor_status = FOCMotorStatus::motor_ready;
+  }else{
+    if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
+    motor_status = FOCMotorStatus::motor_calib_failed;
+    disable();
+  }
+
+  return exit_flag;
+}
+
+// Encoder alignment to electrical 0 angle
+int StepperMotor::alignSensor() {
+  int exit_flag = 1; //success
+  if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
+
+  // if unknown natural direction
+  if(!_isset(sensor_direction)){
+    // check if sensor needs zero search
+    if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
+    // stop init if not found index
+    if(!exit_flag) return exit_flag;
+
+    // find natural direction
+    // move one electrical revolution forward
+    for (int i = 0; i <=500; i++ ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f;
+      setPhaseVoltage(voltage_sensor_align, 0,  angle);
+      _delay(2);
+    }
+    // take and angle in the middle
+    sensor->update();
+    float mid_angle = sensor->getAngle();
+    // move one electrical revolution backwards
+    for (int i = 500; i >=0; i-- ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f ;
+      setPhaseVoltage(voltage_sensor_align, 0,  angle);
+      _delay(2);
+    }
+    sensor->update();
+    float end_angle = sensor->getAngle();
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+    // determine the direction the sensor moved
+    if (mid_angle == end_angle) {
+      if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
+      return 0; // failed calibration
+    } else if (mid_angle < end_angle) {
+      if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
+      sensor_direction = Direction::CCW;
+    } else{
+      if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
+      sensor_direction = Direction::CW;
+    }
+    // check pole pair number
+    if(monitor_port) monitor_port->print(F("MOT: PP check: "));
+    float moved =  fabs(mid_angle - end_angle);
+    if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
+      if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
+      if(monitor_port) monitor_port->println(_2PI/moved,4);
+    }else if(monitor_port) monitor_port->println(F("OK!"));
+
+  }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib."));
+
+  // zero electric angle not known
+  if(!_isset(zero_electric_angle)){
+    // align the electrical phases of the motor and sensor
+    // set angle -90(270 = 3PI/2) degrees
+    setPhaseVoltage(voltage_sensor_align, 0,  _3PI_2);
+    _delay(700);
+    // read the sensor
+    sensor->update();
+    // get the current zero electric angle
+    zero_electric_angle = 0;
+    zero_electric_angle = electricalAngle();
+    _delay(20);
+    if(monitor_port){
+      monitor_port->print(F("MOT: Zero elec. angle: "));
+      monitor_port->println(zero_electric_angle);
+    }
+    // stop everything
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+  }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
+  return exit_flag;
+}
+
+// Encoder alignment the absolute zero angle
+// - to the index
+int StepperMotor::absoluteZeroSearch() {
+
+  if(monitor_port) monitor_port->println(F("MOT: Index search..."));
+  // search the absolute zero with small velocity
+  float limit_vel = velocity_limit;
+  float limit_volt = voltage_limit;
+  velocity_limit = velocity_index_search;
+  voltage_limit = voltage_sensor_align;
+  shaft_angle = 0;
+  while(sensor->needsSearch() && shaft_angle < _2PI){
+    angleOpenloop(1.5f*_2PI);
+    // call important for some sensors not to loose count
+    // not needed for the search
+    sensor->update();
+  }
+  // disable motor
+  setPhaseVoltage(0, 0, 0);
+  // reinit the limits
+  velocity_limit = limit_vel;
+  voltage_limit = limit_volt;
+  // check if the zero found
+  if(monitor_port){
+    if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
+    else monitor_port->println(F("MOT: Success!"));
+  }
+  return !sensor->needsSearch();
+}
+
+
+// Iterative function looping FOC algorithm, setting Uq on the Motor
+// The faster it can be run the better
+void StepperMotor::loopFOC() {
+  
+  // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
+  //                 of full rotations otherwise.
+  if (sensor) sensor->update();
+
+  // if open-loop do nothing
+  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
+  // shaft angle
+  shaft_angle = shaftAngle();
+
+  // if disabled do nothing
+  if(!enabled) return;
+
+  // Needs the update() to be called first
+  // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() 
+  // which is in range 0-2PI
+  electrical_angle = electricalAngle();
+
+  // set the phase voltage - FOC heart function :)
+  setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
+}
+
+// Iterative function running outer loop of the FOC algorithm
+// Behavior of this function is determined by the motor.controller variable
+// It runs either angle, velocity or voltage loop
+// - needs to be called iteratively it is asynchronous function
+// - if target is not set it uses motor.target value
+void StepperMotor::move(float new_target) {
+
+  // downsampling (optional)
+  if(motion_cnt++ < motion_downsample) return;
+  motion_cnt = 0;
+
+  // shaft angle/velocity need the update() to be called first
+  // get shaft angle
+  // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
+  //                        For this reason it is NOT precise when the angles become large.
+  //                        Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
+  //                        when switching to a 2-component representation.
+  if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) 
+    shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
+  // get angular velocity 
+  shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
+
+  // if disabled do nothing
+  if(!enabled) return;
+
+  // set internal target variable
+  if(_isset(new_target) ) target = new_target;
+  // choose control loop
+  switch (controller) {
+    case MotionControlType::torque:
+      if(!_isset(phase_resistance))  voltage.q = target; // if voltage torque control
+      else voltage.q =  target*phase_resistance;
+      voltage.d = 0;
+      break;
+    case MotionControlType::angle:
+      // angle set point
+      shaft_angle_sp = target;
+      // calculate velocity set point
+      shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
+      // calculate the torque command
+      current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
+      // if torque controlled through voltage
+      // use voltage if phase-resistance not provided
+      if(!_isset(phase_resistance))  voltage.q = current_sp;
+      else  voltage.q = current_sp*phase_resistance;
+      voltage.d = 0;
+      break;
+    case MotionControlType::velocity:
+      // velocity set point
+      shaft_velocity_sp = target;
+      // calculate the torque command
+      current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
+      // if torque controlled through voltage control
+      // use voltage if phase-resistance not provided
+      if(!_isset(phase_resistance))  voltage.q = current_sp;
+      else  voltage.q = current_sp*phase_resistance;
+      voltage.d = 0;
+      break;
+    case MotionControlType::velocity_openloop:
+      // velocity control in open loop
+      shaft_velocity_sp = target;
+      voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
+      voltage.d = 0;
+      break;
+    case MotionControlType::angle_openloop:
+      // angle control in open loop
+      shaft_angle_sp = target;
+      voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
+      voltage.d = 0;
+      break;
+  }
+}
+
+
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Sine PWM algorithms
+// - space vector not implemented yet
+//
+// Function using sine approximation
+// regular sin + cos ~300us    (no memory usaage)
+// approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
+void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
+  // Sinusoidal PWM modulation
+  // Inverse Park transformation
+
+  // angle normalization in between 0 and 2pi
+  // only necessary if using _sin and _cos - approximation functions
+  angle_el = _normalizeAngle(angle_el);
+  float _ca = _cos(angle_el);
+  float _sa = _sin(angle_el);
+  // Inverse park transform
+  Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
+  Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;
+
+  // set the voltages in hardware
+  driver->setPwm(Ualpha, Ubeta);
+}
+
+// Function (iterative) generating open loop movement for target velocity
+// - target_velocity - rad/s
+// it uses voltage_limit variable
+float StepperMotor::velocityOpenloop(float target_velocity){
+  // get current timestamp
+  unsigned long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // calculate the necessary angle to achieve target velocity
+  shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
+  // for display purposes
+  shaft_velocity = target_velocity;
+
+  // use voltage limit or current limit
+  float Uq = voltage_limit;
+  if(_isset(phase_resistance)) Uq =  current_limit*phase_resistance;
+
+  // set the maximal allowed voltage (voltage_limit) with the necessary angle
+  setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, pole_pairs));
+
+  // save timestamp for next call
+  open_loop_timestamp = now_us;
+
+  return Uq;
+}
+
+// Function (iterative) generating open loop movement towards the target angle
+// - target_angle - rad
+// it uses voltage_limit and velocity_limit variables
+float StepperMotor::angleOpenloop(float target_angle){
+  // get current timestamp
+  unsigned long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // calculate the necessary angle to move from current position towards target angle
+  // with maximal velocity (velocity_limit)
+  if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){
+    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
+    shaft_velocity = velocity_limit;
+  }else{
+    shaft_angle = target_angle;
+    shaft_velocity = 0;
+  }
+
+  // use voltage limit or current limit
+  float Uq = voltage_limit;
+  if(_isset(phase_resistance)) Uq =  current_limit*phase_resistance;
+  // set the maximal allowed voltage (voltage_limit) with the necessary angle
+  setPhaseVoltage(Uq,  0, _electricalAngle((shaft_angle), pole_pairs));
+
+  // save timestamp for next call
+  open_loop_timestamp = now_us;
+
+  return Uq;
+}

v1.3/lib/Simple_FOC/src/StepperMotor.h

@@ -0,0 +1,118 @@
+/**
+ *  @file StepperMotor.h
+ * 
+ */
+
+#ifndef StepperMotor_h
+#define StepperMotor_h
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/StepperDriver.h"
+#include "common/base_classes/Sensor.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "common/defaults.h"
+
+/**
+ Stepper Motor class
+*/
+class StepperMotor: public FOCMotor
+{
+  public:
+    /**
+      StepperMotor class constructor
+      @param pp  pole pair number 
+      @param R  motor phase resistance
+    */
+    StepperMotor(int pp,  float R = NOT_SET);
+
+    /**
+     * Function linking a motor and a foc driver 
+     * 
+     * @param driver StepperDriver class implementing all the hardware specific functions necessary PWM setting
+     */
+    void linkDriver(StepperDriver* driver);
+
+    /** 
+      * StepperDriver link:
+      * - 4PWM  - L298N for example
+    */
+    StepperDriver* driver; 
+
+    /**  Motor hardware init function */
+  	void init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    /**
+     * Function initializing FOC algorithm
+     * and aligning sensor's and motors' zero position 
+     * 
+     * - If zero_electric_offset parameter is set the alignment procedure is skipped
+     * 
+     * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position.
+     * @param sensor_direction  sensor natural direction - default is CW
+     *
+     */  
+    int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override;
+    /**
+     * Function running FOC algorithm in real-time
+     * it calculates the gets motor angle and sets the appropriate voltages 
+     * to the phase pwm signals
+     * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us
+     */ 
+    void loopFOC() override;
+    /**
+     * Function executing the control loops set by the controller parameter of the StepperMotor.
+     * 
+     * @param target  Either voltage, angle or velocity based on the motor.controller
+     *                If it is not set the motor will use the target set in its variable motor.target
+     * 
+     * This function doesn't need to be run upon each loop execution - depends of the use case
+     */
+    void move(float target = NOT_SET) override;
+    
+    float	Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform
+
+  private:
+  
+    // FOC methods 
+    /**
+    * Method using FOC to set Uq to the motor at the optimal angle
+    * Heart of the FOC algorithm
+    * 
+    * @param Uq Current voltage in q axis to set to the motor
+    * @param Ud Current voltage in d axis to set to the motor
+    * @param angle_el current electrical angle of the motor
+    */
+    void setPhaseVoltage(float Uq, float Ud , float angle_el);
+
+    /** Sensor alignment to electrical 0 angle of the motor */
+    int alignSensor();
+    /** Motor and sensor alignment to the sensors absolute 0 angle  */
+    int absoluteZeroSearch();
+        
+    // Open loop motion control    
+    /**
+     * Function (iterative) generating open loop movement for target velocity
+     * it uses voltage_limit variable
+     * 
+     * @param target_velocity - rad/s
+     */
+    float velocityOpenloop(float target_velocity);
+    /**
+     * Function (iterative) generating open loop movement towards the target angle
+     * it uses voltage_limit and velocity_limit variables
+     * 
+     * @param target_angle - rad
+     */
+    float angleOpenloop(float target_angle);
+    // open loop variables
+    long open_loop_timestamp;
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/common/base_classes/BLDCDriver.h

@@ -0,0 +1,46 @@
+#ifndef BLDCDRIVER_H
+#define BLDCDRIVER_H
+
+#include "Arduino.h"
+
+class BLDCDriver{
+    public:
+
+        /** Initialise hardware */
+        virtual int init() = 0;
+        /** Enable hardware */
+        virtual void enable() = 0;
+        /** Disable hardware */
+        virtual void disable() = 0;
+
+        long pwm_frequency; //!< pwm frequency value in hertz
+        float voltage_power_supply; //!< power supply voltage
+        float voltage_limit; //!< limiting voltage set to the motor
+
+
+        float dc_a; //!< currently set duty cycle on phaseA
+        float dc_b; //!< currently set duty cycle on phaseB
+        float dc_c; //!< currently set duty cycle on phaseC
+
+        bool initialized = false; // true if driver was successfully initialized
+
+        /**
+         * Set phase voltages to the harware
+         *
+         * @param Ua - phase A voltage
+         * @param Ub - phase B voltage
+         * @param Uc - phase C voltage
+        */
+        virtual void setPwm(float Ua, float Ub, float Uc) = 0;
+
+        /**
+         * Set phase state, enable/disable
+         *
+         * @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) = 0;
+};
+
+#endif

v1.3/lib/Simple_FOC/src/common/base_classes/CurrentSense.cpp

@@ -0,0 +1,67 @@
+#include "CurrentSense.h"
+
+
+// get current magnitude 
+//   - absolute  - if no electrical_angle provided 
+//   - signed    - if angle provided
+float CurrentSense::getDCCurrent(float motor_electrical_angle){
+    // read current phase currents
+    PhaseCurrent_s current = getPhaseCurrents();
+    // currnet sign - if motor angle not provided the magnitude is always positive
+    float sign = 1;
+
+    // calculate clarke transform
+    float i_alpha, i_beta;
+    if(!current.c){
+        // if only two measured currents
+        i_alpha = current.a;  
+        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
+    }else{
+        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
+        float mid = (1.f/3) * (current.a + current.b + current.c);
+        float a = current.a - mid;
+        float b = current.b - mid;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
+    }
+
+    // if motor angle provided function returns signed value of the current
+    // determine the sign of the current
+    // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1  
+    if(motor_electrical_angle) 
+        sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1;  
+    // return current magnitude
+    return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta);
+}
+
+// function used with the foc algorihtm
+//   calculating DQ currents from phase currents
+//   - function calculating park and clarke transform of the phase currents 
+//   - using getPhaseCurrents internally
+DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
+    // read current phase currents
+    PhaseCurrent_s current = getPhaseCurrents();
+
+    // calculate clarke transform
+    float i_alpha, i_beta;
+    if(!current.c){
+        // if only two measured currents
+        i_alpha = current.a;  
+        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
+    } else {
+        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
+        float mid = (1.f/3) * (current.a + current.b + current.c);
+        float a = current.a - mid;
+        float b = current.b - mid;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
+    }
+
+    // calculate park transform
+    float ct = _cos(angle_el);
+    float st = _sin(angle_el);
+    DQCurrent_s return_current;
+    return_current.d = i_alpha * ct + i_beta * st;
+    return_current.q = i_beta * ct - i_alpha * st;
+    return return_current;
+}

v1.3/lib/Simple_FOC/src/common/base_classes/CurrentSense.h

@@ -0,0 +1,67 @@
+#ifndef CURRENTSENSE_H
+#define CURRENTSENSE_H
+
+#include "BLDCDriver.h"
+#include "../foc_utils.h"
+
+/**
+ *  Current sensing abstract class defintion
+ * Each current sensoring implementation needs to extend this interface
+ */
+class CurrentSense{
+    public:
+
+    /**
+     *  Function intialising the CurrentSense class
+     *   All the necessary intialisations of adc and sync should be implemented here
+     */
+    virtual void init() = 0;
+    
+    /**
+     * Function intended to implement all that is needed to sync and current sensing with the driver.
+     * If no such thing is needed it can be left empty (return 1)
+     * @returns -  0 - for failure &  1 - for success 
+     */
+    virtual int driverSync(BLDCDriver *driver) = 0;
+
+    // calibration variables
+    bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC()
+    /**
+     * Function intended to verify if:
+     *   - phase current are oriented properly 
+     *   - if their order is the same as driver phases
+     * 
+     * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1)
+     * @returns -  0 - for failure &  positive number (with status) - for success 
+     */
+    virtual int driverAlign(BLDCDriver *driver, float voltage) = 0;
+
+    /**
+     *  Function rading the phase currents a, b and c
+     *   This function will be used with the foc control throught the function 
+     *   CurrentSense::getFOCCurrents(electrical_angle)
+     *   - it returns current c equal to 0 if only two phase measurements available
+     * 
+     *  @return PhaseCurrent_s current values
+     */
+    virtual PhaseCurrent_s getPhaseCurrents() = 0;
+    /**
+     * Function reading the magnitude of the current set to the motor
+     *  It returns the abosolute or signed magnitude if possible
+     *  It can receive the motor electrical angle to help with calculation
+     *  This function is used with the current control  (not foc)
+     *  
+     * @param angle_el - electrical angle of the motor (optional) 
+     */
+    virtual float getDCCurrent(float angle_el = 0);
+
+    /**
+     * Function used for FOC contorl, it reads the DQ currents of the motor 
+     *   It uses the function getPhaseCurrents internally
+     * 
+     * @param angle_el - motor electrical angle
+     */
+    DQCurrent_s getFOCCurrents(float angle_el);
+};
+
+#endif

v1.3/lib/Simple_FOC/src/common/base_classes/FOCMotor.cpp

@@ -0,0 +1,141 @@
+#include "FOCMotor.h"
+
+/**
+ * Default constructor - setting all variabels to default values
+ */
+FOCMotor::FOCMotor()
+{
+  // maximum angular velocity to be used for positioning 
+  velocity_limit = DEF_VEL_LIM;
+  // maximum voltage to be set to the motor
+  voltage_limit = DEF_POWER_SUPPLY;
+  // not set on the begining
+  current_limit = DEF_CURRENT_LIM;
+
+  // index search velocity
+  velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY;
+  // sensor and motor align voltage
+  voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN;
+
+  // default modulation is SinePWM
+  foc_modulation = FOCModulationType::SinePWM;
+
+  // default target value
+  target = 0;
+  voltage.d = 0;
+  voltage.q = 0;
+  // current target values
+  current_sp = 0;
+  current.q = 0;
+  current.d = 0;
+  
+  //monitor_port 
+  monitor_port = nullptr;
+  //sensor 
+  sensor = nullptr;
+  //current sensor 
+  current_sense = nullptr;
+}
+
+
+/**
+	Sensor linking method
+*/
+void FOCMotor::linkSensor(Sensor* _sensor) {
+  sensor = _sensor;
+}
+
+/**
+	CurrentSense linking method
+*/
+void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) {
+  current_sense = _current_sense;
+}
+
+// shaft angle calculation
+float FOCMotor::shaftAngle() {
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return shaft_angle;
+  return sensor_direction*LPF_angle(sensor->getAngle()) - sensor_offset;
+}
+// shaft velocity calculation
+float FOCMotor::shaftVelocity() {
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return shaft_velocity;
+  return sensor_direction*LPF_velocity(sensor->getVelocity());
+}
+
+float FOCMotor::electricalAngle(){
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return electrical_angle;
+  return  _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle()  - zero_electric_angle );
+}
+
+/**
+ *  Monitoring functions
+ */
+// function implementing the monitor_port setter
+void FOCMotor::useMonitoring(Print &print){
+  monitor_port = &print; //operate on the address of print
+  if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!"));
+}
+
+// utility function intended to be used with serial plotter to monitor motor variables
+// significantly slowing the execution down!!!!
+void FOCMotor::monitor() {
+  if( !monitor_downsample || monitor_cnt++ < monitor_downsample ) return;
+  monitor_cnt = 0;
+  if(!monitor_port) return;
+  bool printed = 0;
+
+  if(monitor_variables & _MON_TARGET){
+    monitor_port->print(target,4);
+    monitor_port->print("\t");
+    printed= true;
+  }
+  if(monitor_variables & _MON_VOLT_Q) {
+    monitor_port->print(voltage.q,4);
+    monitor_port->print("\t");
+    printed= true;
+  }
+  if(monitor_variables & _MON_VOLT_D) {
+    monitor_port->print(voltage.d,4);
+    monitor_port->print("\t");
+    printed= true;
+  }
+  // read currents if possible - even in voltage mode (if current_sense available)
+  if(monitor_variables & _MON_CURR_Q || monitor_variables & _MON_CURR_D) {
+    DQCurrent_s c{0,0};
+    if(current_sense){
+      if(torque_controller == TorqueControlType::foc_current) c = current;
+      else{
+    	  c = current_sense->getFOCCurrents(electrical_angle);
+        c.q = LPF_current_q(c.q);
+        c.d = LPF_current_d(c.d);
+      }
+    }
+    if(monitor_variables & _MON_CURR_Q) {
+      monitor_port->print(c.q*1000, 2); // mAmps
+      monitor_port->print("\t");
+      printed= true;
+    }
+    if(monitor_variables & _MON_CURR_D) {
+      monitor_port->print(c.d*1000, 2); // mAmps
+      monitor_port->print("\t");
+      printed= true;
+    }
+  }
+ 
+  if(monitor_variables & _MON_VEL) {
+    monitor_port->print(shaft_velocity,4);
+    monitor_port->print("\t");
+    printed= true;
+  }
+  if(monitor_variables & _MON_ANGLE) {
+    monitor_port->print(shaft_angle,4);
+    printed= true;
+  }
+  if(printed) monitor_port->println();
+  
+}
+

v1.3/lib/Simple_FOC/src/common/base_classes/FOCMotor.h

@@ -0,0 +1,236 @@
+#ifndef FOCMOTOR_H
+#define FOCMOTOR_H
+
+#include "Arduino.h"
+#include "Sensor.h"
+#include "CurrentSense.h"
+
+#include "../time_utils.h"
+#include "../foc_utils.h"
+#include "../defaults.h"
+#include "../pid.h"
+#include "../lowpass_filter.h"
+
+
+// monitoring bitmap
+#define _MON_TARGET 0b1000000 // monitor target value
+#define _MON_VOLT_Q 0b0100000 // monitor voltage q value
+#define _MON_VOLT_D 0b0010000 // monitor voltage d value
+#define _MON_CURR_Q 0b0001000 // monitor current q value - if measured
+#define _MON_CURR_D 0b0000100 // monitor current d value - if measured
+#define _MON_VEL    0b0000010 // monitor velocity value
+#define _MON_ANGLE  0b0000001 // monitor angle value
+
+/**
+ *  Motiron control type
+ */
+enum MotionControlType : uint8_t {
+  torque            = 0x00,     //!< Torque control
+  velocity          = 0x01,     //!< Velocity motion control
+  angle             = 0x02,     //!< Position/angle motion control
+  velocity_openloop = 0x03,
+  angle_openloop    = 0x04
+};
+
+/**
+ *  Motiron control type
+ */
+enum TorqueControlType : uint8_t { 
+  voltage            = 0x00,     //!< Torque control using voltage
+  dc_current         = 0x01,     //!< Torque control using DC current (one current magnitude)
+  foc_current        = 0x02,     //!< torque control using dq currents
+};
+
+/**
+ *  FOC modulation type
+ */
+enum FOCModulationType : uint8_t {
+  SinePWM            = 0x00,     //!< Sinusoidal PWM modulation
+  SpaceVectorPWM     = 0x01,     //!< Space vector modulation method
+  Trapezoid_120      = 0x02,     
+  Trapezoid_150      = 0x03,     
+};
+
+
+
+enum FOCMotorStatus : uint8_t {
+  motor_uninitialized = 0x00,     //!< Motor is not yet initialized
+  motor_initializing  = 0x01,     //!< Motor intiialization is in progress
+  motor_uncalibrated  = 0x02,     //!< Motor is initialized, but not calibrated (open loop possible)
+  motor_calibrating   = 0x03,     //!< Motor calibration in progress
+  motor_ready         = 0x04,     //!< Motor is initialized and calibrated (closed loop possible)
+  motor_error         = 0x08,     //!< Motor is in error state (recoverable, e.g. overcurrent protection active)
+  motor_calib_failed  = 0x0E,     //!< Motor calibration failed (possibly recoverable)
+  motor_init_failed   = 0x0F,     //!< Motor initialization failed (not recoverable)
+};
+
+
+
+/**
+ Generic motor class
+*/
+class FOCMotor
+{
+  public:
+    /**
+     * Default constructor - setting all variabels to default values
+     */
+    FOCMotor();
+
+    /**  Motor hardware init function */
+  	virtual void init()=0;
+    /** Motor disable function */
+  	virtual void disable()=0;
+    /** Motor enable function */
+    virtual void enable()=0;
+
+    /**
+     * Function linking a motor and a sensor 
+     * 
+     * @param sensor Sensor class  wrapper for the FOC algorihtm to read the motor angle and velocity
+     */
+    void linkSensor(Sensor* sensor);
+
+    /**
+     * Function linking a motor and current sensing 
+     * 
+     * @param current_sense CurrentSense class wrapper for the FOC algorihtm to read the motor current measurements
+     */
+    void linkCurrentSense(CurrentSense* current_sense);
+
+
+    /**
+     * Function initializing FOC algorithm
+     * and aligning sensor's and motors' zero position 
+     * 
+     * - If zero_electric_offset parameter is set the alignment procedure is skipped
+     * 
+     * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position.
+     * @param sensor_direction  sensor natural direction - default is CW
+     *
+     */  
+    virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; 
+    /**
+     * Function running FOC algorithm in real-time
+     * it calculates the gets motor angle and sets the appropriate voltages 
+     * to the phase pwm signals
+     * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us
+     */ 
+    virtual void loopFOC()=0;
+    /**
+     * Function executing the control loops set by the controller parameter of the BLDCMotor.
+     * 
+     * @param target  Either voltage, angle or velocity based on the motor.controller
+     *                If it is not set the motor will use the target set in its variable motor.target
+     * 
+     * This function doesn't need to be run upon each loop execution - depends of the use case
+     */
+    virtual void move(float target = NOT_SET)=0;
+
+    // State calculation methods 
+    /** Shaft angle calculation in radians [rad] */
+    float shaftAngle();
+    /** 
+     * Shaft angle calculation function in radian per second [rad/s]
+     * It implements low pass filtering
+     */
+    float shaftVelocity();
+
+
+    /** 
+     * Electrical angle calculation  
+     */
+    float electricalAngle();
+
+    // state variables
+    float target; //!< current target value - depends of the controller
+  	float shaft_angle;//!< current motor angle
+  	float electrical_angle;//!< current electrical angle
+  	float shaft_velocity;//!< current motor velocity 
+    float current_sp;//!< target current ( q current )
+    float shaft_velocity_sp;//!< current target velocity
+    float shaft_angle_sp;//!< current target angle
+    DQVoltage_s voltage;//!< current d and q voltage set to the motor
+    DQCurrent_s current;//!< current d and q current measured
+
+    // motor configuration parameters
+    float voltage_sensor_align;//!< sensor and motor align voltage parameter
+    float velocity_index_search;//!< target velocity for index search 
+    
+    // motor physical parameters
+    float	phase_resistance; //!< motor phase resistance
+    int pole_pairs;//!< motor pole pairs number
+
+    // limiting variables
+    float voltage_limit; //!< Voltage limitting variable - global limit
+    float current_limit; //!< Current limitting variable - global limit
+    float velocity_limit; //!< Velocity limitting variable - global limit
+
+    // motor status vairables
+    int8_t enabled = 0;//!< enabled or disabled motor flag
+    FOCMotorStatus motor_status = FOCMotorStatus::motor_uninitialized; //!< motor status
+    
+    // pwm modulation related variables
+    FOCModulationType foc_modulation;//!<  parameter derterniming modulation algorithm
+    int8_t modulation_centered = 1;//!< flag (1) centered modulation around driver limit /2  or  (0) pulled to 0
+
+
+    // configuration structures
+    TorqueControlType torque_controller; //!< parameter determining the torque control type
+    MotionControlType controller; //!< parameter determining the control loop to be used
+
+    // controllers and low pass filters
+    PIDController PID_current_q{DEF_PID_CURR_P,DEF_PID_CURR_I,DEF_PID_CURR_D,DEF_PID_CURR_RAMP, DEF_POWER_SUPPLY};//!< parameter determining the q current PID config
+    PIDController PID_current_d{DEF_PID_CURR_P,DEF_PID_CURR_I,DEF_PID_CURR_D,DEF_PID_CURR_RAMP, DEF_POWER_SUPPLY};//!< parameter determining the d current PID config
+    LowPassFilter LPF_current_q{DEF_CURR_FILTER_Tf};//!<  parameter determining the current Low pass filter configuration 
+    LowPassFilter LPF_current_d{DEF_CURR_FILTER_Tf};//!<  parameter determining the current Low pass filter configuration 
+    PIDController PID_velocity{DEF_PID_VEL_P,DEF_PID_VEL_I,DEF_PID_VEL_D,DEF_PID_VEL_RAMP,DEF_PID_VEL_LIMIT};//!< parameter determining the velocity PID configuration
+    PIDController P_angle{DEF_P_ANGLE_P,0,0,0,DEF_VEL_LIM};	//!< parameter determining the position PID configuration 
+    LowPassFilter LPF_velocity{DEF_VEL_FILTER_Tf};//!<  parameter determining the velocity Low pass filter configuration 
+    LowPassFilter LPF_angle{0.0};//!<  parameter determining the angle low pass filter configuration 
+    unsigned int motion_downsample = DEF_MOTION_DOWNSMAPLE; //!< parameter defining the ratio of downsampling for move commad
+    unsigned int motion_cnt = 0; //!< counting variable for downsampling for move commad
+
+    // sensor related variabels
+    float sensor_offset; //!< user defined sensor zero offset
+    float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available
+    int sensor_direction = NOT_SET; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW
+
+    /**
+     * Function providing BLDCMotor class with the 
+     * Serial interface and enabling monitoring mode
+     * 
+     * @param serial Monitoring Serial class reference
+     */
+    void useMonitoring(Print &serial);
+
+    /**
+     * Utility function intended to be used with serial plotter to monitor motor variables
+     * significantly slowing the execution down!!!!
+     */
+    void monitor();
+    unsigned int monitor_downsample = DEF_MON_DOWNSMAPLE; //!< show monitor outputs each monitor_downsample calls 
+    // initial monitoring will display target, voltage, velocity and angle
+    uint8_t monitor_variables = _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE; //!< Bit array holding the map of variables the user wants to monitor
+   
+    /** 
+      * Sensor link:
+      * - Encoder 
+      * - MagneticSensor*
+      * - HallSensor
+    */
+    Sensor* sensor; 
+    /** 
+      * CurrentSense link
+    */
+    CurrentSense* current_sense; 
+
+    // monitoring functions
+    Print* monitor_port; //!< Serial terminal variable if provided
+  private:
+    // monitor counting variable
+    unsigned int monitor_cnt = 0 ; //!< counting variable
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/common/base_classes/Sensor.cpp

@@ -0,0 +1,72 @@
+#include "Sensor.h"
+#include "../foc_utils.h"
+#include "../time_utils.h"
+
+// TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0
+
+void Sensor::update() {
+    float val = getSensorAngle();
+    angle_prev_ts = _micros();
+    float d_angle = val - angle_prev;
+    // if overflow happened track it as full rotation
+    if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 
+    angle_prev = val;
+}
+
+
+ /** get current angular velocity (rad/s) */
+float Sensor::getVelocity() {
+    // calculate sample time
+    float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
+    // quick fix for strange cases (micros overflow)
+    if(Ts <= 0) Ts = 1e-3f;
+    // velocity calculation
+    float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;    
+    // save variables for future pass
+    vel_angle_prev = angle_prev;
+    vel_full_rotations = full_rotations;
+    vel_angle_prev_ts = angle_prev_ts;
+    return vel;
+}
+
+void Sensor::init() {
+    // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero)
+    getSensorAngle(); // call once
+    delayMicroseconds(1);
+    vel_angle_prev = getSensorAngle(); // call again
+    vel_angle_prev_ts = _micros();
+    delay(1);
+    getSensorAngle(); // call once
+    delayMicroseconds(1);
+    angle_prev = getSensorAngle(); // call again
+    angle_prev_ts = _micros();
+}
+
+
+float Sensor::getMechanicalAngle() {
+    return angle_prev;
+}
+
+
+
+float Sensor::getAngle(){
+    return (float)full_rotations * _2PI + angle_prev;
+}
+
+
+
+double Sensor::getPreciseAngle() {
+    return (double)full_rotations * (double)_2PI + (double)angle_prev;
+}
+
+
+
+int32_t Sensor::getFullRotations() {
+    return full_rotations;
+}
+
+
+
+int Sensor::needsSearch() {
+    return 0; // default false
+}

v1.3/lib/Simple_FOC/src/common/base_classes/Sensor.h

@@ -0,0 +1,131 @@
+#ifndef SENSOR_H
+#define SENSOR_H
+
+#include <inttypes.h>
+
+/**
+ *  Direction structure
+ */
+enum Direction : int8_t {
+    CW      = 1,  // clockwise
+    CCW     = -1, // counter clockwise
+    UNKNOWN = 0   // not yet known or invalid state
+};
+
+
+/**
+ *  Pullup configuration structure
+ */
+enum Pullup : uint8_t {
+    USE_INTERN = 0x00, //!< Use internal pullups
+    USE_EXTERN = 0x01  //!< Use external pullups
+};
+
+/**
+ * Sensor abstract class defintion
+ * 
+ * This class is purposefully kept simple, as a base for all kinds of sensors. Currently we have
+ * Encoders, Magnetic Encoders and Hall Sensor implementations. This base class extracts the
+ * most basic common features so that a FOC driver can obtain the data it needs for operation.
+ * 
+ * To implement your own sensors, create a sub-class of this class, and implement the getSensorAngle()
+ * method. getSensorAngle() returns a float value, in radians, representing the current shaft angle in the
+ * range 0 to 2*PI (one full turn). 
+ * 
+ * To function correctly, the sensor class update() method has to be called sufficiently quickly. Normally,
+ * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly
+ * enough, both for correct motor and sensor operation.
+ * 
+ * The Sensor base class provides an implementation of getVelocity(), and takes care of counting full
+ * revolutions in a precise way, but if you wish you can additionally override these methods to provide more
+ * optimal implementations for your hardware.
+ * 
+ */
+class Sensor{
+    public:
+        /**
+         * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with
+         * the hardware. Base implementation uses the values returned by update() so that 
+         * the same values are returned until update() is called again.
+         */
+        virtual float getMechanicalAngle();
+
+        /**
+         * Get current position (in rad) including full rotations and shaft angle.
+         * Base implementation uses the values returned by update() so that the same
+         * values are returned until update() is called again.
+         * Note that this value has limited precision as the number of rotations increases,
+         * because the limited precision of float can't capture the large angle of the full 
+         * rotations and the small angle of the shaft angle at the same time.
+         */
+        virtual float getAngle();
+        
+        /** 
+         * On architectures supporting it, this will return a double precision position value,
+         * which should have improved precision for large position values.
+         * Base implementation uses the values returned by update() so that the same
+         * values are returned until update() is called again.
+         */
+        virtual double getPreciseAngle();
+
+        /** 
+         * Get current angular velocity (rad/s)
+         * Can be overridden in subclasses. Base implementation uses the values 
+         * returned by update() so that it only makes sense to call this if update()
+         * has been called in the meantime.
+         */
+        virtual float getVelocity();
+
+        /**
+         * Get the number of full rotations
+         * Base implementation uses the values returned by update() so that the same
+         * values are returned until update() is called again. 
+         */
+        virtual int32_t getFullRotations();
+
+        /**
+         * Updates the sensor values by reading the hardware sensor.
+         * Some implementations may work with interrupts, and not need this.
+         * The base implementation calls getSensorAngle(), and updates internal
+         * fields for angle, timestamp and full rotations.
+         * This method must be called frequently enough to guarantee that full
+         * rotations are not "missed" due to infrequent polling.
+         * Override in subclasses if alternative behaviours are required for your
+         * sensor hardware.
+         */
+        virtual void update();
+
+        /** 
+         * returns 0 if it does need search for absolute zero
+         * 0 - magnetic sensor (& encoder with index which is found)
+         * 1 - ecoder with index (with index not found yet)
+         */
+        virtual int needsSearch();
+    protected:
+        /** 
+         * Get current shaft angle from the sensor hardware, and 
+         * return it as a float in radians, in the range 0 to 2PI.
+         * 
+         * This method is pure virtual and must be implemented in subclasses.
+         * Calling this method directly does not update the base-class internal fields.
+         * Use update() when calling from outside code.
+         */
+        virtual float getSensorAngle()=0;
+        /**
+         * Call Sensor::init() from your sensor subclass's init method if you want smoother startup
+         * The base class init() method calls getSensorAngle() several times to initialize the internal fields
+         * to current values, ensuring there is no discontinuity ("jump from zero") during the first calls
+         * to sensor.getAngle() and sensor.getVelocity()
+         */
+        virtual void init();
+
+        // velocity calculation variables
+        float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity
+        long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity
+        float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity
+        long vel_angle_prev_ts=0; // last velocity calculation timestamp
+        int32_t full_rotations=0; // full rotation tracking
+        int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation
+};
+
+#endif

v1.3/lib/Simple_FOC/src/common/base_classes/StepperDriver.h

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

v1.3/lib/Simple_FOC/src/common/defaults.h

@@ -0,0 +1,49 @@
+// default configuration values
+// change this file to optimal values for your application
+
+#define DEF_POWER_SUPPLY 12.0f //!< default power supply voltage
+// velocity PI controller params
+#define DEF_PID_VEL_P 0.5f //!< default PID controller P value
+#define DEF_PID_VEL_I 10.0f //!<  default PID controller I value
+#define DEF_PID_VEL_D 0.0f //!<  default PID controller D value
+#define DEF_PID_VEL_RAMP 1000.0f //!< default PID controller voltage ramp value
+#define DEF_PID_VEL_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
+
+// current sensing PID values
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)  || defined(__AVR_ATmega2560__)
+// for 16Mhz controllers like Arduino uno and mega
+#define DEF_PID_CURR_P 2 //!< default PID controller P value
+#define DEF_PID_CURR_I 100 //!<  default PID controller I value
+#define DEF_PID_CURR_D 0.0f //!<  default PID controller D value
+#define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value
+#define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
+#define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant
+#else
+// for stm32, due, teensy, esp32 and similar
+#define DEF_PID_CURR_P 3 //!< default PID controller P value
+#define DEF_PID_CURR_I 300.0f //!<  default PID controller I value
+#define DEF_PID_CURR_D 0.0f //!<  default PID controller D value
+#define DEF_PID_CURR_RAMP 0  //!< default PID controller voltage ramp value
+#define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
+#define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant
+#endif
+// default current limit values
+#define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default
+
+// default monitor downsample
+#define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample
+#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable
+
+// angle P params
+#define DEF_P_ANGLE_P 20.0f //!< default P controller P value
+#define DEF_VEL_LIM 20.0f //!< angle velocity limit default
+
+// index search
+#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0f //!< default index search velocity
+// align voltage
+#define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt
+// low pass filter velocity
+#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant
+
+// current sense default parameters
+#define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f  //!< default currnet sense per phase low pass filter time constant 

v1.3/lib/Simple_FOC/src/common/foc_utils.cpp

@@ -0,0 +1,73 @@
+#include "foc_utils.h"
+
+// int array instead of float array
+// 4x200 points per 360 deg
+// 2x storage save (int 2Byte float 4 Byte )
+// sin*10000
+const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000};
+
+// function approximating the sine calculation by using fixed size array
+// ~40us (float array)
+// ~50us (int array)
+// precision +-0.005
+// it has to receive an angle in between 0 and 2PI
+float _sin(float a){
+  if(a < _PI_2){
+    //return sine_array[(int)(199.0f*( a / (_PI/2.0)))];
+    //return sine_array[(int)(126.6873f* a)];           // float array optimized
+    return 0.0001f*sine_array[_round(126.6873f* a)];      // int array optimized
+  }else if(a < _PI){
+    // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))];
+    //return sine_array[398 - (int)(126.6873f*a)];          // float array optimized
+    return 0.0001f*sine_array[398 - _round(126.6873f*a)];     // int array optimized
+  }else if(a < _3PI_2){
+    // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))];
+    //return -sine_array[-398 + (int)(126.6873f*a)];           // float array optimized
+    return -0.0001f*sine_array[-398 + _round(126.6873f*a)];      // int array optimized
+  } else {
+    // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))];
+    //return -sine_array[796 - (int)(126.6873f*a)];           // float array optimized
+    return -0.0001f*sine_array[796 - _round(126.6873f*a)];      // int array optimized
+  }
+}
+
+// function approximating cosine calculation by using fixed size array
+// ~55us (float array)
+// ~56us (int array)
+// precision +-0.005
+// it has to receive an angle in between 0 and 2PI
+float _cos(float a){
+  float a_sin = a + _PI_2;
+  a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin;
+  return _sin(a_sin);
+}
+
+
+// normalizing radian angle to [0,2PI]
+float _normalizeAngle(float angle){
+  float a = fmod(angle, _2PI);
+  return a >= 0 ? a : (a + _2PI);
+}
+
+// Electrical angle calculation
+float _electricalAngle(float shaft_angle, int pole_pairs) {
+  return (shaft_angle * pole_pairs);
+}
+
+// square root approximation function using
+// https://reprap.org/forum/read.php?147,219210
+// https://en.wikipedia.org/wiki/Fast_inverse_square_root
+float _sqrtApprox(float number) {//low in fat
+  long i;
+  float y;
+  // float x;
+  // const float f = 1.5F; // better precision
+
+  // x = number * 0.5F;
+  y = number;
+  i = * ( long * ) &y;
+  i = 0x5f375a86 - ( i >> 1 );
+  y = * ( float * ) &i;
+  // y = y * ( f - ( x * y * y ) ); // better precision
+  return number * y;
+}

v1.3/lib/Simple_FOC/src/common/foc_utils.h

@@ -0,0 +1,92 @@
+#ifndef FOCUTILS_LIB_H
+#define FOCUTILS_LIB_H
+
+#include "Arduino.h"
+
+// sign function
+#define _sign(a) ( ( (a) < 0 )  ?  -1   : ( (a) > 0 ) )
+#define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f))
+#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+#define _sqrt(a) (_sqrtApprox(a))
+#define _isset(a) ( (a) != (NOT_SET) )
+#define _UNUSED(v) (void) (v)
+
+// utility defines
+#define _2_SQRT3 1.15470053838f
+#define _SQRT3 1.73205080757f
+#define _1_SQRT3 0.57735026919f
+#define _SQRT3_2 0.86602540378f
+#define _SQRT2 1.41421356237f
+#define _120_D2R 2.09439510239f
+#define _PI 3.14159265359f
+#define _PI_2 1.57079632679f
+#define _PI_3 1.0471975512f
+#define _2PI 6.28318530718f
+#define _3PI_2 4.71238898038f
+#define _PI_6 0.52359877559f
+
+#define NOT_SET -12345.0
+#define _HIGH_IMPEDANCE 0
+#define _HIGH_Z _HIGH_IMPEDANCE
+#define _ACTIVE 1
+
+// dq current structure
+struct DQCurrent_s
+{
+    float d;
+    float q;
+};
+// phase current structure
+struct PhaseCurrent_s
+{
+    float a;
+    float b;
+    float c;
+};
+// dq voltage structs
+struct DQVoltage_s
+{
+    float d;
+    float q;
+};
+
+
+/**
+ *  Function approximating the sine calculation by using fixed size array
+ * - execution time ~40us (Arduino UNO)
+ *
+ * @param a angle in between 0 and 2PI
+ */
+float _sin(float a);
+/**
+ * Function approximating cosine calculation by using fixed size array
+ * - execution time ~50us (Arduino UNO)
+ *
+ * @param a angle in between 0 and 2PI
+ */
+float _cos(float a);
+
+/**
+ * normalizing radian angle to [0,2PI]
+ * @param angle - angle to be normalized
+ */
+float _normalizeAngle(float angle);
+
+
+/**
+ * Electrical angle calculation
+ *
+ * @param shaft_angle - shaft angle of the motor
+ * @param pole_pairs - number of pole pairs
+ */
+float _electricalAngle(float shaft_angle, int pole_pairs);
+
+/**
+ * Function approximating square root function
+ *  - using fast inverse square root
+ *
+ * @param value - number
+ */
+float _sqrtApprox(float value);
+
+#endif

v1.3/lib/Simple_FOC/src/common/lowpass_filter.cpp

@@ -0,0 +1,28 @@
+#include "lowpass_filter.h"
+
+LowPassFilter::LowPassFilter(float time_constant)
+    : Tf(time_constant)
+    , y_prev(0.0f)
+{
+    timestamp_prev = _micros();
+}
+
+
+float LowPassFilter::operator() (float x)
+{
+    unsigned long timestamp = _micros();
+    float dt = (timestamp - timestamp_prev)*1e-6f;
+
+    if (dt < 0.0f ) dt = 1e-3f;
+    else if(dt > 0.3f) {
+        y_prev = x;
+        timestamp_prev = timestamp;
+        return x;
+    }
+
+    float alpha = Tf/(Tf + dt);
+    float y = alpha*y_prev + (1.0f - alpha)*x;
+    y_prev = y;
+    timestamp_prev = timestamp;
+    return y;
+}

v1.3/lib/Simple_FOC/src/common/lowpass_filter.h

@@ -0,0 +1,28 @@
+#ifndef LOWPASS_FILTER_H
+#define LOWPASS_FILTER_H
+
+
+#include "time_utils.h"
+#include "foc_utils.h"
+
+/**
+ *  Low pass filter class
+ */
+class LowPassFilter
+{
+public:
+    /**
+     * @param Tf - Low pass filter time constant
+     */
+    LowPassFilter(float Tf);
+    ~LowPassFilter() = default;
+
+    float operator() (float x);
+    float Tf; //!< Low pass filter time constant
+
+protected:
+    unsigned long timestamp_prev;  //!< Last execution timestamp
+    float y_prev; //!< filtered value in previous execution step 
+};
+
+#endif // LOWPASS_FILTER_H

v1.3/lib/Simple_FOC/src/common/pid.cpp

@@ -0,0 +1,58 @@
+#include "pid.h"
+
+PIDController::PIDController(float P, float I, float D, float ramp, float limit)
+    : P(P)
+    , I(I)
+    , D(D)
+    , output_ramp(ramp)    // output derivative limit [volts/second]
+    , limit(limit)         // output supply limit     [volts]
+    , error_prev(0.0f)
+    , output_prev(0.0f)
+    , integral_prev(0.0f)
+{
+    timestamp_prev = _micros();
+}
+
+// PID controller function
+float PIDController::operator() (float error){
+    // calculate the time from the last call
+    unsigned long timestamp_now = _micros();
+    float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
+    // quick fix for strange cases (micros overflow)
+    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+    // u(s) = (P + I/s + Ds)e(s)
+    // Discrete implementations
+    // proportional part
+    // u_p  = P *e(k)
+    float proportional = P * error;
+    // Tustin transform of the integral part
+    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
+    float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
+    // antiwindup - limit the output
+    integral = _constrain(integral, -limit, limit);
+    // Discrete derivation
+    // u_dk = D(ek - ek_1)/Ts
+    float derivative = D*(error - error_prev)/Ts;
+
+    // sum all the components
+    float output = proportional + integral + derivative;
+    // antiwindup - limit the output variable
+    output = _constrain(output, -limit, limit);
+
+    // if output ramp defined
+    if(output_ramp > 0){
+        // limit the acceleration by ramping the output
+        float output_rate = (output - output_prev)/Ts;
+        if (output_rate > output_ramp)
+            output = output_prev + output_ramp*Ts;
+        else if (output_rate < -output_ramp)
+            output = output_prev - output_ramp*Ts;
+    }
+    // saving for the next pass
+    integral_prev = integral;
+    output_prev = output;
+    error_prev = error;
+    timestamp_prev = timestamp_now;
+    return output;
+}

v1.3/lib/Simple_FOC/src/common/pid.h

@@ -0,0 +1,44 @@
+#ifndef PID_H
+#define PID_H
+
+
+#include "time_utils.h"
+#include "foc_utils.h"
+
+/**
+ *  PID controller class
+ */
+class PIDController
+{
+public:
+    /**
+     *  
+     * @param P - Proportional gain 
+     * @param I - Integral gain
+     * @param D - Derivative gain 
+     * @param ramp - Maximum speed of change of the output value
+     * @param limit - Maximum output value
+     */
+    PIDController(float P, float I, float D, float ramp, float limit);
+    ~PIDController() = default;
+
+    float operator() (float error);
+
+    float P; //!< Proportional gain 
+    float I; //!< Integral gain 
+    float D; //!< Derivative gain 
+    float output_ramp; //!< Maximum speed of change of the output value
+    float limit; //!< Maximum output value
+
+    float error_prev; //!< last tracking error value
+
+protected:
+    //float error_prev; //!< last tracking error value
+    float output_prev;  //!< last pid output value
+    float integral_prev; //!< last integral component value
+    unsigned long timestamp_prev; //!< Last execution timestamp
+};
+
+#endif // PID_H
+
+//Lyf对此进行了修改，将error_prev从protected移至public，便于清楚积分影响

v1.3/lib/Simple_FOC/src/common/time_utils.cpp

@@ -0,0 +1,31 @@
+#include "time_utils.h"
+
+// function buffering delay() 
+// arduino uno function doesn't work well with interrupts
+void _delay(unsigned long ms){
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)  || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__)
+  // if arduino uno and other atmega328p chips
+  // use while instad of delay, 
+  // due to wrong measurement based on changed timer0
+  unsigned long t = _micros() + ms*1000;
+  while( _micros() < t ){}; 
+#else
+  // regular micros
+  delay(ms);
+#endif
+}
+
+
+// function buffering _micros() 
+// arduino function doesn't work well with interrupts
+unsigned long _micros(){
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)  || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__)
+// if arduino uno and other atmega328p chips
+    //return the value based on the prescaler
+    if((TCCR0B & 0b00000111) == 0x01) return (micros()/32);
+    else return (micros());
+#else
+  // regular micros
+  return micros();
+#endif
+}

v1.3/lib/Simple_FOC/src/common/time_utils.h

@@ -0,0 +1,22 @@
+#ifndef TIME_UTILS_H
+#define TIME_UTILS_H
+
+#include "foc_utils.h"
+
+/** 
+ * Function implementing delay() function in milliseconds 
+ * - blocking function
+ * - hardware specific
+
+ * @param ms number of milliseconds to wait
+ */
+void _delay(unsigned long ms);
+
+/** 
+ * Function implementing timestamp getting function in microseconds
+ * hardware specific
+ */
+unsigned long _micros();
+
+
+#endif

v1.3/lib/Simple_FOC/src/communication/Commander.cpp

@@ -0,0 +1,614 @@
+#include "Commander.h"
+
+Commander::Commander(Stream& serial, char eol, bool echo){
+  com_port = &serial;
+  this->eol = eol;
+  this->echo = echo;
+}
+Commander::Commander(char eol, bool echo){
+  this->eol = eol;
+  this->echo = echo;
+}
+
+
+void Commander::add(char id, CommandCallback onCommand, char* label ){
+  call_list[call_count] = onCommand;
+  call_ids[call_count] = id;
+  call_label[call_count] = label;
+  call_count++;
+}
+
+
+void Commander::run(){
+  if(!com_port) return;
+  run(*com_port, eol);
+}
+
+void Commander::run(Stream& serial, char eol){
+  Stream* tmp = com_port; // save the serial instance
+  char eol_tmp = this->eol;
+  this->eol = eol;
+  com_port = &serial;
+
+  // a string to hold incoming data
+  while (serial.available()) {
+    // get the new byte:
+    int ch = serial.read();
+    received_chars[rec_cnt++] = (char)ch;
+    // end of user input
+    if(echo)
+      print((char)ch);
+    if (isSentinel(ch)) {
+      // execute the user command
+      run(received_chars);
+
+      // reset the command buffer
+      received_chars[0] = 0;
+      rec_cnt=0;
+    }
+    if (rec_cnt>=MAX_COMMAND_LENGTH) { // prevent buffer overrun if message is too long
+        received_chars[0] = 0;
+        rec_cnt=0;
+    }
+  }
+
+  com_port = tmp; // reset the instance to the internal value
+  this->eol = eol_tmp;
+}
+
+void Commander::run(char* user_input){
+  // execute the user command
+  char id = user_input[0];
+  switch(id){
+    case CMD_SCAN:
+      for(int i=0; i < call_count; i++){
+          print(call_ids[i]);
+          print(":");
+          if(call_label[i]) println(call_label[i]);
+          else println("");
+      }
+      break;
+    case CMD_VERBOSE:
+      if(!isSentinel(user_input[1])) verbose = (VerboseMode)atoi(&user_input[1]);
+      printVerbose(F("Verb:"));
+      switch (verbose){
+      case VerboseMode::nothing:
+        println(F("off!"));
+        break;
+      case VerboseMode::on_request:
+      case VerboseMode::user_friendly:
+        println(F("on!"));
+        break;
+      }
+      break;
+    case CMD_DECIMAL:
+      if(!isSentinel(user_input[1])) decimal_places = atoi(&user_input[1]);
+      printVerbose(F("Decimal:"));
+      println(decimal_places);
+      break;
+    default:
+      for(int i=0; i < call_count; i++){
+        if(id == call_ids[i]){
+          call_list[i](&user_input[1]);
+          break;
+        }
+      }
+      break;
+  }
+}
+
+void Commander::motor(FOCMotor* motor, char* user_command) {
+
+  // if target setting
+  if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){
+    target(motor, user_command);
+    return;
+  }
+
+  // parse command letter
+  char cmd = user_command[0];
+  char sub_cmd = user_command[1];
+  // check if there is a subcommand or not
+  int value_index = (sub_cmd >= 'A'  && sub_cmd <= 'Z') ?  2 :  1;
+  // check if get command
+  bool GET = isSentinel(user_command[value_index]);
+  // parse command values
+  float value = atof(&user_command[value_index]);
+
+
+  // a bit of optimisation of variable memory for Arduino UNO (atmega328)
+  switch(cmd){
+    case CMD_C_Q_PID:      //
+      printVerbose(F("PID curr q| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]);
+      else pid(&motor->PID_current_q,&user_command[1]);
+      break;
+    case CMD_C_D_PID:      //
+      printVerbose(F("PID curr d| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]);
+      else pid(&motor->PID_current_d, &user_command[1]);
+      break;
+    case CMD_V_PID:      //
+      printVerbose(F("PID vel| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]);
+      else pid(&motor->PID_velocity, &user_command[1]);
+      break;
+    case CMD_A_PID:      //
+      printVerbose(F("PID angle| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]);
+      else pid(&motor->P_angle, &user_command[1]);
+      break;
+    case CMD_LIMITS:      //
+     printVerbose(F("Limits| "));
+      switch (sub_cmd){
+        case SCMD_LIM_VOLT:      // voltage limit change
+          printVerbose(F("volt: "));
+          if(!GET) {
+            motor->voltage_limit = value;
+            motor->PID_current_d.limit = value;
+            motor->PID_current_q.limit = value;
+            // change velocity pid limit if in voltage mode and no phase resistance set
+            if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value;
+          }
+          println(motor->voltage_limit);
+          break;
+        case SCMD_LIM_CURR:      // current limit
+          printVerbose(F("curr: "));
+          if(!GET){
+            motor->current_limit = value;
+            // if phase resistance specified or the current control is on set the current limit to the velocity PID
+            if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value;
+          }
+          println(motor->current_limit);
+          break;
+        case SCMD_LIM_VEL:      // velocity limit
+          printVerbose(F("vel: "));
+          if(!GET){
+            motor->velocity_limit = value;
+            motor->P_angle.limit = value;
+          }
+          println(motor->velocity_limit);
+          break;
+        default:
+          printError();
+          break;
+      }
+      break;
+    case CMD_MOTION_TYPE:
+    case CMD_TORQUE_TYPE:
+    case CMD_STATUS:
+      motion(motor, &user_command[0]);
+      break;
+    case CMD_PWMMOD:
+       // PWM modulation change
+       printVerbose(F("PWM Mod | "));
+       switch (sub_cmd){
+        case SCMD_PWMMOD_TYPE:      // zero offset
+          printVerbose(F("type: "));
+          if(!GET) motor->foc_modulation = (FOCModulationType)value;
+          switch(motor->foc_modulation){
+            case FOCModulationType::SinePWM:
+              println(F("SinePWM"));
+              break;
+            case FOCModulationType::SpaceVectorPWM:
+              println(F("SVPWM"));
+              break;
+            case FOCModulationType::Trapezoid_120:
+              println(F("Trap 120"));
+              break;
+            case FOCModulationType::Trapezoid_150:
+              println(F("Trap 150"));
+              break;
+          }
+          break;
+        case SCMD_PWMMOD_CENTER:      // centered modulation
+          printVerbose(F("center: "));
+          if(!GET) motor->modulation_centered = value;
+          println(motor->modulation_centered);
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    case CMD_RESIST:
+      // enable/disable
+      printVerbose(F("R phase: "));
+      if(!GET){
+        motor->phase_resistance = value;
+        if(motor->torque_controller==TorqueControlType::voltage){
+          motor->voltage_limit = motor->current_limit*value;
+          motor->PID_velocity.limit= motor->current_limit;
+        }
+      }
+      if(_isset(motor->phase_resistance)) println(motor->phase_resistance);
+      else println(0);
+      break;
+    case CMD_SENSOR:
+      // Sensor zero offset
+       printVerbose(F("Sensor | "));
+       switch (sub_cmd){
+        case SCMD_SENS_MECH_OFFSET:      // zero offset
+          printVerbose(F("offset: "));
+          if(!GET) motor->sensor_offset = value;
+          println(motor->sensor_offset);
+          break;
+        case SCMD_SENS_ELEC_OFFSET:      // electrical zero offset - not suggested to touch
+          printVerbose(F("el. offset: "));
+          if(!GET) motor->zero_electric_angle = value;
+          println(motor->zero_electric_angle);
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    case CMD_MONITOR:     // get current values of the state variables
+      printVerbose(F("Monitor | "));
+      switch (sub_cmd){
+        case SCMD_GET:      // get command
+          switch((uint8_t)value){
+            case 0: // get target
+              printVerbose(F("target: "));
+              println(motor->target);
+              break;
+            case 1: // get voltage q
+              printVerbose(F("Vq: "));
+              println(motor->voltage.q);
+              break;
+            case 2: // get voltage d
+              printVerbose(F("Vd: "));
+              println(motor->voltage.d);
+              break;
+            case 3: // get current q
+              printVerbose(F("Cq: "));
+              println(motor->current.q);
+              break;
+            case 4: // get current d
+              printVerbose(F("Cd: "));
+              println(motor->current.d);
+              break;
+            case 5: // get velocity
+              printVerbose(F("vel: "));
+              println(motor->shaft_velocity);
+              break;
+            case 6: // get angle
+              printVerbose(F("angle: "));
+              println(motor->shaft_angle);
+              break;
+            case 7: // get all states
+              printVerbose(F("all: "));
+              print(motor->target);
+              print(";");
+              print(motor->voltage.q);
+              print(";");
+              print(motor->voltage.d);
+              print(";");
+              print(motor->current.q);
+              print(";");
+              print(motor->current.d);
+              print(";");
+              print(motor->shaft_velocity);
+              print(";");
+              println(motor->shaft_angle);
+              break;
+            default:
+              printError();
+              break;
+          }
+          break;
+        case SCMD_DOWNSAMPLE:
+          printVerbose(F("downsample: "));
+          if(!GET) motor->monitor_downsample = value;
+          println((int)motor->monitor_downsample);
+          break;
+        case SCMD_CLEAR:
+          motor->monitor_variables = (uint8_t) 0;
+          println(F("clear"));
+          break;
+        case SCMD_SET:
+          if(!GET) motor->monitor_variables = (uint8_t) 0;
+          for(int i = 0; i < 7; i++){
+            if(isSentinel(user_command[value_index+i])) break;
+            if(!GET) motor->monitor_variables |=  (user_command[value_index+i] - '0') << (6-i);
+            print( (user_command[value_index+i] - '0') );
+          }
+          println("");
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    default:  // unknown cmd
+      printVerbose(F("unknown cmd "));
+      printError();
+  }
+}
+
+void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
+  char cmd = user_cmd[0];
+  char sub_cmd = user_cmd[1];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[(sub_cmd >= 'A'  && sub_cmd <= 'Z') ?  2 :  1]);
+
+  switch(cmd){
+    case CMD_MOTION_TYPE:
+      printVerbose(F("Motion:"));
+      switch(sub_cmd){
+        case SCMD_DOWNSAMPLE:
+            printVerbose(F(" downsample: "));
+            if(!GET) motor->motion_downsample = value;
+            println((int)motor->motion_downsample);
+          break;
+        default:
+          // change control type
+          if(!GET && value >= 0 && (int)value < 5) // if set command
+            motor->controller = (MotionControlType)value;
+          switch(motor->controller){
+            case MotionControlType::torque:
+              println(F("torque"));
+              break;
+            case MotionControlType::velocity:
+              println(F("vel"));
+              break;
+            case MotionControlType::angle:
+              println(F("angle"));
+              break;
+            case MotionControlType::velocity_openloop:
+              println(F("vel open"));
+              break;
+            case MotionControlType::angle_openloop:
+              println(F("angle open"));
+              break;
+          }
+            break;
+        }
+      break;
+    case CMD_TORQUE_TYPE:
+      // change control type
+      printVerbose(F("Torque: "));
+      if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command
+        motor->torque_controller = (TorqueControlType)value;
+      switch(motor->torque_controller){
+        case TorqueControlType::voltage:
+          println(F("volt"));
+          // change the velocity control limits if necessary
+          if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit;
+          break;
+        case TorqueControlType::dc_current:
+          println(F("dc curr"));
+          // change the velocity control limits if necessary
+          motor->PID_velocity.limit = motor->current_limit;
+          break;
+        case TorqueControlType::foc_current:
+          println(F("foc curr"));
+          // change the velocity control limits if necessary
+          motor->PID_velocity.limit = motor->current_limit;
+          break;
+      }
+      break;
+    case CMD_STATUS:
+      // enable/disable
+      printVerbose(F("Status: "));
+      if(!GET) (bool)value ? motor->enable() : motor->disable();
+       println(motor->enabled);
+      break;
+    default:
+      target(motor,  user_cmd, separator);
+      break;
+  }
+}
+
+void Commander::pid(PIDController* pid, char* user_cmd){
+  char cmd = user_cmd[0];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[1]);
+
+  switch (cmd){
+    case SCMD_PID_P:      // P gain change
+      printVerbose("P: ");
+      if(!GET) pid->P = value;
+      println(pid->P);
+      break;
+    case SCMD_PID_I:      // I gain change
+      printVerbose("I: ");
+      if(!GET) pid->I = value;
+      println(pid->I);
+      break;
+    case SCMD_PID_D:      // D gain change
+      printVerbose("D: ");
+      if(!GET) pid->D = value;
+      println(pid->D);
+      break;
+    case SCMD_PID_RAMP:      //  ramp change
+      printVerbose("ramp: ");
+      if(!GET) pid->output_ramp = value;
+      println(pid->output_ramp);
+      break;
+    case SCMD_PID_LIM:      //  limit change
+      printVerbose("limit: ");
+      if(!GET) pid->limit = value;
+      println(pid->limit);
+      break;
+    default:
+      printError();
+      break;
+  }
+}
+
+void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
+  char cmd = user_cmd[0];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[1]);
+
+  switch (cmd){
+    case SCMD_LPF_TF:      // Tf value change
+      printVerbose(F("Tf: "));
+      if(!GET) lpf->Tf = value;
+      println(lpf->Tf);
+      break;
+    default:
+      printError();
+      break;
+  }
+}
+
+void Commander::scalar(float* value,  char* user_cmd){
+  bool GET  = isSentinel(user_cmd[0]);
+  if(!GET) *value = atof(user_cmd);
+  println(*value);
+}
+
+
+void Commander::target(FOCMotor* motor,  char* user_cmd, char* separator){
+  // if no values sent
+  if(isSentinel(user_cmd[0])) return;
+  
+  float pos, vel, torque;
+  char* next_value;
+  switch(motor->controller){
+    case MotionControlType::torque: // setting torque target
+      torque = atof(strtok (user_cmd, separator));
+      motor->target = torque;
+      break;
+    case MotionControlType::velocity: // setting velocity target + torque limit
+      // set the target
+      vel= atof(strtok (user_cmd, separator));
+      motor->target = vel;
+
+      // allow for setting only the target velocity without chaning the torque limit
+      next_value = strtok (NULL, separator);
+      if (next_value){
+        torque = atof(next_value);
+        motor->PID_velocity.limit = torque;
+        // torque command can be voltage or current
+        if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
+        else  motor->current_limit = torque;
+      }
+      break;
+    case MotionControlType::angle: // setting angle target + torque, velocity limit
+      // setting the target position
+      pos= atof(strtok (user_cmd, separator));
+      motor->target = pos;
+
+      // allow for setting only the target position without chaning the velocity/torque limits 
+      next_value = strtok (NULL, separator);
+      if( next_value ){
+        vel = atof(next_value);
+        motor->velocity_limit = vel;
+        motor->P_angle.limit = vel;
+  
+        // allow for setting only the target position and velocity limit without the torque limit 
+        next_value = strtok (NULL, separator);
+        if( next_value ){
+          torque= atof(next_value);
+          motor->PID_velocity.limit = torque;
+          // torque command can be voltage or current
+          if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
+          else  motor->current_limit = torque;
+        }
+      }
+      break;
+    case MotionControlType::velocity_openloop: // setting velocity target + torque limit
+      // set the target
+      vel= atof(strtok (user_cmd, separator));
+      motor->target = vel;
+      // allow for setting only the target velocity without chaning the torque limit
+      next_value = strtok (NULL, separator);
+      if (next_value ){
+        torque = atof(next_value);
+        // torque command can be voltage or current
+        if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
+        else  motor->current_limit = torque;
+      }
+      break;
+    case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit
+      // set the target
+      pos= atof(strtok (user_cmd, separator));
+      motor->target = pos; 
+      
+      // allow for setting only the target position without chaning the velocity/torque limits 
+      next_value = strtok (NULL, separator);
+      if( next_value ){
+        vel = atof(next_value);
+        motor->velocity_limit = vel;
+        // allow for setting only the target velocity without chaning the torque limit
+        next_value = strtok (NULL, separator);
+        if (next_value ){
+          torque = atof(next_value);
+          // torque command can be voltage or current
+          if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
+          else  motor->current_limit = torque;
+        }
+      }
+      break;
+  }
+  printVerbose(F("Target: "));
+  println(motor->target);
+}
+
+
+bool Commander::isSentinel(char ch)
+{
+  if(ch == eol)
+    return true;
+  else if (ch == '\r')
+  {
+      printVerbose(F("Warn: \\r detected! \n"));
+  }
+  return false;
+}
+
+void Commander::print(const int number){
+  if( !com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(number);
+}
+void Commander::print(const float number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print((float)number,(int)decimal_places);
+}
+void Commander::print(const char* message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+void Commander::print(const __FlashStringHelper *message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+void Commander::print(const char message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+
+void Commander::println(const int number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(number);
+}
+void Commander::println(const float number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println((float)number, (int)decimal_places);
+}
+void Commander::println(const char* message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+void Commander::println(const __FlashStringHelper *message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+void Commander::println(const char message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+
+
+void Commander::printVerbose(const char* message){
+  if(verbose == VerboseMode::user_friendly) print(message);
+}
+void Commander::printVerbose(const __FlashStringHelper *message){
+  if(verbose == VerboseMode::user_friendly) print(message);
+}
+void Commander::printError(){
+ println(F("err"));
+}

v1.3/lib/Simple_FOC/src/communication/Commander.h

@@ -0,0 +1,287 @@
+#ifndef COMMANDS_H
+#define COMMANDS_H
+
+#include "Arduino.h"
+#include "../common/base_classes/FOCMotor.h"
+#include "../common/pid.h"
+#include "../common/lowpass_filter.h"
+#include "commands.h"
+
+
+#define MAX_COMMAND_LENGTH 20
+
+
+// Commander verbose display to the user type
+enum VerboseMode : uint8_t {
+  nothing       = 0x00, // display nothing - good for monitoring
+  on_request    = 0x01, // display only on user request
+  user_friendly = 0x02  // display textual messages to the user
+};
+
+
+// callback function pointer definiton
+typedef void (* CommandCallback)(char*); //!< command callback function pointer
+
+/**
+ * Commander class implementing string communication protocol based on IDvalue (ex AB5.321 - command id `A`, sub-command id `B`,value `5.321`)
+ *
+ *  - This class can be used in combination with HardwareSerial instance which it would read and write
+ *    or it can be used to parse strings that have been received from the user outside this library
+ *  - Commander class implements command protocol for few standard components of the SimpleFOC library
+ *     - FOCMotor
+ *     - PIDController
+ *     - LowPassFilter
+ *  - Commander also provides a very simple command > callback interface that enables user to
+ *    attach a callback function to certain command id - see function add()
+ */
+class Commander
+{
+  public:
+    /**
+     * Default constructor receiving a serial interface that it uses to output the values to
+     * Also if the function run() is used it uses this serial instance to read the serial for user commands
+     *
+     * @param serial - Serial com port instance
+     * @param eol - the end of line sentinel character
+     * @param echo - echo last typed character (for command line feedback)
+     */
+    Commander(Stream &serial, char eol = '\n', bool echo = false);
+    Commander(char eol = '\n', bool echo = false);
+
+    /**
+     * Function reading the serial port and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     */
+    void run();
+    /**
+     * Function reading the string of user input and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     *
+     * @param reader - temporary stream to read user input
+     * @param eol - temporary end of line sentinel
+     */
+    void run(Stream &reader, char eol = '\n');
+    /**
+     * Function reading the string of user input and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     *
+     * @param user_input - string of user inputs
+     */
+    void run(char* user_input);
+
+    /**
+     *  Function adding a callback to the coomander withe the command id
+     * @param id         - char command letter
+     * @param onCommand  - function pointer void function(char*)
+     * @param label      - string label to be displayed when scan command sent
+     */
+    void add(char id , CommandCallback onCommand, char* label = nullptr);
+
+    // printing variables
+    VerboseMode verbose = VerboseMode::user_friendly; //!< flag signaling that the commands should output user understanable text
+    uint8_t decimal_places = 3; //!< number of decimal places to be used when displaying numbers
+
+    // monitoring functions
+    Stream* com_port = nullptr; //!< Serial terminal variable if provided
+    char eol = '\n'; //!< end of line sentinel character
+    bool echo = false; //!< echo last typed character (for command line feedback)
+
+    /**
+     *
+     * FOC motor (StepperMotor and BLDCMotor) command interface
+     * @param motor    - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It has several paramters (the letters can be changed in the commands.h file)
+     *    'Q' - Q current PID controller & LPF (see function pid and lpf for commands)
+     *    'D' - D current PID controller & LPF (see function pid and lpf for commands)
+     *    'V' - Velocity PID controller & LPF  (see function pid and lpf for commands)
+     *    'A' - Angle PID controller & LPF     (see function pid and lpf for commands)
+     *    'L' - Limits
+     *           sub-commands:
+     *           'C' - Current
+     *           'U' - Voltage
+     *           'V' - Velocity
+     *    'C' - Motion control type config
+     *          sub-commands:
+     *          'D' - downsample motiron loop
+     *          '0' - torque
+     *          '1' - velocity
+     *          '2' - angle
+     *    'T' - Torque control type
+     *          sub-commands:
+     *          '0' - voltage
+     *          '1' - current
+     *          '2' - foc_current
+     *    'E' - Motor status (enable/disable)
+     *          sub-commands:
+     *          '0' - enable
+     *          '1' - disable
+     *    'R' - Motor resistance
+     *    'S' - Sensor offsets
+     *          sub-commands:
+     *          'M' - sensor offset
+     *          'E' - sensor electrical zero
+     *    'M' - Monitoring control
+     *          sub-commands:
+     *          'D' - downsample monitoring
+     *          'C' - clear monitor
+     *          'S' - set monitoring variables
+     *          'G' - get variable value
+     *    '' - Target setting interface 
+     *         Depends of the motion control mode:
+     *          - torque                          : torque (ex. M2.5) 
+     *          - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits)
+     *          - angle    (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits)
+     *
+     *  - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance)
+     *  - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f)
+     *
+     */
+    void motor(FOCMotor* motor, char* user_cmd);
+
+    /**
+     * Low pass fileter command interface
+     * @param lpf      - LowPassFilter instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It only has one property - filtering time constant Tf
+     *  - It can be get by sending 'F'
+     *  - It can be set by sending 'Fvalue' - (ex. F0.01 for settin Tf=0.01)
+     */
+    void lpf(LowPassFilter* lpf, char* user_cmd);
+    /**
+     * PID controller command interface
+     * @param pid      - PIDController instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It has several paramters (the letters can be changed in the commands.h file)
+     *     - P gain       - 'P'
+     *     - I gain       - 'I'
+     *     - D gain       - 'D'
+     *     - output ramp  - 'R'
+     *     - output limit - 'L'
+     *  - Each of them can be get by sening the command letter -(ex. 'D' - to get the D gain)
+     *  - Each of them can be set by sending 'IDvalue' - (ex. I1.5 for setting I=1.5)
+     */
+    void pid(PIDController* pid, char* user_cmd);
+    /**
+     * Float variable scalar command interface
+     * @param value    - float variable pointer 
+     * @param user_cmd - the string command
+     * 
+     *  - It only has one property - one float value
+     *  - It can be get by sending an empty string '\n'
+     *  - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01)
+     */
+    void scalar(float* value, char* user_cmd);
+    /**
+     *  Target setting interface, enables setting the target and limiting variables at once. 
+     *  The values are sent separated by a separator specified as the third argument. The default separator is the space.
+     * 
+     * @param motor     - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd  - the string command
+     * @param separator - the string separator in between target and limit values, default is space - " "
+     *  
+     *  Example: P2.34 70 2
+     *  `P` is the user defined command, `2.34` is the target angle `70` is the target 
+     *  velocity and `2` is the desired max current.
+     *  
+     *  It depends of the motion control mode:
+     *    - torque   : torque (ex. P2.5) 
+     *    - velocity : velocity torque (ex.P10 2.5 or P10 to only chanage the target witout limits)
+     *    - angle    : angle velocity torque (ex.P3.5 10 2.5 or P3.5 to only chanage the target witout limits)
+     */
+    void target(FOCMotor* motor, char* user_cmd, char* separator = " ");
+
+    /**
+     * FOC motor (StepperMotor and BLDCMotor) motion control interfaces
+     * @param motor     - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd  - the string command
+     * @param separator - the string separator in between target and limit values, default is space - " "
+     * 
+     * Commands:
+     *    'C' - Motion control type config
+     *          sub-commands:
+     *          'D' - downsample motiron loop
+     *          '0' - torque
+     *          '1' - velocity
+     *          '2' - angle
+     *    'T' - Torque control type
+     *          sub-commands:
+     *          '0' - voltage
+     *          '1' - current
+     *          '2' - foc_current
+     *    'E' - Motor status (enable/disable)
+     *          sub-commands:
+     *          '0' - enable
+     *          '1' - disable
+     *    '' - Target setting interface 
+     *         Depends of the motion control mode:
+     *          - torque                          : torque (ex. M2.5) 
+     *          - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits)
+     *          - angle    (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits)
+     */
+    void motion(FOCMotor* motor, char* user_cmd, char* separator = " ");
+
+  private:
+    // Subscribed command callback variables
+    CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number
+    char call_ids[20]; //!< added callback commands
+    char* call_label[20]; //!< added callback labels
+    int call_count = 0;//!< number callbacks that are subscribed
+
+    // helping variable for serial communication reading
+    char received_chars[MAX_COMMAND_LENGTH] = {0}; //!< so far received user message - waiting for newline
+    int rec_cnt = 0; //!< number of characters receives
+
+    // serial printing functions
+    /**
+     *  print the string message only if verbose mode on
+     *  @param message - message to be printed
+     */
+    void printVerbose(const char* message);
+    /**
+     *  Print the string message only if verbose mode on
+     *  - Function handling the case for strings defined by F macro
+     *  @param message - message to be printed
+     */
+    void printVerbose(const __FlashStringHelper *message);
+    /**
+     *  print the numbers to the serial with desired decimal point number
+     *  @param message - number to be printed
+     *  @param newline - if needs lewline (1) otherwise (0)
+     */
+
+    void print(const float number);
+    void print(const int number);
+    void print(const char* message);
+    void print(const __FlashStringHelper *message);
+    void print(const char message);
+    void println(const float number);
+    void println(const int number);
+    void println(const char* message);
+    void println(const __FlashStringHelper *message);
+    void println(const char message);
+
+    void printError();
+    bool isSentinel(char ch);
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/communication/StepDirListener.cpp

@@ -0,0 +1,40 @@
+#include "StepDirListener.h"
+
+StepDirListener::StepDirListener(int _pinStep, int _pinDir, float _counter_to_value){
+    pin_step = _pinStep;
+    pin_dir = _pinDir;
+    counter_to_value = _counter_to_value;
+}
+
+void StepDirListener::init(){
+    pinMode(pin_dir, INPUT);
+    pinMode(pin_step, INPUT_PULLUP);
+    count = 0;
+}
+
+void StepDirListener::enableInterrupt(void (*doA)()){
+    attachInterrupt(digitalPinToInterrupt(pin_step), doA, CHANGE);
+}
+
+void StepDirListener::attach(float* variable){
+    attached_variable = variable;
+}
+
+void StepDirListener::handle(){ 
+  // read step status
+  bool step = digitalRead(pin_step);
+  // update counter only on rising edge 
+  if(step && step != step_active){
+     if(digitalRead(pin_dir)) 
+        count++;
+     else 
+        count--;
+   }
+   step_active = step;
+   // if attached variable update it
+   if(attached_variable) *attached_variable = getValue();
+}
+// calculate the position from counter
+float StepDirListener::getValue(){
+    return (float) count * counter_to_value;
+}

v1.3/lib/Simple_FOC/src/communication/StepDirListener.h

@@ -0,0 +1,57 @@
+#ifndef STEPDIR_H
+#define STEPDIR_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+
+/**
+ * Step/Dir listenner class for easier interraction with this communication interface.
+ */
+class StepDirListener
+{
+  public:
+
+    /**
+     * Constructor for step/direction interface
+     *  @param step              - pin
+     *  @param direction         - pin
+     *  @param counter_to_value  - step counter to value
+     */
+    StepDirListener(int pinStep, int pinDir, float counter_to_value = 1);
+    /**
+     * Start listenning for step commands
+     * 
+     * @param handleStep - on step received handler
+     */
+    void enableInterrupt(void (*handleStep)());
+
+    /**
+     * Initialise dir and step commands
+     */
+    void init();
+    /**
+     * step handler
+     */
+    void handle();
+    /**
+     * Get so far received valued
+     */
+    float getValue();
+    /**
+     * Attach the value to be updated on each step receive 
+     * - no need to call getValue function
+     */
+    void attach(float* variable);
+
+    // variables
+    int pin_step; //!< step pin
+    int pin_dir; //!< direction pin
+    long count; //!< current counter value - should be set to 0 for homing
+
+  private:
+    float* attached_variable = nullptr; //!< pointer to the attached variable 
+    float counter_to_value; //!< step counter to value 
+    bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable
+};
+
+#endif

v1.3/lib/Simple_FOC/src/communication/commands.h

@@ -0,0 +1,50 @@
+#ifndef COMMANDS_h
+#define  COMMANDS_h
+
+// see docs.simplefoc.com for in depth explanation of the commands
+
+// list of commands
+ #define CMD_C_D_PID   'D' //!< current d PID & LPF
+ #define CMD_C_Q_PID   'Q' //!< current d PID & LPF
+ #define CMD_V_PID     'V' //!< velocity PID & LPF
+ #define CMD_A_PID     'A' //!< angle PID & LPF
+ #define CMD_STATUS    'E' //!< motor status enable/disable
+ #define CMD_LIMITS    'L' //!< limits current/voltage/velocity
+ #define CMD_MOTION_TYPE  'C' //!< motion control type
+ #define CMD_TORQUE_TYPE  'T' //!< torque control type
+ #define CMD_SENSOR    'S' //!< sensor offsets
+ #define CMD_MONITOR   'M' //!< monitoring
+ #define CMD_RESIST    'R' //!< motor phase resistance
+ #define CMD_PWMMOD   'W' //!< pwm modulation
+
+ // commander configuration
+ #define CMD_SCAN    '?' //!< command scaning the network - only for commander
+ #define CMD_VERBOSE '@' //!< command setting output mode - only for commander
+ #define CMD_DECIMAL '#' //!< command setting decimal places - only for commander
+
+ // subcomands
+ //pid - lpf
+ #define SCMD_PID_P     'P' //!< PID gain P
+ #define SCMD_PID_I     'I' //!< PID gain I
+ #define SCMD_PID_D     'D' //!< PID gain D
+ #define SCMD_PID_RAMP  'R' //!< PID ramp
+ #define SCMD_PID_LIM   'L' //!< PID limit
+ #define SCMD_LPF_TF    'F' //!< LPF time constant
+ // limits
+ #define SCMD_LIM_CURR  'C' //!< Limit current
+ #define SCMD_LIM_VOLT  'U' //!< Limit voltage
+ #define SCMD_LIM_VEL   'V' //!< Limit velocity
+ //sensor
+ #define SCMD_SENS_MECH_OFFSET 'M' //!< Sensor offset
+ #define SCMD_SENS_ELEC_OFFSET 'E' //!< Sensor electrical zero offset
+ // monitoring
+ #define SCMD_DOWNSAMPLE 'D' //!< Monitoring downsample value
+ #define SCMD_CLEAR      'C' //!< Clear all monitored variables
+ #define SCMD_GET        'G' //!< Get variable only one value
+ #define SCMD_SET        'S' //!< Set variables to be monitored
+
+ #define SCMD_PWMMOD_TYPE   'T'  //!<< Pwm modulation type
+ #define SCMD_PWMMOD_CENTER 'C'  //!<< Pwm modulation center flag
+
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/GenericCurrentSense.cpp

@@ -0,0 +1,162 @@
+#include "GenericCurrentSense.h"
+
+// GenericCurrentSense constructor
+GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){
+  // if function provided add it to the 
+  if(readCallback != nullptr) this->readCallback = readCallback;
+  if(initCallback != nullptr) this->initCallback = initCallback;
+}
+
+// Inline sensor init function
+void GenericCurrentSense::init(){
+    // configure ADC variables
+    if(initCallback != nullptr) initCallback();
+    // calibrate zero offsets
+    calibrateOffsets();
+}
+// Function finding zero offsets of the ADC
+void GenericCurrentSense::calibrateOffsets(){
+    const int calibration_rounds = 1000;
+
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        PhaseCurrent_s current = readCallback();
+        offset_ia += current.a;
+        offset_ib += current.b;
+        offset_ic += current.c;
+        _delay(1);
+    }
+    // calculate the mean offsets
+    offset_ia = offset_ia / calibration_rounds;
+    offset_ib = offset_ib / calibration_rounds;
+    offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current = readCallback();
+    current.a = (current.a - offset_ia);// amps
+    current.b = (current.a - offset_ib);// amps
+    current.c = (current.a - offset_ic); // amps
+    return current;
+}
+
+// Function synchronizing current sense with motor driver.
+// for in-line sensig no such thing is necessary
+int GenericCurrentSense::driverSync(BLDCDriver *driver){
+    return 1;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int GenericCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    // // set phase A active and phases B and C down
+    // driver->setPwm(voltage, 0, 0);
+    // _delay(200);
+    // PhaseCurrent_s c = getPhaseCurrents();
+    // // read the current 100 times ( arbitrary number )
+    // for (int i = 0; i < 100; i++) {
+    //     PhaseCurrent_s c1 = getPhaseCurrents();
+    //     c.a = c.a*0.6f + 0.4f*c1.a;
+    //     c.b = c.b*0.6f + 0.4f*c1.b;
+    //     c.c = c.c*0.6f + 0.4f*c1.c;
+    //     _delay(3);
+    // }
+    // driver->setPwm(0, 0, 0);
+    // // align phase A
+    // float ab_ratio = fabs(c.a / c.b);
+    // float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+    // if( ab_ratio > 1.5f ){ // should be ~2
+    //     gain_a *= _sign(c.a);
+    // }else if( ab_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and B
+    //     int tmp_pinA = pinA;
+    //     pinA = pinB;
+    //     pinB = tmp_pinA;
+    //     gain_a *= _sign(c.b);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and C
+    //     int tmp_pinA = pinA;
+    //     pinA = pinC;
+    //     pinC= tmp_pinA;
+    //     gain_a *= _sign(c.c);
+    //     exit_flag = 2;// signal that pins have been switched
+    // }else{
+    //     // error in current sense - phase either not measured or bad connection
+    //     return 0;
+    // }
+
+    // // set phase B active and phases A and C down
+    // driver->setPwm(0, voltage, 0);
+    // _delay(200);
+    // c = getPhaseCurrents();
+    // // read the current 50 times
+    // for (int i = 0; i < 100; i++) {
+    //     PhaseCurrent_s c1 = getPhaseCurrents();
+    //     c.a = c.a*0.6f + 0.4f*c1.a;
+    //     c.b = c.b*0.6f + 0.4f*c1.b;
+    //     c.c = c.c*0.6f + 0.4f*c1.c;
+    //     _delay(3);
+    // }
+    // driver->setPwm(0, 0, 0);
+    // float ba_ratio = fabs(c.b/c.a);
+    // float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+    //  if( ba_ratio > 1.5f ){ // should be ~2
+    //     gain_b *= _sign(c.b);
+    // }else if( ba_ratio < 0.7f ){ // it should be ~0.5
+    //     // switch phase A and B
+    //     int tmp_pinB = pinB;
+    //     pinB = pinA;
+    //     pinA = tmp_pinB;
+    //     gain_b *= _sign(c.a);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and C
+    //     int tmp_pinB = pinB;
+    //     pinB = pinC;
+    //     pinC = tmp_pinB;
+    //     gain_b *= _sign(c.c);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else{
+    //     // error in current sense - phase either not measured or bad connection
+    //     return 0;
+    // }
+
+    // // if phase C measured
+    // if(_isset(pinC)){
+    //     // set phase B active and phases A and C down
+    //     driver->setPwm(0, 0, voltage);
+    //     _delay(200);
+    //     c = getPhaseCurrents();
+    //     // read the adc voltage 500 times ( arbitrary number )
+    //     for (int i = 0; i < 50; i++) {
+    //         PhaseCurrent_s c1 = getPhaseCurrents();
+    //         c.c = (c.c+c1.c)/50.0f;
+    //     }
+    //     driver->setPwm(0, 0, 0);
+    //     gain_c *= _sign(c.c);
+    // }
+
+    // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+    return exit_flag;
+}

v1.3/lib/Simple_FOC/src/current_sense/GenericCurrentSense.h

@@ -0,0 +1,41 @@
+#ifndef GENERIC_CS_LIB_H
+#define GENERIC_CS_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "../common/base_classes/CurrentSense.h"
+#include "../common/lowpass_filter.h"
+#include "hardware_api.h"
+
+
+class GenericCurrentSense: public CurrentSense{
+  public:
+    /**
+      GenericCurrentSense class constructor
+    */
+    GenericCurrentSense(PhaseCurrent_s (*readCallback)() = nullptr, void (*initCallback)() = nullptr);
+
+    // CurrentSense interface implementing functions 
+    void init() override;
+    PhaseCurrent_s getPhaseCurrents() override;
+    int driverSync(BLDCDriver *driver) override;
+    int driverAlign(BLDCDriver *driver, float voltage) override;
+
+
+    PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading
+    void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation
+
+  private:
+    /**
+     *  Function finding zero offsets of the ADC
+     */
+    void calibrateOffsets();
+    float offset_ia; //!< zero current A voltage value (center of the adc reading)
+    float offset_ib; //!< zero current B voltage value (center of the adc reading)
+    float offset_ic; //!< zero current C voltage value (center of the adc reading)
+
+};
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/InlineCurrentSense.cpp

@@ -0,0 +1,172 @@
+#include "InlineCurrentSense.h"
+// InlineCurrentSensor constructor
+//  - shunt_resistor  - shunt resistor value
+//  - gain  - current-sense op-amp gain
+//  - phA   - A phase adc pin
+//  - phB   - B phase adc pin
+//  - phC   - C phase adc pin (optional)
+InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    shunt_resistor = _shunt_resistor;
+    amp_gain  = _gain;
+    volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+}
+
+// Inline sensor init function
+void InlineCurrentSense::init(){
+    // configure ADC variables
+    _configureADCInline(pinA,pinB,pinC);
+    // calibrate zero offsets
+    calibrateOffsets();
+}
+// Function finding zero offsets of the ADC
+void InlineCurrentSense::calibrateOffsets(){
+    const int calibration_rounds = 1000;
+
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        offset_ia += _readADCVoltageInline(pinA);
+        offset_ib += _readADCVoltageInline(pinB);
+        if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC);
+        _delay(1);
+    }
+    // calculate the mean offsets
+    offset_ia = offset_ia / calibration_rounds;
+    offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current;
+    current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps
+    current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps
+    current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps
+    return current;
+}
+// Function synchronizing current sense with motor driver.
+// for in-line sensig no such thing is necessary
+int InlineCurrentSense::driverSync(BLDCDriver *driver){
+    return 1;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    // set phase A active and phases B and C down
+    driver->setPwm(voltage, 0, 0);
+    _delay(200);
+    PhaseCurrent_s c = getPhaseCurrents();
+    // read the current 100 times ( arbitrary number )
+    for (int i = 0; i < 100; i++) {
+        PhaseCurrent_s c1 = getPhaseCurrents();
+        c.a = c.a*0.6f + 0.4f*c1.a;
+        c.b = c.b*0.6f + 0.4f*c1.b;
+        c.c = c.c*0.6f + 0.4f*c1.c;
+        _delay(3);
+    }
+    driver->setPwm(0, 0, 0);
+    // align phase A
+    float ab_ratio = fabs(c.a / c.b);
+    float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+    if( ab_ratio > 1.5f ){ // should be ~2
+        gain_a *= _sign(c.a);
+    }else if( ab_ratio < 0.7f ){ // should be ~0.5
+        // switch phase A and B
+        int tmp_pinA = pinA;
+        pinA = pinB;
+        pinB = tmp_pinA;
+        gain_a *= _sign(c.b);
+        exit_flag = 2; // signal that pins have been switched
+    }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+        // switch phase A and C
+        int tmp_pinA = pinA;
+        pinA = pinC;
+        pinC= tmp_pinA;
+        gain_a *= _sign(c.c);
+        exit_flag = 2;// signal that pins have been switched
+    }else{
+        // error in current sense - phase either not measured or bad connection
+        return 0;
+    }
+
+    // set phase B active and phases A and C down
+    driver->setPwm(0, voltage, 0);
+    _delay(200);
+    c = getPhaseCurrents();
+    // read the current 50 times
+    for (int i = 0; i < 100; i++) {
+        PhaseCurrent_s c1 = getPhaseCurrents();
+        c.a = c.a*0.6f + 0.4f*c1.a;
+        c.b = c.b*0.6f + 0.4f*c1.b;
+        c.c = c.c*0.6f + 0.4f*c1.c;
+        _delay(3);
+    }
+    driver->setPwm(0, 0, 0);
+    float ba_ratio = fabs(c.b/c.a);
+    float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+     if( ba_ratio > 1.5f ){ // should be ~2
+        gain_b *= _sign(c.b);
+    }else if( ba_ratio < 0.7f ){ // it should be ~0.5
+        // switch phase A and B
+        int tmp_pinB = pinB;
+        pinB = pinA;
+        pinA = tmp_pinB;
+        gain_b *= _sign(c.a);
+        exit_flag = 2; // signal that pins have been switched
+    }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+        // switch phase A and C
+        int tmp_pinB = pinB;
+        pinB = pinC;
+        pinC = tmp_pinB;
+        gain_b *= _sign(c.c);
+        exit_flag = 2; // signal that pins have been switched
+    }else{
+        // error in current sense - phase either not measured or bad connection
+        return 0;
+    }
+
+    // if phase C measured
+    if(_isset(pinC)){
+        // set phase B active and phases A and C down
+        driver->setPwm(0, 0, voltage);
+        _delay(200);
+        c = getPhaseCurrents();
+        // read the adc voltage 500 times ( arbitrary number )
+        for (int i = 0; i < 50; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.c = (c.c+c1.c)/50.0f;
+        }
+        driver->setPwm(0, 0, 0);
+        gain_c *= _sign(c.c);
+    }
+
+    if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+    return exit_flag;
+}

v1.3/lib/Simple_FOC/src/current_sense/InlineCurrentSense.h

@@ -0,0 +1,65 @@
+#ifndef INLINE_CS_LIB_H
+#define INLINE_CS_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "../common/base_classes/CurrentSense.h"
+#include "../common/lowpass_filter.h"
+#include "hardware_api.h"
+
+
+class InlineCurrentSense: public CurrentSense{
+  public:
+    /**
+      InlineCurrentSense class constructor
+      @param shunt_resistor shunt resistor value
+      @param gain current-sense op-amp gain
+      @param phA A phase adc pin
+      @param phB B phase adc pin
+      @param phC C phase adc pin (optional)
+    */
+    InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
+
+    // CurrentSense interface implementing functions 
+    void init() override;
+    PhaseCurrent_s getPhaseCurrents() override;
+    int driverSync(BLDCDriver *driver) override;
+    int driverAlign(BLDCDriver *driver, float voltage) override;
+
+    // ADC measuremnet gain for each phase
+    // support for different gains for different phases of more commonly - inverted phase currents
+    // this should be automated later
+    float gain_a; //!< phase A gain
+    float gain_b; //!< phase B gain
+    float gain_c; //!< phase C gain
+
+    // // per phase low pass fileters
+    // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current A low pass filter
+    // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current B low pass filter
+    // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current C low pass filter
+
+  private:
+  
+    // hardware variables
+  	int pinA; //!< pin A analog pin for current measurement
+  	int pinB; //!< pin B analog pin for current measurement
+  	int pinC; //!< pin C analog pin for current measurement
+
+    // gain variables
+    float shunt_resistor; //!< Shunt resistor value
+    float amp_gain; //!< amp gain value
+    float volts_to_amps_ratio; //!< Volts to amps ratio
+    
+    /**
+     *  Function finding zero offsets of the ADC
+     */
+    void calibrateOffsets();
+    float offset_ia; //!< zero current A voltage value (center of the adc reading)
+    float offset_ib; //!< zero current B voltage value (center of the adc reading)
+    float offset_ic; //!< zero current C voltage value (center of the adc reading)
+
+};
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/LowsideCurrentSense.cpp

@@ -0,0 +1,179 @@
+#include "LowsideCurrentSense.h"
+// LowsideCurrentSensor constructor
+//  - shunt_resistor  - shunt resistor value
+//  - gain  - current-sense op-amp gain
+//  - phA   - A phase adc pin
+//  - phB   - B phase adc pin
+//  - phC   - C phase adc pin (optional)
+LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    shunt_resistor = _shunt_resistor;
+    amp_gain  = _gain;
+    volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+}
+
+// Lowside sensor init function
+void LowsideCurrentSense::init(){
+    // configure ADC variables
+    _configureADCLowSide(pinA,pinB,pinC);
+    // sync the driver
+    _driverSyncLowSide();
+    // calibrate zero offsets
+    calibrateOffsets();
+}
+// Function finding zero offsets of the ADC
+void LowsideCurrentSense::calibrateOffsets(){    
+    const int calibration_rounds = 1000;
+
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        _startADC3PinConversionLowSide();
+        offset_ia += _readADCVoltageLowSide(pinA);
+        offset_ib += _readADCVoltageLowSide(pinB);
+        if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC);
+        _delay(1);
+    }
+    // calculate the mean offsets
+    offset_ia = offset_ia / calibration_rounds;
+    offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current;
+    _startADC3PinConversionLowSide();
+    current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps
+    current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps
+    current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps
+    return current;
+}
+// Function synchronizing current sense with motor driver.
+// for in-line sensig no such thing is necessary
+int LowsideCurrentSense::driverSync(BLDCDriver *driver){
+    _driverSyncLowSide();
+    return 1;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
+    
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    // set phase A active and phases B and C down
+    driver->setPwm(voltage, 0, 0);
+    _delay(2000);
+    PhaseCurrent_s c = getPhaseCurrents();
+    // read the current 100 times ( arbitrary number )
+    for (int i = 0; i < 100; i++) {
+        PhaseCurrent_s c1 = getPhaseCurrents();
+        c.a = c.a*0.6f + 0.4f*c1.a;
+        c.b = c.b*0.6f + 0.4f*c1.b;
+        c.c = c.c*0.6f + 0.4f*c1.c;
+        _delay(3);
+    }
+    driver->setPwm(0, 0, 0);
+    // align phase A
+    float ab_ratio = fabs(c.a / c.b);
+    float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+    if( ab_ratio > 1.5f ){ // should be ~2
+        gain_a *= _sign(c.a);
+    }else if( ab_ratio < 0.7f ){ // should be ~0.5
+        // switch phase A and B
+        int tmp_pinA = pinA;
+        pinA = pinB;
+        pinB = tmp_pinA;
+        gain_a *= _sign(c.b);
+        exit_flag = 2; // signal that pins have been switched
+    }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+        // switch phase A and C
+        int tmp_pinA = pinA;
+        pinA = pinC;
+        pinC= tmp_pinA;
+        gain_a *= _sign(c.c);
+        exit_flag = 2;// signal that pins have been switched
+    }else{
+        // error in current sense - phase either not measured or bad connection
+        return 0;
+    }
+
+    // set phase B active and phases A and C down
+    driver->setPwm(0, voltage, 0);
+    _delay(200);
+    c = getPhaseCurrents();
+    // read the current 50 times
+    for (int i = 0; i < 100; i++) {
+        PhaseCurrent_s c1 = getPhaseCurrents();
+        c.a = c.a*0.6 + 0.4f*c1.a;
+        c.b = c.b*0.6 + 0.4f*c1.b;
+        c.c = c.c*0.6 + 0.4f*c1.c;
+        _delay(3);
+    }
+    driver->setPwm(0, 0, 0);
+    float ba_ratio = fabs(c.b/c.a);
+    float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+     if( ba_ratio > 1.5f ){ // should be ~2
+        gain_b *= _sign(c.b);
+    }else if( ba_ratio < 0.7f ){ // it should be ~0.5
+        // switch phase A and B
+        int tmp_pinB = pinB;
+        pinB = pinA;
+        pinA = tmp_pinB;
+        gain_b *= _sign(c.a);
+        exit_flag = 2; // signal that pins have been switched
+    }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+        // switch phase A and C
+        int tmp_pinB = pinB;
+        pinB = pinC;
+        pinC = tmp_pinB;
+        gain_b *= _sign(c.c);
+        exit_flag = 2; // signal that pins have been switched
+    }else{
+        // error in current sense - phase either not measured or bad connection
+        return 0;
+    }
+
+    // if phase C measured
+    if(_isset(pinC)){
+        // set phase B active and phases A and C down
+        driver->setPwm(0, 0, voltage);
+        _delay(200);
+        c = getPhaseCurrents();
+        // read the adc voltage 500 times ( arbitrary number )
+        for (int i = 0; i < 50; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.c = (c.c+c1.c)/50.0f;
+        }
+        driver->setPwm(0, 0, 0);
+        gain_c *= _sign(c.c);
+    }
+
+    if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+
+    return exit_flag;
+}

v1.3/lib/Simple_FOC/src/current_sense/LowsideCurrentSense.h

@@ -0,0 +1,66 @@
+#ifndef LOWSIDE_CS_LIB_H
+#define LOWSIDE_CS_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "../common/base_classes/CurrentSense.h"
+#include "../common/base_classes/FOCMotor.h"
+#include "../common/lowpass_filter.h"
+#include "hardware_api.h"
+
+
+class LowsideCurrentSense: public CurrentSense{
+  public:
+    /**
+      LowsideCurrentSense class constructor
+      @param shunt_resistor shunt resistor value
+      @param gain current-sense op-amp gain
+      @param phA A phase adc pin
+      @param phB B phase adc pin
+      @param phC C phase adc pin (optional)
+    */
+    LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
+
+    // CurrentSense interface implementing functions
+    void init() override;
+    PhaseCurrent_s getPhaseCurrents() override;
+    int driverSync(BLDCDriver *driver) override;
+    int driverAlign(BLDCDriver *driver, float voltage) override;
+
+    // ADC measuremnet gain for each phase
+    // support for different gains for different phases of more commonly - inverted phase currents
+    // this should be automated later
+    float gain_a; //!< phase A gain
+    float gain_b; //!< phase B gain
+    float gain_c; //!< phase C gain
+
+    // // per phase low pass fileters
+    // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current A low pass filter
+    // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current B low pass filter
+    // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current C low pass filter
+
+  private:
+
+    // hardware variables
+  	int pinA; //!< pin A analog pin for current measurement
+  	int pinB; //!< pin B analog pin for current measurement
+  	int pinC; //!< pin C analog pin for current measurement
+
+    // gain variables
+    float shunt_resistor; //!< Shunt resistor value
+    float amp_gain; //!< amp gain value
+    float volts_to_amps_ratio; //!< Volts to amps ratio
+
+    /**
+     *  Function finding zero offsets of the ADC
+     */
+    void calibrateOffsets();
+    float offset_ia; //!< zero current A voltage value (center of the adc reading)
+    float offset_ib; //!< zero current B voltage value (center of the adc reading)
+    float offset_ic; //!< zero current C voltage value (center of the adc reading)
+
+};
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_api.h

@@ -0,0 +1,46 @@
+#ifndef HARDWARE_UTILS_CURRENT_H
+#define HARDWARE_UTILS_CURRENT_H
+
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+
+/**
+ *  function reading an ADC value and returning the read voltage
+ *
+ * @param pinA - the arduino pin to be read (it has to be ADC pin)
+ */
+float _readADCVoltageInline(const int pinA);
+
+/**
+ *  function reading an ADC value and returning the read voltage
+ *
+ * @param pinA - adc pin A
+ * @param pinB - adc pin B
+ * @param pinC - adc pin C
+ */
+void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET);
+
+/**
+ *  function reading an ADC value and returning the read voltage
+ *
+ * @param pinA - adc pin A
+ * @param pinB - adc pin B
+ * @param pinC - adc pin C
+ */
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET);
+
+void _startADC3PinConversionLowSide();
+
+/**
+ *  function reading an ADC value and returning the read voltage
+ *
+ * @param pinA - the arduino pin to be read (it has to be ADC pin)
+ */
+float _readADCVoltageLowSide(const int pinA);
+
+/**
+ *  function syncing the Driver with the ADC  for the LowSide Sensing
+ */
+void _driverSyncLowSide();
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/atmega_mcu.cpp

@@ -0,0 +1,55 @@
+#include "../hardware_api.h"
+
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) 
+
+#define _ADC_VOLTAGE 5.0f
+#define _ADC_RESOLUTION 1024.0f
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+#ifndef cbi
+  #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+#endif
+#ifndef sbi
+  #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+#endif
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+
+  // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default)  
+  // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz 
+  // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample 
+  cbi(ADCSRA, ADPS2);
+  sbi(ADCSRA, ADPS1);
+  sbi(ADCSRA, ADPS0);
+}
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pinA){
+  return _readADCVoltageInline(pinA);
+}
+// Configure low side for generic mcu
+// cannot do much but 
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/due_mcu.cpp

@@ -0,0 +1,38 @@
+#include "../hardware_api.h" 
+
+#if defined(__arm__) && defined(__SAM3X8E__) 
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 1024.0f
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pinA){
+  return _readADCVoltageInline(pinA);
+}
+// Configure low side for generic mcu
+// cannot do much but 
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/esp32_adc_driver.cpp

@@ -0,0 +1,225 @@
+#include "esp32_adc_driver.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) 
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "rom/ets_sys.h"
+#include "esp_attr.h"
+#include "esp_intr.h"
+#include "soc/rtc_io_reg.h"
+#include "soc/rtc_cntl_reg.h"
+#include "soc/sens_reg.h"
+
+static uint8_t __analogAttenuation = 3;//11db
+static uint8_t __analogWidth = 3;//12 bits
+static uint8_t __analogCycles = 8;
+static uint8_t __analogSamples = 0;//1 sample
+static uint8_t __analogClockDiv = 1;
+
+// Width of returned answer ()
+static uint8_t __analogReturnedWidth = 12;
+
+void __analogSetWidth(uint8_t bits){
+    if(bits < 9){
+        bits = 9;
+    } else if(bits > 12){
+        bits = 12;
+    }
+    __analogReturnedWidth = bits;
+    __analogWidth = bits - 9;
+    SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S);
+
+    SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S);
+}
+
+void __analogSetCycles(uint8_t cycles){
+    __analogCycles = cycles;
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S);
+}
+
+void __analogSetSamples(uint8_t samples){
+    if(!samples){
+        return;
+    }
+    __analogSamples = samples - 1;
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S);
+}
+
+void __analogSetClockDiv(uint8_t clockDiv){
+    if(!clockDiv){
+        return;
+    }
+    __analogClockDiv = clockDiv;
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S);
+}
+
+void __analogSetAttenuation(uint8_t attenuation)
+{
+    __analogAttenuation = attenuation & 3;
+    uint32_t att_data = 0;
+    int i = 10;
+    while(i--){
+        att_data |= __analogAttenuation << (i * 2);
+    }
+    WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels
+    WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data);
+}
+
+void IRAM_ATTR __analogInit(){
+    static bool initialized = false;
+    if(initialized){
+        return;
+    }
+
+    __analogSetAttenuation(__analogAttenuation);
+    __analogSetCycles(__analogCycles);
+    __analogSetSamples(__analogSamples + 1);//in samples
+    __analogSetClockDiv(__analogClockDiv);
+    __analogSetWidth(__analogWidth + 9);//in bits
+
+    SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV);
+    SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV);
+
+    SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW
+
+    CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM
+    SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0
+
+    CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S);  //clear FSM
+    SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S);
+    SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S);
+    SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S);
+    while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm==
+
+    initialized = true;
+}
+
+void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation)
+{
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0 || attenuation > 3){
+        return ;
+    }
+    __analogInit();
+    if(channel > 7){
+        SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2));
+    } else {
+        SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2));
+    }
+}
+
+bool IRAM_ATTR __adcAttachPin(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    int8_t pad = digitalPinToTouchChannel(pin);
+    if(pad >= 0){
+        uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG);
+        if(touch & (1 << pad)){
+            touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S))
+                    | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S))
+                    | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S)));
+            WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch);
+        }
+    } else if(pin == 25){
+        CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1
+    } else if(pin == 26){
+        CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2
+    }
+
+    pinMode(pin, ANALOG);
+
+    __analogInit();
+    return true;
+}
+
+bool IRAM_ATTR __adcStart(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    if(channel > 9){
+        channel -= 10;
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
+    } else {
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
+    }
+    return true;
+}
+
+bool IRAM_ATTR __adcBusy(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    if(channel > 7){
+        return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0);
+    }
+    return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0);
+}
+
+uint16_t IRAM_ATTR __adcEnd(uint8_t pin)
+{
+
+    uint16_t value = 0;
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return 0;//not adc pin
+    }
+    if(channel > 7){
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
+    } else {
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S);
+    }
+
+    // Shift result if necessary
+    uint8_t from = __analogWidth + 9;
+    if (from == __analogReturnedWidth) {
+        return value;
+    }
+    if (from > __analogReturnedWidth) {
+        return value >> (from - __analogReturnedWidth);
+    }
+    return value << (__analogReturnedWidth - from);
+}
+
+void __analogReadResolution(uint8_t bits)
+{
+    if(!bits || bits > 16){
+        return;
+    }
+    __analogSetWidth(bits);         // hadware from 9 to 12
+    __analogReturnedWidth = bits;   // software from 1 to 16
+}
+
+uint16_t IRAM_ATTR adcRead(uint8_t pin)
+{
+    if(!__adcAttachPin(pin) || !__adcStart(pin)){
+        return 0;
+    }
+    return __adcEnd(pin);
+}
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/esp32_adc_driver.h

@@ -0,0 +1,88 @@
+
+
+#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ 
+#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_
+
+#include "Arduino.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) 
+/*
+ * Get ADC value for pin
+ * */
+uint16_t adcRead(uint8_t pin);
+
+/*
+ * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096).
+ * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted.
+ * Range is 1 - 16
+ *
+ * Note: compatibility with Arduino SAM
+ */
+void __analogReadResolution(uint8_t bits);
+
+/*
+ * Sets the sample bits and read resolution
+ * Default is 12bit (0 - 4095)
+ * Range is 9 - 12
+ * */
+void __analogSetWidth(uint8_t bits);
+
+/*
+ * Set number of cycles per sample
+ * Default is 8 and seems to do well
+ * Range is 1 - 255
+ * */
+void __analogSetCycles(uint8_t cycles);
+
+/*
+ * Set number of samples in the range.
+ * Default is 1
+ * Range is 1 - 255
+ * This setting splits the range into
+ * "samples" pieces, which could look
+ * like the sensitivity has been multiplied
+ * that many times
+ * */
+void __analogSetSamples(uint8_t samples);
+
+/*
+ * Set the divider for the ADC clock.
+ * Default is 1
+ * Range is 1 - 255
+ * */
+void __analogSetClockDiv(uint8_t clockDiv);
+
+/*
+ * Set the attenuation for all channels
+ * Default is 11db
+ * */
+void __analogSetAttenuation(uint8_t attenuation);
+
+/*
+ * Set the attenuation for particular pin
+ * Default is 11db
+ * */
+void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation);
+
+/*
+ * Attach pin to ADC (will also clear any other analog mode that could be on)
+ * */
+bool __adcAttachPin(uint8_t pin);
+
+/*
+ * Start ADC conversion on attached pin's bus
+ * */
+bool __adcStart(uint8_t pin);
+
+/*
+ * Check if conversion on the pin's ADC bus is currently running
+ * */
+bool __adcBusy(uint8_t pin);
+
+/*
+ * Get the result of the conversion (will wait if it have not finished)
+ * */
+uint16_t __adcEnd(uint8_t pin);
+
+#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */
+#endif /* ESP32 */

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp

@@ -0,0 +1,30 @@
+#include "../hardware_api.h"
+#include "../../drivers/hardware_api.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) 
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 4095.0f
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+
+/**
+ *  Inline adc reading implementation 
+*/
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  // uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB, const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/esp32_mcu.cpp

@@ -0,0 +1,124 @@
+#include "../hardware_api.h"
+#include "../../drivers/hardware_api.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) 
+
+#include "esp32_adc_driver.h"
+
+#include "driver/mcpwm.h"
+#include "soc/mcpwm_reg.h"
+#include "soc/mcpwm_struct.h"
+
+#include <soc/sens_reg.h>
+#include <soc/sens_struct.h>
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 4095.0f
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+
+/**
+ *  Inline adc reading implementation 
+*/
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = adcRead(pinA);
+  // uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB, const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+/**
+ *  Low side adc reading implementation 
+*/
+static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1};
+int a1, a2, a3;         // Current readings from internal current sensor amplifiers
+int _pinA, _pinB, _pinC;
+static void IRAM_ATTR isr_handler(void*);
+byte currentState = 1;
+
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pin){
+  uint32_t raw_adc;
+
+  if (pin == _pinA) raw_adc = a1;
+  else if (pin == _pinB) raw_adc = a2;
+  else if (pin == _pinC) raw_adc = a3;
+
+  return raw_adc * _ADC_CONV;
+}
+
+// function reading an ADC value and returning the read voltage
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  _pinA = pinA;
+  _pinB = pinB;
+  _pinC = pinC;
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+void _driverSyncLowSide(){
+  // high side registers enable interrupt 
+  // MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
+  // MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tez_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt
+  // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tez_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt
+
+  // low-side register enable interrupt
+  MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
+  MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt
+  if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt
+  mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL);  //Set ISR Handler
+}
+
+// Read currents when interrupt is triggered
+static void IRAM_ATTR isr_handler(void*){
+  // // high side
+  // uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st;
+  // uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st;
+  // uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0;
+
+  // low side
+  uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st;
+  uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st;
+  uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st : 0;
+
+  switch (currentState)
+  {
+  case 1 :
+    if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA);
+    currentState = 2;
+    break;
+  case 2 :
+    if (mcpwm_intr_status_1 > 0) a2 = adcRead(_pinB);
+    currentState = _isset(_pinC) ?  3 : 1;
+    break;
+  case 3 :
+    if (mcpwm_intr_status_2 > 0) a3 = adcRead(_pinC);
+    currentState = 1;
+    break;
+  }
+
+  // high side
+  // MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0;
+  // MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1;
+  // if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2;
+  // low side
+  MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0;
+  MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1;
+  if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2;
+}
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/generic_mcu.cpp

@@ -0,0 +1,31 @@
+#include "../hardware_api.h"
+
+// function reading an ADC value and returning the read voltage
+__attribute__((weak))  float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * 5.0f/1024.0f;
+}
+
+// function reading an ADC value and returning the read voltage
+__attribute__((weak))  void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+// function reading an ADC value and returning the read voltage
+__attribute__((weak))  float _readADCVoltageLowSide(const int pinA){
+  return _readADCVoltageInline(pinA);
+}
+
+// Configure low side for generic mcu
+// cannot do much but 
+__attribute__((weak))  void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+// sync driver and the adc
+__attribute__((weak)) void _driverSyncLowSide(){ }
+__attribute__((weak)) void _startADC3PinConversionLowSide(){ }

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/samd21_mcu.cpp

@@ -0,0 +1,311 @@
+#ifdef _SAMD21_
+
+#include "samd21_mcu.h"
+#include "../hardware_api.h"
+
+
+static bool freeRunning = false;
+static int _pinA, _pinB, _pinC;
+static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode
+static SAMDCurrentSenseADCDMA instance;
+/**
+ *  function reading an ADC value and returning the read voltage 
+ * 
+ * @param pinA - adc pin A
+ * @param pinB - adc pin B
+ * @param pinC - adc pin C
+ */
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC)
+{
+  _pinA = pinA;
+  _pinB = pinB; 
+  _pinC = pinC;
+  freeRunning = true;
+  instance.init(pinA, pinB, pinC);
+
+}
+void _startADC3PinConversionLowSide()
+{
+  instance.startADCScan();
+}
+/**
+ *  function reading an ADC value and returning the read voltage
+ * 
+ * @param pinA - the arduino pin to be read (it has to be ADC pin)
+ */
+float _readADCVoltageLowSide(const int pinA)
+{
+  instance.readResults(a, b, c);
+  
+  if(pinA == _pinA)
+    return instance.toVolts(a);
+  if(pinA == _pinB)
+    return instance.toVolts(b);
+  if(pinA == _pinC)
+    return instance.toVolts(c);
+
+  return NAN;
+}
+
+/**
+ *  function syncing the Driver with the ADC  for the LowSide Sensing
+ */
+void _driverSyncLowSide()
+{
+  SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented"));
+  instance.startADCScan();
+  //TODO: hook with PWM interrupts
+}
+
+
+
+
+
+
+
+
+
+
+ // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless
+
+static void adcStopWithDMA(void);
+static void adcStartWithDMA(void);
+
+/**
+ * @brief  ADC sync wait 
+ * @retval void
+ */
+static __inline__ void ADCsync() __attribute__((always_inline, unused));
+static void   ADCsync() {
+  while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free
+}
+
+//  ADC DMA sequential free running (6) with Interrupts /////////////////
+
+SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() 
+{
+  
+  return &instance;
+}
+
+SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA()
+{
+}
+
+void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA)
+{
+  this->pinA = pinA;
+  this->pinB = pinB;
+  this->pinC = pinC;
+  this->pinAREF = pinAREF;
+  this->channelDMA = channelDMA;
+  this->voltageAREF = voltageAREF;
+  this->maxCountsADC = 1 << adcBits;
+  countsToVolts = ( voltageAREF / maxCountsADC );
+
+  initPins();
+  initADC();
+  initDMA();
+  startADCScan(); //so we have something to read next time we call readResults()
+}
+
+
+void SAMDCurrentSenseADCDMA::startADCScan(){
+  adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize);
+  adcStartWithDMA();
+}
+
+bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){
+  if(ADC->CTRLA.bit.ENABLE)
+    return false;
+  uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber;
+  uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber;
+  a = adcBuffer[ainA];
+  b = adcBuffer[ainB];
+  if(_isset(pinC))
+  {
+    uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber;
+    c = adcBuffer[ainC];
+  }
+  return true;
+}
+
+
+float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) {
+  return counts * countsToVolts;
+}
+
+void SAMDCurrentSenseADCDMA::initPins(){
+  
+  if (pinAREF>=0)
+    pinMode(pinAREF, INPUT);
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+
+  uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber;
+  uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber;
+  firstAIN = min(ainA, ainB);
+  lastAIN = max(ainA, ainB);
+  if( _isset(pinC) ) 
+  {
+    uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber;
+    pinMode(pinC, INPUT);
+    firstAIN = min(firstAIN, ainC);
+    lastAIN = max(lastAIN, ainC);
+  }
+
+  oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout
+  BufferSize = lastAIN - oneBeforeFirstAIN + 1;
+
+}
+
+void SAMDCurrentSenseADCDMA::initADC(){
+
+  analogRead(pinA);  // do some pin init  pinPeripheral() 
+  analogRead(pinB);  // do some pin init  pinPeripheral() 
+  analogRead(pinC);  // do some pin init  pinPeripheral() 
+
+  ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC
+  ADCsync();
+  //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; //  2.2297 V Supply VDDANA
+  ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X
+  // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val;  // default
+  if (pinAREF>=0)
+    ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA;
+  else
+    ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0;
+  ADCsync();  //  ref 31.6.16
+
+  /*
+  Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan
+  This register gives the number of input sources included in the pin scan. The number of input sources included is
+  INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS +
+  INPUTOFFSET + INPUTSCAN.
+  The range of the scan mode must not exceed the number of input channels available on the device.
+  Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection
+  These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If
+  the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit
+  group in the SamplingControl register must be written.
+  Table 32-14. Positive Mux Input Selection
+  MUXPOS[4:0] Group configuration Description
+  0x00 PIN0 ADC AIN0 pin
+  0x01 PIN1 ADC AIN1 pin
+  0x02 PIN2 ADC AIN2 pin
+  0x03 PIN3 ADC AIN3 pin
+  0x04 PIN4 ADC AIN4 pin
+  0x05 PIN5 ADC AIN5 pin
+  0x06 PIN6 ADC AIN6 pin
+  0x07 PIN7 ADC AIN7 pin
+  0x08 PIN8 ADC AIN8 pin
+  0x09 PIN9 ADC AIN9 pin
+  0x0A PIN10 ADC AIN10 pin
+  0x0B PIN11 ADC AIN11 pin
+  0x0C PIN12 ADC AIN12 pin
+  0x0D PIN13 ADC AIN13 pin
+  0x0E PIN14 ADC AIN14 pin
+  0x0F PIN15 ADC AIN15 pin
+  0x10 PIN16 ADC AIN16 pin
+  0x11 PIN17 ADC AIN17 pin
+  0x12 PIN18 ADC AIN18 pin
+  0x13 PIN19 ADC AIN19 pin
+  0x14-0x17 Reserved
+  0x18 TEMP Temperature reference
+  0x19 BANDGAP Bandgap voltage
+  0x1A SCALEDCOREVCC 1/4 scaled core supply
+  0x1B SCALEDIOVCC 1/4 scaled I/O supply
+  0x1C DAC DAC output
+  0x1D-0x1F Reserved
+  */
+  ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN;
+  ADCsync();
+  ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) 
+  ADCsync();
+  ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor
+  ADCsync();
+  ADC->AVGCTRL.reg = 0x00 ;  //no averaging
+  ADC->SAMPCTRL.reg = 0x05;  ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16
+  // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU
+  ADCsync();
+  ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT;
+  ADCsync();
+}
+
+volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16)));
+
+void SAMDCurrentSenseADCDMA::initDMA() {
+  // probably on by default
+  PM->AHBMASK.reg |= PM_AHBMASK_DMAC ;
+  PM->APBBMASK.reg |= PM_APBBMASK_DMAC ;
+  NVIC_EnableIRQ( DMAC_IRQn ) ;
+  DMAC->BASEADDR.reg = (uint32_t)descriptor_section;
+  DMAC->WRBADDR.reg = (uint32_t)wrb;
+  DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf);
+}
+
+
+void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata,  uint32_t hwords) {
+
+  DMAC->CHID.reg = DMAC_CHID_ID(channelDMA);
+  DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE;
+  DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST;
+  DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA));
+  
+  DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) 
+  | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) 
+  | DMAC_CHCTRLB_TRIGACT_BEAT;
+  DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts
+  descriptor.descaddr = 0;
+  descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg;
+  descriptor.btcnt =  hwords;
+  descriptor.dstaddr = (uint32_t)rxdata + hwords*2;   // end address
+  descriptor.btctrl =  DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID;
+  memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor));
+
+  // start channel
+  DMAC->CHID.reg = DMAC_CHID_ID(channelDMA);
+  DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
+}
+
+
+int iii = 0;
+
+void adcStopWithDMA(void){
+  ADCsync();
+  ADC->CTRLA.bit.ENABLE = 0x00;
+  // ADCsync();
+  // if(iii++ % 1000 == 0)
+  // {
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a);
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: ");
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b);
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: ");
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c);
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!");
+  // }
+
+
+}
+
+void adcStartWithDMA(void){
+  ADCsync();
+  ADC->INPUTCTRL.bit.INPUTOFFSET = 0;
+  ADCsync();
+  ADC->SWTRIG.bit.FLUSH = 1;
+  ADCsync();
+  ADC->CTRLA.bit.ENABLE = 0x01; 
+  ADCsync();
+}
+
+void DMAC_Handler() {
+  uint8_t active_channel;
+  active_channel =  DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number
+  DMAC->CHID.reg = DMAC_CHID_ID(active_channel);
+  adcStopWithDMA();
+  DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear
+  DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR;
+  DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP;
+
+}
+
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/samd21_mcu.h

@@ -0,0 +1,69 @@
+#ifdef _SAMD21_
+
+#ifndef CURRENT_SENSE_SAMD21_H
+#define CURRENT_SENSE_SAMD21_H
+
+#define SIMPLEFOC_SAMD_DEBUG
+#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL)
+#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial
+#endif
+
+#include <stdint.h>
+  typedef struct {
+    uint16_t btctrl;
+    uint16_t btcnt;
+    uint32_t srcaddr;
+    uint32_t dstaddr;
+    uint32_t descaddr;
+  } dmacdescriptor ;
+
+
+// AREF pin is 42
+
+class SAMDCurrentSenseADCDMA
+{
+
+public:
+  static SAMDCurrentSenseADCDMA * getHardwareAPIInstance();
+  SAMDCurrentSenseADCDMA();
+  void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3);
+  void startADCScan();
+  bool readResults(uint16_t & a, uint16_t & b, uint16_t & c);
+  float toVolts(uint16_t counts);
+private:
+
+  void adcToDMATransfer(void *rxdata,  uint32_t hwords);
+
+  void initPins();
+  void initADC();
+  void initDMA();
+ 
+  uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout
+  uint32_t firstAIN;
+  uint32_t lastAIN; 
+  uint32_t BufferSize = 0;
+
+  uint16_t adcBuffer[20];
+
+
+  uint32_t pinA;
+  uint32_t pinB;
+  uint32_t pinC;
+  uint32_t pinAREF;
+  uint32_t channelDMA;  // DMA channel
+  bool freeRunning;
+
+  float voltageAREF;
+  float maxCountsADC;
+  float countsToVolts;
+  
+  dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16)));
+  dmacdescriptor descriptor __attribute__ ((aligned (16)));
+
+};
+
+#endif
+
+
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/samd_mcu.cpp

@@ -0,0 +1,37 @@
+#include "../hardware_api.h" 
+
+#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 1024.0f
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pinA){
+  return _readADCVoltageInline(pinA);
+}
+// Configure low side for generic mcu
+// cannot do much but 
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/stm32_mcu.cpp

@@ -0,0 +1,39 @@
+
+#include "../hardware_api.h"
+
+#if defined(_STM32_DEF_) and !defined(STM32G4xx)
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 1024.0f
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pinA){
+  return _readADCVoltageInline(pinA);
+}
+// Configure low side for generic mcu
+// cannot do much but 
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/stm32g4_hal.cpp

@@ -0,0 +1,333 @@
+#include "../hardware_api.h"
+#if defined(STM32G4xx)
+
+#include "stm32g4xx_hal.h"
+#include "stm32g4xx_ll_pwr.h"
+#include "stm32g4xx_hal_adc.h"
+#include "stm32g4_hal.h"
+// From STM32 cube IDE
+/**
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
+  * All rights reserved.</center></h2>
+  *
+  * This software component is licensed by ST under BSD 3-Clause license,
+  * the "License"; You may not use this file except in compliance with the
+  * License. You may obtain a copy of the License at:
+  *                        opensource.org/licenses/BSD-3-Clause
+  *
+  ******************************************************************************
+  */
+
+
+/**
+  * @brief GPIO Initialization Function
+  * @param None
+  * @retval None
+  */
+void MX_GPIO_Init(void)
+{
+  /* GPIO Ports Clock Enable */
+  __HAL_RCC_GPIOC_CLK_ENABLE();
+  __HAL_RCC_GPIOF_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+
+  __HAL_RCC_ADC12_CLK_ENABLE();
+}
+
+/** 
+  * Enable DMA controller clock
+  */
+void  MX_DMA_Init(void) 
+{
+  /* DMA controller clock enable */
+  __HAL_RCC_DMAMUX1_CLK_ENABLE();
+  __HAL_RCC_DMA1_CLK_ENABLE();
+  __HAL_RCC_DMA2_CLK_ENABLE();
+
+  /* DMA interrupt init */
+  /* DMA1_Channel1_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
+  /* DMA1_Channel2_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
+
+  // Enable external clock for ADC
+  RCC_PeriphCLKInitTypeDef PeriphClkInit;
+  PeriphClkInit.PeriphClockSelection   = RCC_PERIPHCLK_ADC12;
+  PeriphClkInit.Adc12ClockSelection    = RCC_ADC12CLKSOURCE_PLL;
+  HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
+}
+
+
+/**
+  * @brief ADC1 Initialization Function
+  * @param None
+  * @retval None
+  */
+void  MX_ADC1_Init(ADC_HandleTypeDef* hadc1)
+{
+  /* USER CODE BEGIN ADC1_Init 0 */
+
+  /* USER CODE END ADC1_Init 0 */
+
+  ADC_MultiModeTypeDef multimode = {0};
+  ADC_ChannelConfTypeDef sConfig = {0};
+
+  /* USER CODE BEGIN ADC1_Init 1 */
+
+  /* USER CODE END ADC1_Init 1 */
+  /** Common config 
+  */
+  hadc1->Instance = ADC1;
+  hadc1->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16;
+  hadc1->Init.Resolution = ADC_RESOLUTION_12B;
+  hadc1->Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc1->Init.GainCompensation = 0;
+  hadc1->Init.ScanConvMode = ADC_SCAN_ENABLE;
+  hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  hadc1->Init.LowPowerAutoWait = DISABLE;
+  hadc1->Init.ContinuousConvMode = DISABLE;
+  hadc1->Init.NbrOfConversion = 2;
+  hadc1->Init.DiscontinuousConvMode = DISABLE;
+  hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO;
+  hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
+  hadc1->Init.DMAContinuousRequests = ENABLE;
+  hadc1->Init.Overrun = ADC_OVR_DATA_PRESERVED;
+
+  if (HAL_ADC_Init(hadc1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  /** Configure the ADC multi-mode 
+  */
+  multimode.Mode = ADC_MODE_INDEPENDENT;
+  if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /** Configure Regular Channel 
+  */
+  sConfig.Channel = ADC_CHANNEL_12;     // ADC1_IN12 = PB1 = OP3_OUT
+  sConfig.Rank = ADC_REGULAR_RANK_1;
+  sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
+  sConfig.SingleDiff = ADC_SINGLE_ENDED;
+  sConfig.OffsetNumber = ADC_OFFSET_NONE;
+  sConfig.Offset = 0;
+  if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /** Configure Regular Channel 
+  */
+  sConfig.Channel = ADC_CHANNEL_3;  // ADC1_IN3 = PA2 = OP1_OUT
+  sConfig.Rank = ADC_REGULAR_RANK_2;
+  if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN ADC1_Init 2 */
+
+  /* USER CODE END ADC1_Init 2 */
+
+}
+
+/**
+  * @brief ADC2 Initialization Function
+  * @param None
+  * @retval None
+  */
+void MX_ADC2_Init(ADC_HandleTypeDef* hadc2)
+{
+  /* USER CODE BEGIN ADC2_Init 0 */
+
+  /* USER CODE END ADC2_Init 0 */
+
+  ADC_ChannelConfTypeDef sConfig = {0};
+
+  /* USER CODE BEGIN ADC2_Init 1 */
+
+  /* USER CODE END ADC2_Init 1 */
+  /** Common config 
+  */
+  hadc2->Instance = ADC2;
+  hadc2->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16;
+  hadc2->Init.Resolution = ADC_RESOLUTION_12B;
+  hadc2->Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc2->Init.GainCompensation = 0;
+  hadc2->Init.ScanConvMode = ADC_SCAN_ENABLE;
+  hadc2->Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  hadc2->Init.LowPowerAutoWait = DISABLE;
+  hadc2->Init.ContinuousConvMode = DISABLE;
+  hadc2->Init.NbrOfConversion = 1;
+  hadc2->Init.DiscontinuousConvMode = DISABLE;
+  hadc2->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO;
+  hadc2->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
+  hadc2->Init.DMAContinuousRequests = ENABLE;
+  hadc2->Init.Overrun = ADC_OVR_DATA_PRESERVED;
+
+  if (HAL_ADC_Init(hadc2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /** Configure Regular Channel 
+  */
+  sConfig.Channel = ADC_CHANNEL_3;  // ADC2_IN3 = PA6
+  sConfig.Rank = ADC_REGULAR_RANK_1;
+  sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
+  sConfig.SingleDiff = ADC_SINGLE_ENDED;
+  sConfig.OffsetNumber = ADC_OFFSET_NONE;
+  sConfig.Offset = 0;
+  if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN ADC2_Init 2 */
+
+  /* USER CODE END ADC2_Init 2 */
+
+}
+
+/**
+* @brief OPAMP MSP Initialization
+* This function configures the hardware resources used in this example
+* @param hopamp-> OPAMP handle pointer
+* @retval None
+*/
+void HAL_OPAMP_MspInit(OPAMP_HandleTypeDef* hopamp)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(hopamp->Instance==OPAMP1)
+  {
+  /* USER CODE BEGIN OPAMP1_MspInit 0 */
+
+  /* USER CODE END OPAMP1_MspInit 0 */
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**OPAMP1 GPIO Configuration    
+    PA1     ------> OPAMP1_VINP
+    PA2     ------> OPAMP1_VOUT
+    PA3     ------> OPAMP1_VINM 
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN OPAMP1_MspInit 1 */
+
+  /* USER CODE END OPAMP1_MspInit 1 */
+  }
+  else if(hopamp->Instance==OPAMP2)
+  {
+  /* USER CODE BEGIN OPAMP2_MspInit 0 */
+
+  /* USER CODE END OPAMP2_MspInit 0 */
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**OPAMP2 GPIO Configuration    
+    PA5     ------> OPAMP2_VINM
+    PA6     ------> OPAMP2_VOUT
+    PA7     ------> OPAMP2_VINP 
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN OPAMP2_MspInit 1 */
+
+  /* USER CODE END OPAMP2_MspInit 1 */
+  }
+  else if(hopamp->Instance==OPAMP3)
+  {
+  /* USER CODE BEGIN OPAMP3_MspInit 0 */
+
+  /* USER CODE END OPAMP3_MspInit 0 */
+
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    /**OPAMP3 GPIO Configuration    
+    PB0     ------> OPAMP3_VINP
+    PB1     ------> OPAMP3_VOUT
+    PB2     ------> OPAMP3_VINM 
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN OPAMP3_MspInit 1 */
+
+  /* USER CODE END OPAMP3_MspInit 1 */
+  }
+
+}
+
+/**
+* @brief OPAMP MSP De-Initialization
+* This function freeze the hardware resources used in this example
+* @param hopamp-> OPAMP handle pointer
+* @retval None
+*/
+void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* hopamp)
+{
+  if(hopamp->Instance==OPAMP1)
+  {
+  /* USER CODE BEGIN OPAMP1_MspDeInit 0 */
+
+  /* USER CODE END OPAMP1_MspDeInit 0 */
+
+    /**OPAMP1 GPIO Configuration    
+    PA1     ------> OPAMP1_VINP
+    PA2     ------> OPAMP1_VOUT
+    PA3     ------> OPAMP1_VINM 
+    */
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3);
+
+  /* USER CODE BEGIN OPAMP1_MspDeInit 1 */
+
+  /* USER CODE END OPAMP1_MspDeInit 1 */
+  }
+  else if(hopamp->Instance==OPAMP2)
+  {
+  /* USER CODE BEGIN OPAMP2_MspDeInit 0 */
+
+  /* USER CODE END OPAMP2_MspDeInit 0 */
+
+    /**OPAMP2 GPIO Configuration    
+    PA5     ------> OPAMP2_VINM
+    PA6     ------> OPAMP2_VOUT
+    PA7     ------> OPAMP2_VINP 
+    */
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7);
+
+  /* USER CODE BEGIN OPAMP2_MspDeInit 1 */
+
+  /* USER CODE END OPAMP2_MspDeInit 1 */
+  }
+  else if(hopamp->Instance==OPAMP3)
+  {
+  /* USER CODE BEGIN OPAMP3_MspDeInit 0 */
+
+  /* USER CODE END OPAMP3_MspDeInit 0 */
+
+    /**OPAMP3 GPIO Configuration    
+    PB0     ------> OPAMP3_VINP
+    PB1     ------> OPAMP3_VOUT
+    PB2     ------> OPAMP3_VINM 
+    */
+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2);
+
+  /* USER CODE BEGIN OPAMP3_MspDeInit 1 */
+
+  /* USER CODE END OPAMP3_MspDeInit 1 */
+  }
+
+}
+
+#endif 

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/stm32g4_hal.h

@@ -0,0 +1,18 @@
+#ifndef stm32g4_hal
+#define stm32g4_hal
+
+#if defined(STM32G4xx)
+
+#include <stm32g4xx_hal_adc.h>
+#include <stm32g4xx_hal_opamp.h>
+
+void MX_GPIO_Init(void);
+void MX_DMA_Init(void);
+void MX_ADC1_Init(ADC_HandleTypeDef* hadc1);
+void MX_ADC2_Init(ADC_HandleTypeDef* hadc2);
+void MX_OPAMP1_Init(OPAMP_HandleTypeDef* hopamp);
+void MX_OPAMP2_Init(OPAMP_HandleTypeDef* hopamp);
+void MX_OPAMP3_Init(OPAMP_HandleTypeDef* hopamp);
+#endif
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/stm32g4_mcu.cpp

@@ -0,0 +1,130 @@
+#include "../hardware_api.h"
+#include "stm32g4_hal.h"
+
+#if defined(STM32G4xx) 
+#define _ADC_VOLTAGE 3.3
+#define _ADC_RESOLUTION 4096.0
+#define ADC_BUF_LEN_1 2
+#define ADC_BUF_LEN_2 1
+
+static ADC_HandleTypeDef hadc1;
+static ADC_HandleTypeDef hadc2;
+static OPAMP_HandleTypeDef hopamp1;
+static OPAMP_HandleTypeDef hopamp2;
+static OPAMP_HandleTypeDef hopamp3;
+
+static DMA_HandleTypeDef hdma_adc1;
+static DMA_HandleTypeDef hdma_adc2;
+
+uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion
+uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion
+
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+// As DMA is being used just return the DMA result
+float _readADCVoltageInline(const int pin){
+  uint32_t raw_adc = 0;
+  if(pin == PA2)  // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1
+    raw_adc = adcBuffer1[1];
+  else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1
+    raw_adc = adcBuffer2[0];
+  else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1
+    raw_adc = adcBuffer1[0];
+
+  return raw_adc * _ADC_CONV;
+}
+// do the same for low side sensing
+float _readADCVoltageLowSide(const int pin){
+  return _readADCVoltageInline(pin);
+}
+
+void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){
+  // could this be replaced with LL_OPAMP calls??
+  hopamp->Instance = OPAMPx_Def;
+  hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED;
+  hopamp->Init.Mode = OPAMP_PGA_MODE;
+  hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0;
+  hopamp->Init.InternalOutput = DISABLE;
+  hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
+  hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS;
+  hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15;
+  hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
+  if (HAL_OPAMP_Init(hopamp) != HAL_OK)
+  {
+    Error_Handler();
+  }
+}
+void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){
+  // Configure the opamps
+  _configureOPAMP(OPAMPA, OPAMP1);
+  _configureOPAMP(OPAMPB, OPAMP2);
+  _configureOPAMP(OPAMPC, OPAMP3);
+}
+
+void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request) {
+  hdma_adc->Instance = channel;
+  hdma_adc->Init.Request = request;
+  hdma_adc->Init.Direction = DMA_PERIPH_TO_MEMORY;
+  hdma_adc->Init.PeriphInc = DMA_PINC_DISABLE;
+  hdma_adc->Init.MemInc = DMA_MINC_ENABLE;
+  hdma_adc->Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+  hdma_adc->Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+  hdma_adc->Init.Mode = DMA_CIRCULAR;
+  hdma_adc->Init.Priority = DMA_PRIORITY_LOW;
+  HAL_DMA_DeInit(hdma_adc);
+  if (HAL_DMA_Init(hdma_adc) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc);
+}
+
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  HAL_Init();
+  MX_GPIO_Init();
+  MX_DMA_Init(); 
+  _configureOPAMPs(&hopamp1, &hopamp3, &hopamp2);
+  MX_ADC1_Init(&hadc1);
+  MX_ADC2_Init(&hadc2);
+
+  MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1);
+  MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2);
+
+  if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  HAL_OPAMP_Start(&hopamp1);
+  HAL_OPAMP_Start(&hopamp2);
+  HAL_OPAMP_Start(&hopamp3); 
+
+  // Check if the ADC DMA is collecting any data.
+  // If this fails, it likely means timer1 has not started. Verify that your application starts
+  // the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine.
+  _delay(5);
+  if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) {
+    Error_Handler();
+  }
+}
+// do the same for low side
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  _configureADCInline(pinA, pinB, pinC);
+}
+
+extern "C" {
+void DMA1_Channel1_IRQHandler(void) {
+  HAL_DMA_IRQHandler(&hdma_adc1);
+}
+
+void DMA1_Channel2_IRQHandler(void) {
+   HAL_DMA_IRQHandler(&hdma_adc2);
+}
+}
+
+#endif

v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/teensy_mcu.cpp

@@ -0,0 +1,37 @@
+#include "../hardware_api.h" 
+
+#if defined(__arm__) && defined(CORE_TEENSY)
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 1024.0f
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pinA){
+  return _readADCVoltageInline(pinA);
+}
+// Configure low side for generic mcu
+// cannot do much but 
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+#endif

v1.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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.3/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

v1.3/lib/Simple_FOC/src/sensors/Encoder.cpp

@@ -0,0 +1,230 @@
+#include "Encoder.h"
+
+
+/*
+  Encoder(int encA, int encB , int cpr, int index)
+  - encA, encB    - encoder A and B pins
+  - cpr           - counts per rotation number (cpm=ppm*4)
+  - index pin     - (optional input)
+*/
+
+Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){
+
+  // Encoder measurement structure init
+  // hardware pins
+  pinA = _encA;
+  pinB = _encB;
+  // counter setup
+  pulse_counter = 0;
+  pulse_timestamp = 0;
+
+  cpr = _ppr;
+  A_active = 0;
+  B_active = 0;
+  I_active = 0;
+  // index pin
+  index_pin = _index; // its 0 if not used
+
+  // velocity calculation variables
+  prev_Th = 0;
+  pulse_per_second = 0;
+  prev_pulse_counter = 0;
+  prev_timestamp_us = _micros();
+
+  // extern pullup as default
+  pullup = Pullup::USE_EXTERN;
+  // enable quadrature encoder by default
+  quadrature = Quadrature::ON;
+}
+
+//  Encoder interrupt callback functions
+// A channel
+void Encoder::handleA() {
+  bool A = digitalRead(pinA);
+  switch (quadrature){
+    case Quadrature::ON:
+      // CPR = 4xPPR
+      if ( A != A_active ) {
+        pulse_counter += (A_active == B_active) ? 1 : -1;
+        pulse_timestamp = _micros();
+        A_active = A;
+      }
+      break;
+    case Quadrature::OFF:
+      // CPR = PPR
+      if(A && !digitalRead(pinB)){
+        pulse_counter++;
+        pulse_timestamp = _micros();
+      }
+      break;
+  }
+}
+// B channel
+void Encoder::handleB() {
+  bool B = digitalRead(pinB);
+  switch (quadrature){
+    case Quadrature::ON:
+  //     // CPR = 4xPPR
+      if ( B != B_active ) {
+        pulse_counter += (A_active != B_active) ? 1 : -1;
+        pulse_timestamp = _micros();
+        B_active = B;
+      }
+      break;
+    case Quadrature::OFF:
+      // CPR = PPR
+      if(B && !digitalRead(pinA)){
+        pulse_counter--;
+        pulse_timestamp = _micros();
+      }
+      break;
+  }
+}
+
+// Index channel
+void Encoder::handleIndex() {
+  if(hasIndex()){
+    bool I = digitalRead(index_pin);
+    if(I && !I_active){
+      index_found = true;
+      // align encoder on each index
+      long tmp = pulse_counter;
+      // corrent the counter value
+      pulse_counter = round((double)pulse_counter/(double)cpr)*cpr;
+      // preserve relative speed
+      prev_pulse_counter += pulse_counter - tmp;
+    }
+    I_active = I;
+  }
+}
+
+
+void Encoder::update() {
+    // do nothing for Encoder
+}
+
+/*
+	Shaft angle calculation
+*/
+float Encoder::getSensorAngle(){
+  return getAngle();
+}
+// TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost
+float Encoder::getMechanicalAngle(){
+  return  _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr);
+}
+
+float Encoder::getAngle(){
+  return  _2PI * (pulse_counter) / ((float)cpr);
+}
+double Encoder::getPreciseAngle(){
+  return  _2PI * (pulse_counter) / ((double)cpr);
+}
+int32_t Encoder::getFullRotations(){
+  return  pulse_counter / (int)cpr;
+}
+
+
+
+/*
+  Shaft velocity calculation
+  function using mixed time and frequency measurement technique
+*/
+float Encoder::getVelocity(){
+  // timestamp
+  long timestamp_us = _micros();
+  // sampling time calculation
+  float Ts = (timestamp_us - prev_timestamp_us) * 1e-6f;
+  // quick fix for strange cases (micros overflow)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // time from last impulse
+  float Th = (timestamp_us - pulse_timestamp) * 1e-6f;
+  long dN = pulse_counter - prev_pulse_counter;
+
+  // Pulse per second calculation (Eq.3.)
+  // dN - impulses received
+  // Ts - sampling time - time in between function calls
+  // Th - time from last impulse
+  // Th_1 - time form last impulse of the previous call
+  // only increment if some impulses received
+  float dt = Ts + prev_Th - Th;
+  pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second;
+
+  // if more than 0.05f passed in between impulses
+  if ( Th > 0.1f) pulse_per_second = 0;
+
+  // velocity calculation
+  float velocity = pulse_per_second / ((float)cpr) * (_2PI);
+
+  // save variables for next pass
+  prev_timestamp_us = timestamp_us;
+  // save velocity calculation variables
+  prev_Th = Th;
+  prev_pulse_counter = pulse_counter;
+  return velocity;
+}
+
+// getter for index pin
+// return -1 if no index
+int Encoder::needsSearch(){
+  return hasIndex() && !index_found;
+}
+
+// private function used to determine if encoder has index
+int Encoder::hasIndex(){
+  return index_pin != 0;
+}
+
+
+// encoder initialisation of the hardware pins
+// and calculation variables
+void Encoder::init(){
+
+  // Encoder - check if pullup needed for your encoder
+  if(pullup == Pullup::USE_INTERN){
+    pinMode(pinA, INPUT_PULLUP);
+    pinMode(pinB, INPUT_PULLUP);
+    if(hasIndex()) pinMode(index_pin,INPUT_PULLUP);
+  }else{
+    pinMode(pinA, INPUT);
+    pinMode(pinB, INPUT);
+    if(hasIndex()) pinMode(index_pin,INPUT);
+  }
+
+  // counter setup
+  pulse_counter = 0;
+  pulse_timestamp = _micros();
+  // velocity calculation variables
+  prev_Th = 0;
+  pulse_per_second = 0;
+  prev_pulse_counter = 0;
+  prev_timestamp_us = _micros();
+
+  // initial cpr = PPR
+  // change it if the mode is quadrature
+  if(quadrature == Quadrature::ON) cpr = 4*cpr;
+
+  // we don't call Sensor::init() here because init is handled in Encoder class.
+}
+
+// function enabling hardware interrupts of the for the callback provided
+// if callback is not provided then the interrupt is not enabled
+void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){
+  // attach interrupt if functions provided
+  switch(quadrature){
+    case Quadrature::ON:
+      // A callback and B callback
+      if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE);
+      if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
+      break;
+    case Quadrature::OFF:
+      // A callback and B callback
+      if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING);
+      if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING);
+      break;
+  }
+
+  // if index used initialize the index interrupt
+  if(hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE);
+}

v1.3/lib/Simple_FOC/src/sensors/Encoder.h

@@ -0,0 +1,95 @@
+#ifndef ENCODER_LIB_H
+#define ENCODER_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/base_classes/Sensor.h"
+
+
+/**
+ *  Quadrature mode configuration structure
+ */
+enum Quadrature : uint8_t {
+  ON    = 0x00, //!<  Enable quadrature mode CPR = 4xPPR
+  OFF   = 0x01  //!<  Disable quadrature mode / CPR = PPR
+};
+
+class Encoder: public Sensor{
+ public:
+    /**
+    Encoder class constructor
+    @param encA  encoder B pin
+    @param encB  encoder B pin
+    @param ppr  impulses per rotation  (cpr=ppr*4)
+    @param index index pin number (optional input)
+    */
+    Encoder(int encA, int encB , float ppr, int index = 0);
+
+    /** encoder initialise pins */
+    void init() override;
+    /**
+     *  function enabling hardware interrupts for the encoder channels with provided callback functions
+     *  if callback is not provided then the interrupt is not enabled
+     * 
+     * @param doA pointer to the A channel interrupt handler function
+     * @param doB pointer to the B channel interrupt handler function
+     * @param doIndex pointer to the Index channel interrupt handler function
+     * 
+     */
+    void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doIndex)() = nullptr);
+    
+    //  Encoder interrupt callback functions
+    /** A channel callback function */
+    void handleA();
+    /** B channel callback function */
+    void handleB();
+    /** Index channel callback function */
+    void handleIndex();
+    
+    
+    // pins A and B
+    int pinA; //!< encoder hardware pin A
+    int pinB; //!< encoder hardware pin B
+    int index_pin; //!< index pin
+
+    // Encoder configuration
+    Pullup pullup; //!< Configuration parameter internal or external pullups
+    Quadrature quadrature;//!< Configuration parameter enable or disable quadrature mode
+    float cpr;//!< encoder cpr number
+
+    // Abstract functions of the Sensor class implementation
+    /** get current angle (rad) */
+    float getSensorAngle() override;
+    float getMechanicalAngle() override;
+    /**  get current angular velocity (rad/s) */
+    float getVelocity() override;
+    float getAngle() override;
+    double getPreciseAngle() override;
+    int32_t getFullRotations() override;
+    virtual void update() override;
+
+    /**
+     * returns 0 if it does need search for absolute zero
+     * 0 - encoder without index 
+     * 1 - ecoder with index
+     */
+    int needsSearch() override;
+
+  private:
+    int hasIndex(); //!< function returning 1 if encoder has index pin and 0 if not.
+
+    volatile long pulse_counter;//!< current pulse counter
+    volatile long pulse_timestamp;//!< last impulse timestamp in us
+    volatile int A_active; //!< current active states of A channel
+    volatile int B_active; //!< current active states of B channel
+    volatile int I_active; //!< current active states of Index channel
+    volatile bool index_found = false; //!< flag stating that the index has been found
+
+    // velocity calculation variables
+    float prev_Th, pulse_per_second;
+    volatile long prev_pulse_counter, prev_timestamp_us;
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/sensors/GenericSensor.cpp

@@ -0,0 +1,26 @@
+#include "GenericSensor.h"
+
+
+/*
+  GenericSensor( float (*readCallback)() )
+  - readCallback - pointer to the function which reads the sensor angle.
+*/
+
+GenericSensor::GenericSensor(float (*readCallback)(), void (*initCallback)()){
+  // if function provided add it to the 
+  if(readCallback != nullptr) this->readCallback = readCallback;
+  if(initCallback != nullptr) this->initCallback = initCallback;
+}
+
+void GenericSensor::init(){
+  // if init callback specified run it
+  if(initCallback != nullptr) this->initCallback();
+  this->Sensor::init(); // call base class init
+}
+
+/*
+	Shaft angle calculation
+*/
+float GenericSensor::getSensorAngle(){
+  return this->readCallback();
+}

v1.3/lib/Simple_FOC/src/sensors/GenericSensor.h

@@ -0,0 +1,31 @@
+#ifndef GENERIC_SENSOR_LIB_H
+#define GENERIC_SENSOR_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/base_classes/Sensor.h"
+
+
+class GenericSensor: public Sensor{
+ public:
+    /**
+    GenericSensor class constructor
+     * @param readCallback pointer to the function reading the sensor angle
+     * @param initCallback pointer to the function initialising the sensor
+    */
+    GenericSensor(float (*readCallback)() = nullptr, void (*initCallback)() = nullptr);
+  
+    float (*readCallback)() = nullptr; //!< function pointer to sensor reading
+    void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation
+
+    void init() override;
+
+    // Abstract functions of the Sensor class implementation
+    /** get current angle (rad) */
+    float getSensorAngle() override;
+
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/sensors/HallSensor.cpp

@@ -0,0 +1,181 @@
+#include "HallSensor.h"
+
+
+/*
+  HallSensor(int hallA, int hallB , int cpr, int index)
+  - hallA, hallB, hallC    - HallSensor A, B and C pins
+  - pp           - pole pairs
+*/
+HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){
+
+  // hardware pins
+  pinA = _hallA;
+  pinB = _hallB;
+  pinC = _hallC;
+
+  // hall has 6 segments per electrical revolution
+  cpr = _pp * 6;
+
+  // extern pullup as default
+  pullup = Pullup::USE_EXTERN;
+}
+
+//  HallSensor interrupt callback functions
+// A channel
+void HallSensor::handleA() {
+  A_active= digitalRead(pinA);
+  updateState();
+}
+// B channel
+void HallSensor::handleB() {
+  B_active = digitalRead(pinB);
+  updateState();
+}
+
+// C channel
+void HallSensor::handleC() {
+  C_active = digitalRead(pinC);
+  updateState();
+}
+
+/**
+ * Updates the state and sector following an interrupt
+ */
+void HallSensor::updateState() {
+  long new_pulse_timestamp = _micros();
+
+  int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2);
+
+  // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed
+  if (new_hall_state == hall_state) {
+    return;
+  }
+  hall_state = new_hall_state;
+
+  int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state];
+  static Direction old_direction;
+  if (new_electric_sector - electric_sector > 3) {
+    //underflow
+    direction = Direction::CCW;
+    electric_rotations += direction;
+  } else if (new_electric_sector - electric_sector < (-3)) {
+    //overflow
+    direction = Direction::CW;
+    electric_rotations += direction;
+  } else {
+    direction = (new_electric_sector > electric_sector)? Direction::CW : Direction::CCW;
+  }
+  electric_sector = new_electric_sector;
+
+  // glitch avoidance #2 changes in direction can cause velocity spikes.  Possible improvements needed in this area
+  if (direction == old_direction) {
+    // not oscilating or just changed direction
+    pulse_diff = new_pulse_timestamp - pulse_timestamp;
+  } else {
+    pulse_diff = 0;
+  }
+
+  pulse_timestamp = new_pulse_timestamp;
+  total_interrupts++;
+  old_direction = direction;
+  if (onSectorChange != nullptr) onSectorChange(electric_sector);
+}
+
+/**
+ * Optionally set a function callback to be fired when sector changes
+ * void onSectorChange(int sector) {
+ *  ... // for debug or call driver directly?
+ * }
+ * sensor.attachSectorCallback(onSectorChange);
+ */
+void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) {
+  onSectorChange = _onSectorChange;
+}
+
+
+
+float HallSensor::getSensorAngle() {
+  return getAngle();
+}
+
+
+
+/*
+	Shaft angle calculation
+  TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost
+*/
+float HallSensor::getMechanicalAngle() {
+  return ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ;
+}
+
+/*
+  Shaft velocity calculation
+  function using mixed time and frequency measurement technique
+*/
+float HallSensor::getVelocity(){
+  if (pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old
+    return 0;
+  } else {
+    return direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f);
+  }
+
+}
+
+
+
+float HallSensor::getAngle() {
+  return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ;
+}
+
+
+double HallSensor::getPreciseAngle() {
+  return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ;
+}
+
+
+int32_t HallSensor::getFullRotations() {
+  return (int32_t)((electric_rotations * 6 + electric_sector) / cpr);
+}
+
+
+
+
+
+// HallSensor initialisation of the hardware pins 
+// and calculation variables
+void HallSensor::init(){
+  // initialise the electrical rotations to 0
+  electric_rotations = 0;
+
+  // HallSensor - check if pullup needed for your HallSensor
+  if(pullup == Pullup::USE_INTERN){
+    pinMode(pinA, INPUT_PULLUP);
+    pinMode(pinB, INPUT_PULLUP);
+    pinMode(pinC, INPUT_PULLUP);
+  }else{
+    pinMode(pinA, INPUT);
+    pinMode(pinB, INPUT);
+    pinMode(pinC, INPUT);
+  }
+
+    // init hall_state
+  A_active= digitalRead(pinA);
+  B_active = digitalRead(pinB);
+  C_active = digitalRead(pinC);
+  updateState();
+
+  pulse_timestamp = _micros();
+
+  // we don't call Sensor::init() here because init is handled in HallSensor class.
+}
+
+// function enabling hardware interrupts for the callback provided
+// if callback is not provided then the interrupt is not enabled
+void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){
+  // attach interrupt if functions provided
+
+  // A, B and C callback
+  if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE);
+  if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
+  if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE);
+}

v1.3/lib/Simple_FOC/src/sensors/HallSensor.h

@@ -0,0 +1,97 @@
+#ifndef HALL_SENSOR_LIB_H
+#define HALL_SENSOR_LIB_H
+
+#include "Arduino.h"
+#include "../common/base_classes/Sensor.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+
+// seq 1 > 5 > 4 > 6 > 2 > 3 > 1     000 001 010 011 100 101 110 111
+const int8_t ELECTRIC_SECTORS[8] = { -1,  0,  4,  5,  2,  1,  3 , -1 };
+
+class HallSensor: public Sensor{
+ public:
+    /**
+    HallSensor class constructor
+    @param encA  HallSensor A pin
+    @param encB  HallSensor B pin
+    @param encC  HallSensor C pin
+    @param pp  pole pairs  (e.g hoverboard motor has 15pp and small gimbals often have 7pp)
+    @param index index pin number (optional input)
+    */
+    HallSensor(int encA, int encB, int encC, int pp);
+
+    /** HallSensor initialise pins */
+    void init();
+    /**
+     *  function enabling hardware interrupts for the HallSensor channels with provided callback functions
+     *  if callback is not provided then the interrupt is not enabled
+     * 
+     * @param doA pointer to the A channel interrupt handler function
+     * @param doB pointer to the B channel interrupt handler function
+     * @param doIndex pointer to the Index channel interrupt handler function
+     * 
+     */
+    void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doC)() = nullptr);
+    
+    //  HallSensor interrupt callback functions
+    /** A channel callback function */
+    void handleA();
+    /** B channel callback function */
+    void handleB();
+    /** C channel callback function */
+    void handleC();
+    
+    
+    // pins A and B
+    int pinA; //!< HallSensor hardware pin A
+    int pinB; //!< HallSensor hardware pin B
+    int pinC; //!< HallSensor hardware pin C
+
+    // HallSensor configuration
+    Pullup pullup; //!< Configuration parameter internal or external pullups
+    int cpr;//!< HallSensor cpr number
+
+    // Abstract functions of the Sensor class implementation
+    /** get current angle (rad) */
+    float getSensorAngle() override;
+    float getMechanicalAngle() override;
+    float getAngle() override;
+    /**  get current angular velocity (rad/s) */
+    float getVelocity() override;
+    double getPreciseAngle() override;
+    int32_t getFullRotations() override;
+
+    // whether last step was CW (+1) or CCW (-1).  
+    Direction direction;
+
+    void attachSectorCallback(void (*onSectorChange)(int a) = nullptr);
+
+    // the current 3bit state of the hall sensors
+    volatile int8_t hall_state;
+    // the current sector of the sensor. Each sector is 60deg electrical
+    volatile int8_t electric_sector;
+    // the number of electric rotations
+    volatile long electric_rotations;
+    // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts)
+    volatile long total_interrupts; 
+
+  private:
+    
+    Direction decodeDirection(int oldState, int newState);
+    void updateState();
+
+    volatile unsigned long pulse_timestamp;//!< last impulse timestamp in us
+    volatile int A_active; //!< current active states of A channel
+    volatile int B_active; //!< current active states of B channel
+    volatile int C_active; //!< current active states of C channel
+
+    // function pointer for on sector change call back
+    void (*onSectorChange)(int sector) = nullptr;
+
+    volatile long pulse_diff;
+    
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/sensors/MagneticSensorAnalog.cpp

@@ -0,0 +1,42 @@
+#include "MagneticSensorAnalog.h"
+
+/** MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max)
+ * @param _pinAnalog  the pin that is reading the pwm from magnetic sensor
+ * @param _min_raw_count  the smallest expected reading.  Whilst you might expect it to be 0 it is often ~15.  Getting this wrong results in a small click once per revolution
+ * @param _max_raw_count  the largest value read.  whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note: For ESP32 (with 12bit ADC the value will be nearer 4096)
+ */
+MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_count, int _max_raw_count){
+
+  pinAnalog = _pinAnalog;
+
+  cpr = _max_raw_count - _min_raw_count;
+  min_raw_count = _min_raw_count;
+  max_raw_count = _max_raw_count;
+
+  if(pullup == Pullup::USE_INTERN){
+    pinMode(pinAnalog, INPUT_PULLUP);
+  }else{
+    pinMode(pinAnalog, INPUT);
+  }
+
+}
+
+
+void MagneticSensorAnalog::init(){
+	raw_count = getRawCount();
+
+  this->Sensor::init(); // call base class init
+}
+
+//  Shaft angle calculation
+//  angle is in radians [rad]
+float MagneticSensorAnalog::getSensorAngle(){
+  // raw data from the sensor
+  raw_count = getRawCount();   
+  return ( (float) (raw_count) / (float)cpr) * _2PI;
+}
+
+// function reading the raw counter of the magnetic sensor
+int MagneticSensorAnalog::getRawCount(){
+	return analogRead(pinAnalog);
+}

v1.3/lib/Simple_FOC/src/sensors/MagneticSensorAnalog.h

@@ -0,0 +1,51 @@
+#ifndef MAGNETICSENSORANALOG_LIB_H
+#define MAGNETICSENSORANALOG_LIB_H
+
+#include "Arduino.h"
+#include "../common/base_classes/Sensor.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+
+/**
+ * This sensor has been tested with AS5600 running in 'analog mode'.  This is where output pin of AS6000 is connected to an analog pin on your microcontroller.
+ * This approach is very simple but you may more accurate results with MagneticSensorI2C if that is also supported as its skips the DAC -> ADC conversion (ADC supports MagneticSensorI2C)
+ */
+class MagneticSensorAnalog: public Sensor{
+ public:
+    /**
+     * MagneticSensorAnalog class constructor
+     * @param _pinAnalog  the pin to read the PWM signal
+     */
+    MagneticSensorAnalog(uint8_t _pinAnalog, int _min = 0, int _max = 0);
+    
+
+    /** sensor initialise pins */
+    void init();
+
+    int pinAnalog; //!< encoder hardware pin A
+    
+    // Encoder configuration
+    Pullup pullup;
+    
+    // implementation of abstract functions of the Sensor class
+    /** get current angle (rad) */
+    float getSensorAngle() override;
+    /** raw count (typically in range of 0-1023), useful for debugging resolution issues */
+    int raw_count;
+
+  private:
+    int min_raw_count;
+    int max_raw_count;
+    int cpr;
+    int read();
+
+    /**
+     * Function getting current angle register value
+     * it uses angle_register variable
+     */
+    int getRawCount();
+
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/sensors/MagneticSensorI2C.cpp

@@ -0,0 +1,156 @@
+#include "MagneticSensorI2C.h"
+
+/** Typical configuration for the 12bit AMS AS5600 magnetic sensor over I2C interface */
+MagneticSensorI2CConfig_s AS5600_I2C = {
+  .chip_address = 0x36,
+  .bit_resolution = 12,
+  .angle_register = 0x0C,
+  .data_start_bit = 11
+};
+
+/** Typical configuration for the 12bit AMS AS5048 magnetic sensor over I2C interface */
+MagneticSensorI2CConfig_s AS5048_I2C = {
+  .chip_address = 0x40,  // highly configurable.  if A1 and A2 are held low, this is probable value
+  .bit_resolution = 14,
+  .angle_register = 0xFE,
+  .data_start_bit = 15
+};
+
+
+// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
+//  @param _chip_address  I2C chip address
+//  @param _bit_resolution  bit resolution of the sensor
+//  @param _angle_register_msb  angle read register
+//  @param _bits_used_msb number of used bits in msb
+MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){
+  // chip I2C address
+  chip_address = _chip_address;
+  // angle read register of the magnetic sensor
+  angle_register_msb = _angle_register_msb;
+  // register maximum value (counts per revolution)
+  cpr = pow(2, _bit_resolution);
+
+  // depending on the sensor architecture there are different combinations of
+  // LSB and MSB register used bits
+  // AS5600 uses 0..7 LSB and 8..11 MSB
+  // AS5048 uses 0..5 LSB and 6..13 MSB
+  // used bits in LSB
+  lsb_used = _bit_resolution - _bits_used_msb;
+  // extraction masks
+  lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
+  msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 );
+  wire = &Wire;
+}
+
+MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){
+  chip_address = config.chip_address; 
+
+  // angle read register of the magnetic sensor
+  angle_register_msb = config.angle_register;
+  // register maximum value (counts per revolution)
+  cpr = pow(2, config.bit_resolution);
+
+  int bits_used_msb = config.data_start_bit - 7;
+  lsb_used = config.bit_resolution - bits_used_msb;
+  // extraction masks
+  lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
+  msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 );
+  wire = &Wire;
+}
+
+void MagneticSensorI2C::init(TwoWire* _wire){
+
+  wire = _wire;
+
+  // I2C communication begin
+  wire->begin();
+
+  this->Sensor::init(); // call base class init
+}
+
+//  Shaft angle calculation
+//  angle is in radians [rad]
+float MagneticSensorI2C::getSensorAngle(){
+  // (number of full rotations)*2PI + current sensor angle 
+  return  ( getRawCount() / (float)cpr) * _2PI ;
+}
+
+
+
+// function reading the raw counter of the magnetic sensor
+int MagneticSensorI2C::getRawCount(){
+	return (int)MagneticSensorI2C::read(angle_register_msb);
+}
+
+// I2C functions
+/*
+* Read a register from the sensor
+* Takes the address of the register as a uint8_t
+* Returns the value of the register
+*/
+int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
+  // read the angle register first MSB then LSB
+	byte readArray[2];
+	uint16_t readValue = 0;
+  // notify the device that is aboout to be read
+	wire->beginTransmission(chip_address);
+	wire->write(angle_reg_msb);
+  wire->endTransmission(false);
+
+  // read the data msb and lsb
+	wire->requestFrom(chip_address, (uint8_t)2);
+	for (byte i=0; i < 2; i++) {
+		readArray[i] = wire->read();
+	}
+
+  // depending on the sensor architecture there are different combinations of
+  // LSB and MSB register used bits
+  // AS5600 uses 0..7 LSB and 8..11 MSB
+  // AS5048 uses 0..5 LSB and 6..13 MSB
+  readValue = ( readArray[1] &  lsb_mask );
+	readValue += ( ( readArray[0] & msb_mask ) << lsb_used );
+	return readValue;
+}
+
+/*
+* Checks whether other devices have locked the bus. Can clear SDA locks.
+* This should be called before sensor.init() on devices that suffer i2c slaves locking sda
+* e.g some stm32 boards with AS5600 chips
+* Takes the sda_pin and scl_pin
+* Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW
+*/
+int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) {
+
+  pinMode(scl_pin, INPUT_PULLUP);
+  pinMode(sda_pin, INPUT_PULLUP);
+  delay(250);
+
+  if (digitalRead(scl_pin) == LOW) {
+    // Someone else has claimed master!");
+    return 1;
+  }
+
+  if(digitalRead(sda_pin) == LOW) {
+    // slave is communicating and awaiting clocks, we are blocked
+    pinMode(scl_pin, OUTPUT);
+    for (byte i = 0; i < 16; i++) {
+      // toggle clock for 2 bytes of data
+      digitalWrite(scl_pin, LOW);
+      delayMicroseconds(20);
+      digitalWrite(scl_pin, HIGH);
+      delayMicroseconds(20);
+    }
+    pinMode(sda_pin, INPUT);
+    delayMicroseconds(20);
+    if (digitalRead(sda_pin) == LOW) {
+      // SDA still blocked
+      return 2;
+    }
+    _delay(1000);
+  }
+  // SDA is clear (HIGH)
+  pinMode(sda_pin, INPUT);
+  pinMode(scl_pin, INPUT);
+
+  return 0;
+}

v1.3/lib/Simple_FOC/src/sensors/MagneticSensorI2C.h

@@ -0,0 +1,81 @@
+#ifndef MAGNETICSENSORI2C_LIB_H
+#define MAGNETICSENSORI2C_LIB_H
+
+#include "Arduino.h"
+#include <Wire.h>
+#include "../common/base_classes/Sensor.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+
+struct MagneticSensorI2CConfig_s  {
+  int chip_address;
+  int bit_resolution;
+  int angle_register;
+  int data_start_bit; 
+};
+// some predefined structures
+extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C;
+
+#if defined(TARGET_RP2040)
+#define SDA I2C_SDA
+#define SCL I2C_SCL
+#endif
+
+
+class MagneticSensorI2C: public Sensor{
+ public:
+    /**
+     * MagneticSensorI2C class constructor
+     * @param chip_address  I2C chip address
+     * @param bits number of bits of the sensor resolution 
+     * @param angle_register_msb  angle read register msb
+     * @param _bits_used_msb number of used bits in msb
+     */
+    MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used);
+
+    /**
+     * MagneticSensorI2C class constructor
+     * @param config  I2C config
+     */
+    MagneticSensorI2C(MagneticSensorI2CConfig_s config);
+
+    static MagneticSensorI2C AS5600();
+        
+    /** sensor initialise pins */
+    void init(TwoWire* _wire = &Wire);
+
+    // implementation of abstract functions of the Sensor class
+    /** get current angle (rad) */
+    float getSensorAngle() override;
+
+    /** experimental function to check and fix SDA locked LOW issues */
+    int checkBus(byte sda_pin , byte scl_pin );
+
+  private:
+    float cpr; //!< Maximum range of the magnetic sensor
+    uint16_t lsb_used; //!< Number of bits used in LSB register
+    uint8_t lsb_mask;
+    uint8_t msb_mask;
+    
+    // I2C variables
+    uint8_t angle_register_msb; //!< I2C angle register to read
+    uint8_t chip_address; //!< I2C chip select pins
+
+    // I2C functions
+    /** Read one I2C register value */
+    int read(uint8_t angle_register_msb);
+
+    /**
+     * Function getting current angle register value
+     * it uses angle_register variable
+     */
+    int getRawCount();
+    
+    /* the two wire instance for this sensor */
+    TwoWire* wire;
+
+
+};
+
+
+#endif

v1.3/lib/Simple_FOC/src/sensors/MagneticSensorPWM.cpp

@@ -0,0 +1,71 @@
+#include "MagneticSensorPWM.h"
+#include "Arduino.h"
+
+/** MagneticSensorPWM(uint8_t _pinPWM, int _min, int _max)
+ * @param _pinPWM  the pin that is reading the pwm from magnetic sensor
+ * @param _min_raw_count  the smallest expected reading
+ * @param _max_raw_count  the largest expected reading
+ */
+MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _max_raw_count){
+
+    pinPWM = _pinPWM;
+
+    cpr = _max_raw_count - _min_raw_count;
+    min_raw_count = _min_raw_count;
+    max_raw_count = _max_raw_count;
+
+    // define if the sensor uses interrupts
+    is_interrupt_based = false;
+
+    // define as not set
+    last_call_us = _micros();
+}
+
+
+void MagneticSensorPWM::init(){
+
+    // initial hardware
+    pinMode(pinPWM, INPUT);
+    raw_count = getRawCount();
+    
+    this->Sensor::init(); // call base class init
+}
+
+// get current angle (rad)
+float MagneticSensorPWM::getSensorAngle(){
+    // raw data from sensor
+    raw_count = getRawCount();
+    return( (float) (raw_count) / (float)cpr) * _2PI;
+}
+
+
+// read the raw counter of the magnetic sensor
+int MagneticSensorPWM::getRawCount(){
+    if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way
+        pulse_length_us = pulseIn(pinPWM, HIGH);
+    }
+    return pulse_length_us;
+}
+
+
+void MagneticSensorPWM::handlePWM() {
+    //  unsigned long now_us = ticks();
+    unsigned long now_us = _micros();
+
+    // if falling edge, calculate the pulse length
+    if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us;
+
+    // save the currrent timestamp for the next call
+    last_call_us = now_us;
+    is_interrupt_based = true; // set the flag to true
+}
+
+// function enabling hardware interrupts of the for the callback provided
+// if callback is not provided then the interrupt is not enabled
+void MagneticSensorPWM::enableInterrupt(void (*doPWM)()){
+    // declare it's interrupt based
+    is_interrupt_based  = true;
+
+    // enable interrupts on pwm input pin
+    attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE);
+}

v1.3/lib/Simple_FOC/src/sensors/MagneticSensorPWM.h

@@ -0,0 +1,58 @@
+#ifndef MAGNETICSENSORPWM_LIB_H
+#define MAGNETICSENSORPWM_LIB_H
+
+#include "Arduino.h"
+#include "../common/base_classes/Sensor.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+
+// This sensor has been tested with AS5048a running in PWM mode.
+
+class MagneticSensorPWM: public Sensor{
+ public:
+  /**
+    * MagneticSensorPWM class constructor
+    * @param _pinPWM the pin to read the PWM sensor input signal
+    */
+    MagneticSensorPWM(uint8_t _pinPWM,int _min = 0, int _max = 0);
+
+
+    // initialize the sensor hardware
+    void init();
+
+    int pinPWM;
+
+    // get current angle (rad)
+    float getSensorAngle() override;
+  
+    // pwm handler
+    void handlePWM();
+    void enableInterrupt(void (*doPWM)());
+    unsigned long pulse_length_us;
+
+  private:
+    // raw count (typically in range of 0-1023)
+    int raw_count;
+    int min_raw_count;
+    int max_raw_count;
+    int cpr;
+
+    // flag saying if the readings are interrupt based or not
+    bool is_interrupt_based;
+
+    int read();
+
+    /**
+     * Function getting current angle register value
+     * it uses angle_register variable
+     */
+    int getRawCount();
+
+    // time tracking variables
+    unsigned long last_call_us;
+    // unsigned long pulse_length_us;
+    
+
+};
+
+#endif

v1.3/lib/Simple_FOC/src/sensors/MagneticSensorSPI.cpp

@@ -0,0 +1,161 @@
+
+#include "MagneticSensorSPI.h"
+
+/** Typical configuration for the 14bit AMS AS5147 magnetic sensor over SPI interface */
+MagneticSensorSPIConfig_s AS5147_SPI = {
+  .spi_mode = SPI_MODE1,
+  .clock_speed = 1000000,
+  .bit_resolution = 14,
+  .angle_register = 0x3FFF,
+  .data_start_bit = 13,
+  .command_rw_bit = 14,
+  .command_parity_bit = 15
+};
+// AS5048 and AS5047 are the same as AS5147
+MagneticSensorSPIConfig_s AS5048_SPI = AS5147_SPI;
+MagneticSensorSPIConfig_s AS5047_SPI = AS5147_SPI;
+
+/** Typical configuration for the 14bit MonolithicPower MA730 magnetic sensor over SPI interface */
+MagneticSensorSPIConfig_s MA730_SPI = {
+  .spi_mode = SPI_MODE0,
+  .clock_speed = 1000000,
+  .bit_resolution = 14,
+  .angle_register = 0x0000,
+  .data_start_bit = 15,
+  .command_rw_bit = 0,  // not required
+  .command_parity_bit = 0 // parity not implemented
+};
+
+
+// MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register)
+//  cs              - SPI chip select pin
+//  _bit_resolution   sensor resolution bit number
+// _angle_register  - (optional) angle read register - default 0x3FFF
+MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register){
+
+  chip_select_pin = cs;
+  // angle read register of the magnetic sensor
+  angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER;
+  // register maximum value (counts per revolution)
+  cpr = pow(2,_bit_resolution);
+  spi_mode = SPI_MODE1;
+  clock_speed = 1000000;
+  bit_resolution = _bit_resolution;
+
+  command_parity_bit = 15; // for backwards compatibilty
+  command_rw_bit = 14; // for backwards compatibilty
+  data_start_bit = 13; // for backwards compatibilty
+}
+
+MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){
+  chip_select_pin = cs;
+  // angle read register of the magnetic sensor
+  angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTER;
+  // register maximum value (counts per revolution)
+  cpr = pow(2, config.bit_resolution);
+  spi_mode = config.spi_mode;
+  clock_speed = config.clock_speed;
+  bit_resolution = config.bit_resolution;
+
+  command_parity_bit = config.command_parity_bit; // for backwards compatibilty
+  command_rw_bit = config.command_rw_bit; // for backwards compatibilty
+  data_start_bit = config.data_start_bit; // for backwards compatibilty
+}
+
+void MagneticSensorSPI::init(SPIClass* _spi){
+  spi = _spi;
+	// 1MHz clock (AMS should be able to accept up to 10MHz)
+	settings = SPISettings(clock_speed, MSBFIRST, spi_mode);
+	//setup pins
+	pinMode(chip_select_pin, OUTPUT);
+	//SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices
+	spi->begin();
+	// do any architectures need to set the clock divider for SPI? Why was this in the code?
+  //spi->setClockDivider(SPI_CLOCK_DIV8);
+	digitalWrite(chip_select_pin, HIGH);
+
+  this->Sensor::init(); // call base class init
+}
+
+//  Shaft angle calculation
+//  angle is in radians [rad]
+float MagneticSensorSPI::getSensorAngle(){
+  return (getRawCount() / (float)cpr) * _2PI;
+}
+
+// function reading the raw counter of the magnetic sensor
+int MagneticSensorSPI::getRawCount(){
+	return (int)MagneticSensorSPI::read(angle_register);
+}
+
+// SPI functions 
+/**
+ * Utility function used to calculate even parity of word
+ */
+byte MagneticSensorSPI::spiCalcEvenParity(word value){
+	byte cnt = 0;
+	byte i;
+
+	for (i = 0; i < 16; i++)
+	{
+		if (value & 0x1) cnt++;
+		value >>= 1;
+	}
+	return cnt & 0x1;
+}
+
+  /*
+  * Read a register from the sensor
+  * Takes the address of the register as a 16 bit word
+  * Returns the value of the register
+  */
+word MagneticSensorSPI::read(word angle_register){
+
+  word command = angle_register;
+
+  if (command_rw_bit > 0) {
+    command = angle_register | (1 << command_rw_bit);
+  }
+  if (command_parity_bit > 0) {
+   	//Add a parity bit on the the MSB
+  	command |= ((word)spiCalcEvenParity(command) << command_parity_bit);
+  }
+
+  //SPI - begin transaction
+  spi->beginTransaction(settings);
+
+  //Send the command
+  digitalWrite(chip_select_pin, LOW);
+  spi->transfer16(command);
+  digitalWrite(chip_select_pin,HIGH);
+  
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) // if ESP32 board
+  delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested...
+#else
+  delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702
+#endif
+
+  //Now read the response
+  digitalWrite(chip_select_pin, LOW);
+  word register_value = spi->transfer16(0x00);
+  digitalWrite(chip_select_pin, HIGH);
+
+  //SPI - end transaction
+  spi->endTransaction();
+
+  register_value = register_value >> (1 + data_start_bit - bit_resolution);  //this should shift data to the rightmost bits of the word
+
+  const static word data_mask = 0xFFFF >> (16 - bit_resolution);
+
+	return register_value & data_mask;  // Return the data, stripping the non data (e.g parity) bits
+}
+
+/**
+ * Closes the SPI connection
+ * SPI has an internal SPI-device counter, for each init()-call the close() function must be called exactly 1 time
+ */
+void MagneticSensorSPI::close(){
+	spi->end();
+}
+
+

v1.3/lib/Simple_FOC/src/sensors/MagneticSensorSPI.h

@@ -0,0 +1,84 @@
+#ifndef MAGNETICSENSORSPI_LIB_H
+#define MAGNETICSENSORSPI_LIB_H
+
+
+#include "Arduino.h"
+#include <SPI.h>
+#include "../common/base_classes/Sensor.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+
+#define DEF_ANGLE_REGISTER 0x3FFF
+
+struct MagneticSensorSPIConfig_s  {
+  int spi_mode;
+  long clock_speed;
+  int bit_resolution;
+  int angle_register;
+  int data_start_bit;
+  int command_rw_bit;
+  int command_parity_bit;
+};
+// typical configuration structures
+extern MagneticSensorSPIConfig_s AS5147_SPI,AS5048_SPI,AS5047_SPI, MA730_SPI;
+
+class MagneticSensorSPI: public Sensor{
+ public:
+    /**
+     *  MagneticSensorSPI class constructor
+     * @param cs  SPI chip select pin 
+     * @param bit_resolution   sensor resolution bit number
+     * @param angle_register  (optional) angle read register - default 0x3FFF
+     */
+    MagneticSensorSPI(int cs, float bit_resolution, int angle_register = 0);
+    /**
+     *  MagneticSensorSPI class constructor
+     * @param config   SPI config
+     * @param cs  SPI chip select pin
+     */
+    MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs);
+
+    /** sensor initialise pins */
+    void init(SPIClass* _spi = &SPI);
+
+    // implementation of abstract functions of the Sensor class
+    /** get current angle (rad) */
+    float getSensorAngle() override;
+
+    // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3
+    int spi_mode;
+    
+    /* returns the speed of the SPI clock signal */
+    long clock_speed;
+    
+
+  private:
+    float cpr; //!< Maximum range of the magnetic sensor
+    // spi variables
+    int angle_register; //!< SPI angle register to read
+    int chip_select_pin; //!< SPI chip select pin
+	  SPISettings settings; //!< SPI settings variable
+    // spi functions
+    /** Stop SPI communication */
+    void close(); 
+    /** Read one SPI register value */
+    word read(word angle_register);
+    /** Calculate parity value  */
+    byte spiCalcEvenParity(word value);
+
+    /**
+     * Function getting current angle register value
+     * it uses angle_register variable
+     */
+    int getRawCount();
+    
+    int bit_resolution; //!< the number of bites of angle data
+    int command_parity_bit; //!< the bit where parity flag is stored in command
+    int command_rw_bit; //!< the bit where read/write flag is stored in command
+    int data_start_bit; //!< the the position of first bit
+
+    SPIClass* spi;
+};
+
+
+#endif