FEAT current limiting in voltage mode and support for adding the KV rating of the motor

Author: askuric

README.md

@@ -26,6 +26,9 @@ Therefore this is an attempt to:
 >    - Current sense API
 > - Low-side current sensing
 >    - ESP32 generic support for multiple motors
+> - New handlign of current limit using voltage 
+>    - Support for motor KV rating - back emf estimation
+>    - Using motor phase resistance
 
 
 

keywords.txt

@@ -235,6 +235,7 @@ _PI_3	LITERAL1
 _SQRT3	LITERAL1
 _PI	LITERAL1
 _HIGH_Z	LITERAL1
+_NC	LITERAL1
 
 _MON_TARGET	LITERAL1
 _MON_VOLT_Q	LITERAL1

src/BLDCMotor.cpp

@@ -3,13 +3,17 @@
 // BLDCMotor( int pp , float R)
 // - pp            - pole pair number
 // - R             - motor phase resistance
-BLDCMotor::BLDCMotor(int pp, float _R)
+// - KV            - motor kv rating
+BLDCMotor::BLDCMotor(int pp, float _R, float _KV)
 : FOCMotor()
 {
   // save pole pairs number
   pole_pairs = pp;
   // save phase resistance number
   phase_resistance = _R;
+  // save back emf constant KV = 1/KV
+  K_bemf = 1.0/_KV;
+
   // torque control type is voltage by default
   torque_controller = TorqueControlType::voltage;
 }
@@ -31,12 +35,6 @@ void BLDCMotor::init() {
   motor_status = FOCMotorStatus::motor_initializing;
   if(monitor_port) monitor_port->println(F("MOT: Init"));
 
-  // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit
-  if( !current_sense && _isset(phase_resistance)) {
-    float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case
-    // use it if it is less then voltage_limit set by the user
-    voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit;
-  }
   // sanity check for the voltage limit configuration
   if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
   // constrain voltage for sensor alignment
@@ -344,11 +342,18 @@ void BLDCMotor::move(float new_target) {
   // set internal target variable
   if(_isset(new_target)) target = new_target;
 
+  // calculate the back-emf voltage if K_bemf available
+  if (_isset(K_bemf)) voltage_bemf = K_bemf*shaft_velocity;
+  // 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;
+
+  // upgrade the current based voltage limit
   switch (controller) {
     case MotionControlType::torque:
       if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
         if(!_isset(phase_resistance))  voltage.q = target;
-        else voltage.q =  target*phase_resistance;
+        else  voltage.q =  target*phase_resistance + voltage_bemf;
+        voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
         voltage.d = 0;
       }else{
         current_sp = target; // if current/foc_current torque control
@@ -368,7 +373,7 @@ void BLDCMotor::move(float new_target) {
       if(torque_controller == TorqueControlType::voltage){
         // use voltage if phase-resistance not provided
         if(!_isset(phase_resistance))  voltage.q = current_sp;
-        else  voltage.q = current_sp*phase_resistance;
+        else  voltage.q =  _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
         voltage.d = 0;
       }
       break;
@@ -381,7 +386,7 @@ void BLDCMotor::move(float new_target) {
       if(torque_controller == TorqueControlType::voltage){
         // use voltage if phase-resistance not provided
         if(!_isset(phase_resistance))  voltage.q = current_sp;
-        else  voltage.q = current_sp*phase_resistance;
+        else  voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
         voltage.d = 0;
       }
       break;
@@ -620,7 +625,8 @@ float BLDCMotor::velocityOpenloop(float target_velocity){
 
   // use voltage limit or current limit
   float Uq = voltage_limit;
-  if(_isset(phase_resistance)) Uq =  current_limit*phase_resistance;
+  if(_isset(phase_resistance)) 
+    Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit);
 
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, pole_pairs));
@@ -655,10 +661,10 @@ float BLDCMotor::angleOpenloop(float target_angle){
     shaft_velocity = 0;
   }
 
-
   // use voltage limit or current limit
   float Uq = voltage_limit;
-  if(_isset(phase_resistance)) Uq =  current_limit*phase_resistance;
+  if(_isset(phase_resistance)) 
+    Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit);
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   // sensor precision: this calculation is OK due to the normalisation
   setPhaseVoltage(Uq,  0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs));

src/BLDCMotor.h

@@ -19,8 +19,9 @@ class BLDCMotor: public FOCMotor
      BLDCMotor class constructor
      @param pp pole pairs number
      @param R  motor phase resistance
+     @param KV  motor KV rating (1/K_bemf)
      */ 
-    BLDCMotor(int pp,  float R = NOT_SET);
+    BLDCMotor(int pp,  float R = NOT_SET, float KV = NOT_SET);
 
     /**
      * Function linking a motor and a foc driver 

src/StepperMotor.cpp

@@ -3,13 +3,16 @@
 // StepperMotor(int pp)
 // - pp            - pole pair number
 // - R             - motor phase resistance
-StepperMotor::StepperMotor(int pp, float _R)
+// - KV            - motor kv rating
+StepperMotor::StepperMotor(int pp, float _R, float _KV)
 : FOCMotor()
 {
   // number od pole pairs
   pole_pairs = pp;
   // save phase resistance number
   phase_resistance = _R;
+  // save back emf constant KV = 1/KV
+  K_bemf = 1.0/_KV;
 
   // torque control type is voltage by default
   // current and foc_current not supported yet
@@ -32,12 +35,6 @@ void StepperMotor::init() {
   motor_status = FOCMotorStatus::motor_initializing;
   if(monitor_port) monitor_port->println(F("MOT: Init"));
 
-  // if set the phase resistance of the motor use current limit to calculate the voltage limit
-  if(_isset(phase_resistance)) {
-    float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case
-    // use it if it is less then voltage_limit set by the user
-    voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit;
-  }
   // sanity check for the voltage limit configuration
   if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
   // constrain voltage for sensor alignment
@@ -283,11 +280,18 @@ void StepperMotor::move(float new_target) {
 
   // set internal target variable
   if(_isset(new_target) ) target = new_target;
+
+  // calculate the back-emf voltage if K_bemf available
+  if (_isset(K_bemf)) voltage_bemf = K_bemf*shaft_velocity;
+  // 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
   switch (controller) {
     case MotionControlType::torque:
       if(!_isset(phase_resistance))  voltage.q = target; // if voltage torque control
-      else voltage.q =  target*phase_resistance;
+      else  voltage.q =  target*phase_resistance + voltage_bemf;
+      voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
       voltage.d = 0;
       break;
     case MotionControlType::angle:
@@ -300,8 +304,8 @@ void StepperMotor::move(float new_target) {
       // if torque controlled through voltage
       // use voltage if phase-resistance not provided
       if(!_isset(phase_resistance))  voltage.q = current_sp;
-      else  voltage.q = current_sp*phase_resistance;
-      voltage.d = 0;
+      else  voltage.q =  _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+        voltage.d = 0;
       break;
     case MotionControlType::velocity:
       // velocity set point
@@ -311,7 +315,7 @@ void StepperMotor::move(float new_target) {
       // if torque controlled through voltage control
       // use voltage if phase-resistance not provided
       if(!_isset(phase_resistance))  voltage.q = current_sp;
-      else  voltage.q = current_sp*phase_resistance;
+      else  voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
       voltage.d = 0;
       break;
     case MotionControlType::velocity_openloop:
@@ -372,7 +376,8 @@ float StepperMotor::velocityOpenloop(float target_velocity){
 
   // use voltage limit or current limit
   float Uq = voltage_limit;
-  if(_isset(phase_resistance)) Uq =  current_limit*phase_resistance;
+  if(_isset(phase_resistance)) 
+    Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit);
 
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, pole_pairs));
@@ -406,7 +411,8 @@ float StepperMotor::angleOpenloop(float target_angle){
 
   // use voltage limit or current limit
   float Uq = voltage_limit;
-  if(_isset(phase_resistance)) Uq =  current_limit*phase_resistance;
+  if(_isset(phase_resistance)) 
+    Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit);
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   setPhaseVoltage(Uq,  0, _electricalAngle((shaft_angle), pole_pairs));
 

src/StepperMotor.h

@@ -24,8 +24,9 @@ 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)
     */
-    StepperMotor(int pp,  float R = NOT_SET);
+    StepperMotor(int pp,  float R = NOT_SET, float KV = NOT_SET);
 
     /**
      * Function linking a motor and a foc driver 

src/common/base_classes/FOCMotor.cpp

@@ -28,6 +28,9 @@ FOCMotor::FOCMotor()
   current_sp = 0;
   current.q = 0;
   current.d = 0;
+
+  // voltage bemf 
+  voltage_bemf = 0;
 
   //monitor_port 
   monitor_port = nullptr;
@@ -105,14 +108,11 @@ void FOCMotor::monitor() {
   }
   // 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);
-      }
+    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) {
       monitor_port->print(c.q*1000, 2); // mAmps

src/common/base_classes/FOCMotor.h

@@ -152,6 +152,7 @@ class FOCMotor
     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
@@ -160,6 +161,7 @@ class FOCMotor
     // motor physical parameters
     float	phase_resistance; //!< motor phase resistance
     int pole_pairs;//!< motor pole pairs number
+    float K_bemf; //!< motor back emf constant (1/KV)
 
     // limiting variables
     float voltage_limit; //!< Voltage limitting variable - global limit