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

2025-10-06 12:34:26 +08:00
1 parent 0986832fd1
commit aacd052c06
@@ -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
@@ -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;
+}
@@ -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
@@ -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();
+  
+}
+
@@ -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
@@ -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
+}
@@ -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
@@ -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
@@ -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 
@@ -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;
+}
@@ -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
@@ -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;
+}
@@ -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
@@ -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;
+}
@@ -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，便于清楚积分影响
@@ -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
+}
@@ -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