try to fix submodule

firmware/lib/Arduino-FOC/src/common/base_classes/BLDCDriver.h

@@ -0,0 +1,42 @@
+#ifndef BLDCDRIVER_H
+#define BLDCDRIVER_H
+
+#include "Arduino.h"
+#include "FOCDriver.h"
+
+
+enum PhaseState : uint8_t {
+  PHASE_OFF = 0, // both sides of the phase are off
+  PHASE_ON = 1,  // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode
+  PHASE_HI = 2,  // only the high side of the phase is driven with PWM (6-PWM mode only)
+  PHASE_LO = 3,  // only the low side of the phase is driven with PWM (6-PWM mode only)
+};
+
+
+class BLDCDriver: public FOCDriver{
+    public:
+
+        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
+
+        /**
+         * 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(PhaseState sa, PhaseState sb, PhaseState sc) = 0;
+};
+
+#endif

firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp

@@ -0,0 +1,121 @@
+#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;
+    }
+    if (!current.a)
+    {
+        // if only two measured currents
+        float a = -current.c - current.b;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
+    }
+    if (!current.b)
+    {
+        // if only two measured currents
+        float b = -current.a - current.c;
+        i_alpha = current.a;
+        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * 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)
+    {
+        float ct;
+        float st;
+        _sincos(motor_electrical_angle, &st, &ct);
+        sign = (i_beta * ct - i_alpha * st) > 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;
+    }
+    if (!current.a)
+    {
+        // if only two measured currents
+        float a = -current.c - current.b;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
+    }
+    if (!current.b)
+    {
+        // if only two measured currents
+        float b = -current.a - current.c;
+        i_alpha = current.a;
+        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * 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;
+    float st;
+    _sincos(angle_el, &st, &ct);
+    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;
+}
+
+/**
+    Driver linking to the current sense
+*/
+void CurrentSense::linkDriver(BLDCDriver *_driver)
+{
+    driver = _driver;
+}
+
+void CurrentSense::linkDriver(StepperDriver *_driver)
+{
+    driver = _driver;
+}

firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h

@@ -0,0 +1,75 @@
+#ifndef CURRENTSENSE_H
+#define CURRENTSENSE_H
+
+#include "BLDCDriver.h"
+#include "StepperDriver.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
+     *   
+     * @returns -  0 - for failure &  1 - for success 
+     */
+    virtual int init() = 0;
+    
+    /**
+     * Linking the current sense with the motor driver
+     * Only necessary if synchronisation in between the two is required
+     */
+    void linkDriver(BLDCDriver *driver);
+    void linkDriver(StepperDriver *driver);
+
+    // variables
+    bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC()
+    
+    FOCDriver* driver = nullptr; //!< driver link
+    bool initialized = false; // true if current sense was successfully initialized   
+    void* params = 0; //!< pointer to hardware specific parameters of current sensing
+    
+    /**
+     * 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(float align_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

firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h

@@ -0,0 +1,21 @@
+#ifndef FOCMOTOR_H
+#define FOTMOTOR_H
+
+#include "Arduino.h"
+
+class FOCDriver{
+    public: 
+        virtual int init() = 0;
+
+        virtual void enable() = 0;
+
+        virtual void disable() = 0;
+
+        long pwm_frequency;
+        float voltage_power_supply;
+        float voltage_limit;
+        bool initialized = false;
+        void* params = 0;
+};
+
+#endif

firmware/lib/Arduino-FOC/src/common/base_classes/FOCMotor.cpp

@@ -0,0 +1,155 @@
+#include "FOCMotor.h"
+#include "../../communication/SimpleFOCDebug.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;
+
+  // voltage bemf 
+  voltage_bemf = 0;
+  
+  //monitor_port 
+  monitor_port = nullptr;
+  //sensor 
+  sensor_offset = 0.0f;
+  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
+  #ifndef SIMPLEFOC_DISABLE_DEBUG
+  SimpleFOCDebug::enable(&print);
+  SIMPLEFOC_DEBUG("MOT: Monitor enabled!");
+  #endif
+}
+
+// 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){
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    monitor_port->print(target,monitor_decimals);    
+    printed= true;
+  }
+  if(monitor_variables & _MON_VOLT_Q) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(voltage.q,monitor_decimals);
+    printed= true;
+  }
+  if(monitor_variables & _MON_VOLT_D) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(voltage.d,monitor_decimals);
+    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 = current;
+    if( current_sense && torque_controller != TorqueControlType::foc_current ){
+      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) {
+      if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+      else if(printed) monitor_port->print(monitor_separator);
+      monitor_port->print(c.q*1000, monitor_decimals); // mAmps
+      printed= true;
+    }
+    if(monitor_variables & _MON_CURR_D) {
+      if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+      else if(printed) monitor_port->print(monitor_separator);
+      monitor_port->print(c.d*1000, monitor_decimals); // mAmps
+      printed= true;
+    }
+  }
+ 
+  if(monitor_variables & _MON_VEL) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(shaft_velocity,monitor_decimals);
+    printed= true;
+  }
+  if(monitor_variables & _MON_ANGLE) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(shaft_angle,monitor_decimals);
+    printed= true;
+  }
+  if(printed){
+    if(monitor_end_char) monitor_port->println(monitor_end_char);
+    else monitor_port->println("");
+  }
+}   
+

firmware/lib/Arduino-FOC/src/common/base_classes/FOCMotor.h

@@ -0,0 +1,251 @@
+#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
+     */  
+    virtual int initFOC()=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;
+
+    /**
+    * 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
+    */
+    virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=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 feed_forward_velocity = 0.0f; //!< current feed forward velocity
+  	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
+    float voltage_bemf; //!< estimated backemf voltage (if provided KV constant)
+
+    // 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
+    float KV_rating; //!< motor KV rating
+    float	phase_inductance; //!< motor phase inductance
+
+    // limiting variables
+    float voltage_limit; //!< Voltage limiting variable - global limit
+    float current_limit; //!< Current limiting variable - global limit
+    float velocity_limit; //!< Velocity limiting 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 determining 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
+    Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration
+
+    /**
+     * 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 
+    char monitor_start_char = '\0'; //!< monitor starting character 
+    char monitor_end_char = '\0'; //!< monitor outputs ending character 
+    char monitor_separator = '\t'; //!< monitor outputs separation character
+    unsigned int  monitor_decimals = 4; //!< monitor outputs decimal places
+    // 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

firmware/lib/Arduino-FOC/src/common/base_classes/Sensor.cpp

@@ -0,0 +1,78 @@
+#include "Sensor.h"
+#include "../foc_utils.h"
+#include "../time_utils.h"
+
+
+
+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;
+    if (Ts < 0.0f) {    // handle micros() overflow - we need to reset vel_angle_prev_ts
+        vel_angle_prev = angle_prev;
+        vel_full_rotations = full_rotations;
+        vel_angle_prev_ts = angle_prev_ts;
+        return velocity;
+    }
+    if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
+
+    velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;
+    vel_angle_prev = angle_prev;
+    vel_full_rotations = full_rotations;
+    vel_angle_prev_ts = angle_prev_ts;
+    return velocity;
+}
+
+
+
+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
+}

firmware/lib/Arduino-FOC/src/common/base_classes/Sensor.h

@@ -0,0 +1,139 @@
+#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{
+	friend class SmoothingSensor;
+    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();
+
+        /**
+         * Minimum time between updates to velocity. If time elapsed is lower than this, the velocity is not updated.
+         */
+        float min_elapsed_time = 0.000100; // default is 100 microseconds, or 10kHz
+
+    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 velocity=0.0f;
+        float angle_prev=0.0f; // 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.0f; // 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

firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h

@@ -0,0 +1,18 @@
+#ifndef STEPPERDRIVER_H
+#define STEPPERDRIVER_H
+
+#include "drivers/hardware_api.h"
+#include "FOCDriver.h"
+
+class StepperDriver: public FOCDriver{
+    public:
+        /** 
+         * 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

firmware/lib/Arduino-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 

firmware/lib/Arduino-FOC/src/common/foc_utils.cpp

@@ -0,0 +1,73 @@
+#include "foc_utils.h"
+
+
+// function approximating the sine calculation by using fixed size array
+// uses a 65 element lookup table and interpolation
+// thanks to @dekutree for his work on optimizing this
+__attribute__((weak)) float _sin(float a){
+  // 16bit integer array for sine lookup. interpolation is used for better precision
+  // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size
+  // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps)
+  static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768};
+  unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI));
+  int t1, t2, frac = i & 0xff;
+  i = (i >> 8) & 0xff;
+  if (i < 64) {
+    t1 = sine_array[i]; t2 = sine_array[i+1];
+  }
+  else if(i < 128) {
+    t1 = sine_array[128 - i]; t2 = sine_array[127 - i];
+  }
+  else if(i < 192) {
+    t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i];
+  }
+  else {
+    t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i];
+  }
+  return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8));
+}
+
+// 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
+__attribute__((weak)) float _cos(float a){
+  float a_sin = a + _PI_2;
+  a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin;
+  return _sin(a_sin);
+}
+
+
+__attribute__((weak)) void _sincos(float a, float* s, float* c){
+  *s = _sin(a);
+  *c = _cos(a);
+}
+
+
+// normalizing radian angle to [0,2PI]
+__attribute__((weak)) 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
+__attribute__((weak)) float _sqrtApprox(float number) {//low in fat
+  // float x;
+  // const float f = 1.5F; // better precision
+
+  // x = number * 0.5F;
+  float y = number;
+  long i = * ( long * ) &y;
+  i = 0x5f375a86 - ( i >> 1 );
+  y = * ( float * ) &i;
+  // y = y * ( f - ( x * y * y ) ); // better precision
+  return number * y;
+}

firmware/lib/Arduino-FOC/src/common/foc_utils.h

@@ -0,0 +1,106 @@
+#ifndef FOCUTILS_LIB_H
+#define FOCUTILS_LIB_H
+
+#include "Arduino.h"
+
+// sign function
+#define _sign(a) ( ( (a) < 0 )  ?  -1   : ( (a) > 0 ) )
+#ifndef _round
+#define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f))
+#endif
+#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)
+#define _powtwo(x) (1 << (x))
+
+// 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 _RPM_TO_RADS 0.10471975512f
+
+#define NOT_SET -12345.0f
+#define _HIGH_IMPEDANCE 0
+#define _HIGH_Z _HIGH_IMPEDANCE
+#define _ACTIVE 1
+#define _NC (NOT_SET)
+
+#define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f)
+
+// 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);
+/**
+ * Function returning both sine and cosine of the angle in one call.
+ * Internally it uses the _sin and _cos functions, but you may wish to
+ * provide your own implementation which is more optimized.
+ */
+void _sincos(float a, float* s, float* c);
+
+
+/**
+ * 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

firmware/lib/Arduino-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;
+}

firmware/lib/Arduino-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

firmware/lib/Arduino-FOC/src/common/pid.cpp

@@ -0,0 +1,64 @@
+#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;
+}
+
+void PIDController::reset(){
+    integral_prev = 0.0f;
+    output_prev = 0.0f;
+    error_prev = 0.0f;
+}

firmware/lib/Arduino-FOC/src/common/pid.h

@@ -0,0 +1,41 @@
+#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);
+    void reset();
+
+    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
+
+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

firmware/lib/Arduino-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
+}

firmware/lib/Arduino-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