初始化

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

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;
+}

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

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();
+  
+}
+

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

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
+}

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

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