added initial support for current sensing for stepper motors

src/BLDCMotor.h

@@ -4,6 +4,7 @@
 #include "Arduino.h"
 #include "common/base_classes/FOCMotor.h"
 #include "common/base_classes/Sensor.h"
+#include "common/base_classes/FOCDriver.h"
 #include "common/base_classes/BLDCDriver.h"
 #include "common/foc_utils.h"
 #include "common/time_utils.h"

src/StepperMotor.cpp

@@ -108,12 +108,28 @@ int  StepperMotor::initFOC() {
   // alignment necessary for encoders!
   // sensor and motor alignment - can be skipped
   // by setting motor.sensor_direction and motor.zero_electric_angle
-  _delay(500);
   if(sensor){
     exit_flag *= alignSensor();
     // added the shaft_angle update
     sensor->update();
-    shaft_angle = sensor->getAngle();
+    shaft_angle = shaftAngle();
+
+    // aligning the current sensor - can be skipped
+    // checks if driver phases are the same as current sense phases
+    // and checks the direction of measuremnt.
+    if(exit_flag){
+      if(current_sense){ 
+        if (!current_sense->initialized) {
+          motor_status = FOCMotorStatus::motor_calib_failed;
+          SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized");
+          exit_flag = 0;
+        }else{
+          exit_flag *= alignCurrentSense();
+        }
+      }
+      else { SIMPLEFOC_DEBUG("MOT: No current sense."); }
+    }
+
   } else {
     SIMPLEFOC_DEBUG("MOT: No sensor.");
     if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){
@@ -136,6 +152,26 @@ int  StepperMotor::initFOC() {
   return exit_flag;
 }
 
+// Calibrate the motor and current sense phases
+int StepperMotor::alignCurrentSense() {
+  int exit_flag = 1; // success
+
+  SIMPLEFOC_DEBUG("MOT: Align current sense.");
+
+  // align current sense and the driver
+  exit_flag = current_sense->driverAlign(voltage_sensor_align);
+  if(!exit_flag){
+    // error in current sense - phase either not measured or bad connection
+    SIMPLEFOC_DEBUG("MOT: Align error!");
+    exit_flag = 0;
+  }else{
+    // output the alignment status flag
+    SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
+  }
+
+  return exit_flag > 0;
+}
+
 // Encoder alignment to electrical 0 angle
 int StepperMotor::alignSensor() {
   int exit_flag = 1; //success
@@ -261,8 +297,6 @@ void StepperMotor::loopFOC() {
 
   // if open-loop do nothing
   if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
-  // shaft angle
-  shaft_angle = shaftAngle();
 
   // if disabled do nothing
   if(!enabled) return;
@@ -272,6 +306,44 @@ void StepperMotor::loopFOC() {
   // which is in range 0-2PI
   electrical_angle = electricalAngle();
 
+  // Needs the update() to be called first
+  // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() 
+  // which is in range 0-2PI
+  electrical_angle = electricalAngle();
+  switch (torque_controller) {
+    case TorqueControlType::voltage:
+      // no need to do anything really
+      break;
+    case TorqueControlType::dc_current:
+      if(!current_sense) return;
+      // read overall current magnitude
+      current.q = current_sense->getDCCurrent(electrical_angle);
+      // filter the value values
+      current.q = LPF_current_q(current.q);
+      // calculate the phase voltage
+      voltage.q = PID_current_q(current_sp - current.q);
+      // d voltage  - lag compensation
+      if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      else voltage.d = 0;
+      break;
+    case TorqueControlType::foc_current:
+      if(!current_sense) return;
+      // read dq currents
+      current = current_sense->getFOCCurrents(electrical_angle);
+      // filter values
+      current.q = LPF_current_q(current.q);
+      current.d = LPF_current_d(current.d);
+      // calculate the phase voltages
+      voltage.q = PID_current_q(current_sp - current.q);
+      voltage.d = PID_current_d(-current.d);
+      // d voltage - lag compensation - TODO verify
+      // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      break;
+    default:
+      // no torque control selected
+      SIMPLEFOC_DEBUG("MOT: no torque control selected!");
+      break;
+  }
   // set the phase voltage - FOC heart function :)
   setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
 }
@@ -309,56 +381,70 @@ void StepperMotor::move(float new_target) {
   // estimate the motor current if phase reistance available and current_sense not available
   if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance;
 
-  // choose control loop
+   // upgrade the current based voltage limit
   switch (controller) {
     case MotionControlType::torque:
-      if(!_isset(phase_resistance))  voltage.q = target; // if voltage torque control
-      else  voltage.q =  target*phase_resistance + voltage_bemf;
-      voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
-      // set d-component (lag compensation if known inductance)
-      if(!_isset(phase_inductance)) voltage.d = 0;
-      else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
+        if(!_isset(phase_resistance))  voltage.q = target;
+        else  voltage.q =  target*phase_resistance + voltage_bemf;
+        voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }else{
+        current_sp = target; // if current/foc_current torque control
+      }
       break;
     case MotionControlType::angle:
+      // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
+      //                        the angles are large. This results in not being able to command small changes at high position values.
+      //                        to solve this, the delta-angle has to be calculated in a numerically precise way.
       // angle set point
       shaft_angle_sp = target;
       // calculate velocity set point
       shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
-      shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
-      // calculate the torque command
+      shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit);
+      // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
-      // use voltage if phase-resistance not provided
-      if(!_isset(phase_resistance))  voltage.q = current_sp;
-      else  voltage.q =  _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
-      // set d-component (lag compensation if known inductance)
-      if(!_isset(phase_inductance)) voltage.d = 0;
-      else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      if(torque_controller == TorqueControlType::voltage){
+        // use voltage if phase-resistance not provided
+        if(!_isset(phase_resistance))  voltage.q = current_sp;
+        else  voltage.q =  _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }
       break;
     case MotionControlType::velocity:
-      // velocity set point
+      // velocity set point - sensor precision: this calculation is numerically precise.
       shaft_velocity_sp = target;
       // calculate the torque command
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
       // if torque controlled through voltage control
-      // use voltage if phase-resistance not provided
-      if(!_isset(phase_resistance))  voltage.q = current_sp;
-      else  voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
-      // set d-component (lag compensation if known inductance)
-      if(!_isset(phase_inductance)) voltage.d = 0;
-      else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      if(torque_controller == TorqueControlType::voltage){
+        // use voltage if phase-resistance not provided
+        if(!_isset(phase_resistance))  voltage.q = current_sp;
+        else  voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }
       break;
     case MotionControlType::velocity_openloop:
-      // velocity control in open loop
+      // velocity control in open loop - sensor precision: this calculation is numerically precise.
       shaft_velocity_sp = target;
       voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
-      voltage.d = 0; // TODO d-component lag-compensation 
+      voltage.d = 0;
       break;
     case MotionControlType::angle_openloop:
-      // angle control in open loop
+      // angle control in open loop - 
+      // TODO sensor precision: this calculation NOT numerically precise, and subject
+      //                        to the same problems in small set-point changes at high angles 
+      //                        as the closed loop version.
       shaft_angle_sp = target;
       voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
-      voltage.d = 0; // TODO d-component lag-compensation 
+      voltage.d = 0;
       break;
   }
 }

src/StepperMotor.h

@@ -89,6 +89,8 @@ class StepperMotor: public FOCMotor
     int alignSensor();
     /** Motor and sensor alignment to the sensors absolute 0 angle  */
     int absoluteZeroSearch();
+    /** Current sense and motor phase alignment */
+    int alignCurrentSense();
 
     // Open loop motion control    
     /**

src/common/base_classes/BLDCDriver.h

@@ -2,40 +2,17 @@
 #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{
+class BLDCDriver: public FOCDriver{
     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
-        void* params = 0; // pointer to hardware specific parameters of driver
-
         /**
-         * Set phase voltages to the harware
+         * Set phase voltages to the hardware
          *
          * @param Ua - phase A voltage
          * @param Ub - phase B voltage
@@ -51,6 +28,9 @@ class BLDCDriver{
          * @param sa - phase C state : active / disabled ( high impedance )
         */
         virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) = 0;
+
+        /** driver type getter function */
+        virtual DriverType type() override { return DriverType::BLDC; };
 };
 
 #endif