[ENHANCEMENT] lag compensation using phase inductance - proposed in #246

Author: askuric

README.md

@@ -37,6 +37,10 @@ Therefore this is an attempt to:
 >   - stm32 - software and hardware 6pwm
 >   - atmega328 
 >   - atmega2560
+> - Lag compensation using motor inductance [#246](https://github.com/simplefoc/Arduino-FOC/issues/246)
+>   - current control through voltage torque mode enhancement
+>   - extended `BLDCMotor` and `StepperMotor` constructors to receive the inductance paramerer
+>   - can also be set using `motor.phase_inductance` or through `Commander`
 ## Arduino *SimpleFOClibrary* v2.2.3
 
 <p align="">

src/BLDCMotor.h

@@ -18,10 +18,11 @@ class BLDCMotor: public FOCMotor
     /**
      BLDCMotor class constructor
      @param pp pole pairs number
-     @param R  motor phase resistance
+     @param R  motor phase resistance - [Ohm]
      @param KV  motor KV rating (1/K_bemf) - rpm/V
+     @param L  motor phase inductance - [H]
      */ 
-    BLDCMotor(int pp,  float R = NOT_SET, float KV = NOT_SET);
+    BLDCMotor(int pp,  float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
 
     /**
      * Function linking a motor and a foc driver 

src/StepperMotor.cpp

@@ -6,7 +6,8 @@
 // - pp            - pole pair number
 // - R             - motor phase resistance
 // - KV            - motor kv rating (rmp/v)
-StepperMotor::StepperMotor(int pp, float _R, float _KV)
+// - L             - motor phase inductance [H]
+StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance)
 : FOCMotor()
 {
   // number od pole pairs
@@ -16,6 +17,8 @@ StepperMotor::StepperMotor(int pp, float _R, float _KV)
   // save back emf constant KV = 1/K_bemf
   // usually used rms
   KV_rating = _KV*_SQRT2;
+  // save phase inductance
+  phase_inductance = _inductance;
 
   // torque control type is voltage by default
   // current and foc_current not supported yet
@@ -296,7 +299,9 @@ void StepperMotor::move(float new_target) {
       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);
-      voltage.d = 0;
+      // 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);
       break;
     case MotionControlType::angle:
       // angle set point
@@ -309,7 +314,9 @@ void StepperMotor::move(float new_target) {
       // 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);
-        voltage.d = 0;
+      // 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
@@ -320,19 +327,21 @@ void StepperMotor::move(float new_target) {
       // 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);
-      voltage.d = 0;
+      // 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
       shaft_velocity_sp = target;
       voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
-      voltage.d = 0;
+      voltage.d = 0; // TODO d-component lag-compensation 
       break;
     case MotionControlType::angle_openloop:
       // angle control in open loop
       shaft_angle_sp = target;
       voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
-      voltage.d = 0;
+      voltage.d = 0; // TODO d-component lag-compensation 
       break;
   }
 }

src/StepperMotor.h

@@ -23,10 +23,11 @@ class StepperMotor: public FOCMotor
     /**
       StepperMotor class constructor
       @param pp  pole pair number 
-      @param R  motor phase resistance
-      @param KV  motor KV rating (1/K_bemf) - rpm/V
+     @param R  motor phase resistance - [Ohm]
+     @param KV  motor KV rating (1/K_bemf) - rpm/V
+     @param L  motor phase inductance - [H]
     */
-    StepperMotor(int pp,  float R = NOT_SET, float KV = NOT_SET);
+    StepperMotor(int pp,  float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
 
     /**
      * Function linking a motor and a foc driver 

src/communication/Commander.cpp

@@ -221,6 +221,14 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
       if(_isset(motor->phase_resistance)) println(motor->phase_resistance);
       else println(0);
       break;
+    case CMD_INDUCTANCE:
+      printVerbose(F("L phase: "));
+      if(!GET){
+        motor->phase_inductance = value;
+      }
+      if(_isset(motor->phase_inductance)) println(motor->phase_inductance);
+      else println(0);
+      break;
     case CMD_KV_RATING:
       printVerbose(F("Motor KV: "));
       if(!GET){

src/communication/commands.h

@@ -15,6 +15,7 @@
  #define CMD_SENSOR    'S' //!< sensor offsets
  #define CMD_MONITOR   'M' //!< monitoring
  #define CMD_RESIST    'R' //!< motor phase resistance
+ #define CMD_INDUCTANCE    'I' //!< motor phase inductance
  #define CMD_KV_RATING 'K' //!< motor kv rating
  #define CMD_PWMMOD   'W' //!< pwm modulation