added the estimated curret torque control mode

Author: askuric

src/BLDCMotor.cpp

@@ -100,25 +100,6 @@ class BLDCMotor: public FOCMotor
     int alignCurrentSense();
     /** Motor and sensor alignment to the sensors absolute 0 angle  */
     int absoluteZeroSearch();
-
-        
-    // Open loop motion control    
-    /**
-     * Function (iterative) generating open loop movement for target velocity
-     * it uses voltage_limit variable
-     * 
-     * @param target_velocity - rad/s
-     */
-    float velocityOpenloop(float target_velocity);
-    /**
-     * Function (iterative) generating open loop movement towards the target angle
-     * it uses voltage_limit and velocity_limit variables
-     * 
-     * @param target_angle - rad
-     */
-    float angleOpenloop(float target_angle);
-    // open loop variables
-    long open_loop_timestamp;
 };
 
 

src/BLDCMotor.h

@@ -15,7 +15,7 @@ HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _induc
   phase_resistance = _R;
   // save back emf constant KV = 1/K_bemf
   // usually used rms
-  KV_rating = _KV * _SQRT2;
+  KV_rating = _KV;
   // save phase inductance
   phase_inductance = _inductance;
 
@@ -52,17 +52,10 @@ int HybridStepperMotor::init()
   if (voltage_sensor_align > voltage_limit)
     voltage_sensor_align = voltage_limit;
 
-  // update the controller limits
-  if (_isset(phase_resistance))
-  {
-    // velocity control loop controls current
-    PID_velocity.limit = current_limit;
-  }
-  else
-  {
-    PID_velocity.limit = voltage_limit;
-  }
-  P_angle.limit = velocity_limit;
+  // update limits in the motor controllers
+  updateCurrentLimit(current_limit);
+  updateVoltageLimit(voltage_limit);
+  updateVelocityLimit(velocity_limit);
 
   // if using open loop control, set a CW as the default direction if not already set
   // only if no sensor is used
@@ -320,42 +313,66 @@ void HybridStepperMotor::loopFOC() {
   //                 of full rotations otherwise.
   if (sensor) sensor->update();
 
-  // if open-loop do nothing
-  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
-
   // if disabled do nothing
   if(!enabled) return;
 
-  // 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();
+  // if open-loop do nothing
+  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) 
+    // calculate the open loop electirical angle
+    electrical_angle = _electricalAngle((shaft_angle), pole_pairs);
+  else
+    // 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
+      voltage.q = _constrain(current_sp, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
+      voltage.d = feed_forward_voltage.d;
+      break;
+    case TorqueControlType::estimated_current:
+      if(! _isset(phase_resistance)) return; 
+      // constrain current setpoint
+      current_sp = _constrain(current_sp, -current_limit, current_limit)  + feed_forward_current.q; // desired current is the setpoint
+      // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
+      if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT2)/_RPM_TO_RADS;
+      // filter the value values
+      current.q = LPF_current_q(current_sp);
+      // calculate the phase voltage
+      voltage.q = current.q * phase_resistance + voltage_bemf;
+      // constrain voltage within limits
+      voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit) + feed_forward_voltage.q;
+      // d voltage  - lag compensation
+      if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
+      else voltage.d = feed_forward_voltage.d;
       break;
     case TorqueControlType::dc_current:
       if(!current_sense) return;
+      // constrain current setpoint
+      current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
       // 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);
+      voltage.q = PID_current_q(current_sp - current.q) + feed_forward_voltage.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;
+      if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit) + feed_forward_voltage.d;
+      else voltage.d = feed_forward_voltage.d;
       break;
     case TorqueControlType::foc_current:
       if(!current_sense) return;
+      // constrain current setpoint
+      current_sp = _constrain(current_sp, -current_limit, current_limit) + feed_forward_current.q;
       // 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);
+      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);
+      voltage.q = PID_current_q(current_sp - current.q) + feed_forward_voltage.q;
+      voltage.d = PID_current_d(feed_forward_current.d - current.d) + feed_forward_voltage.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;
@@ -382,40 +399,31 @@ void HybridStepperMotor::move(float new_target) {
   if(motion_cnt++ < motion_downsample) return;
   motion_cnt = 0;
 
+  // read value even if motor is disabled to keep the monitoring updated
+  // except for the open loop modes where the values are updated within angle/velocityOpenLoop functions
+  
   // shaft angle/velocity need the update() to be called first
   // get shaft angle
   // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
   //                        For this reason it is NOT precise when the angles become large.
   //                        Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
   //                        when switching to a 2-component representation.
-  if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) 
-    shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
-  // get angular velocity 
-  shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
+  if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ){
+    // read the values only if the motor is not in open loop
+    // because in open loop the shaft angle/velocity is updated within angle/velocityOpenLoop functions
+    shaft_angle = shaftAngle(); 
+    shaft_velocity = shaftVelocity(); 
+  }
 
   // if disabled do nothing
   if(!enabled) return;
+  
 
-
-  // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
-  if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS;
-  // 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
+  // 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 + 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;
+        current_sp =  target;
+        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.
@@ -424,48 +432,34 @@ void HybridStepperMotor::move(float new_target) {
       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);
+      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
-      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 - 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
-      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 - 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;
+      // this function updates the shaft_angle and shaft_velocity
+      // returns the voltage or current that is to be set to the motor (depending on torque control mode)
+      // returned values correspond to the voltage_limit and current_limit
+      current_sp = velocityOpenloop(shaft_velocity_sp); 
       break;
     case MotionControlType::angle_openloop:
       // 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;
+      // this function updates the shaft_angle and shaft_velocity
+      // returns the voltage or current that is to be set to the motor (depending on torque control mode)
+      // returned values correspond to the voltage_limit and current_limit
+      current_sp = angleOpenloop(shaft_angle_sp); 
       break;
   }
 }
@@ -513,82 +507,3 @@ void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el)
   driver->setPwm(Ua, Ub, Uc);
 
 }
-
-// Function (iterative) generating open loop movement for target velocity
-// - target_velocity - rad/s
-// it uses voltage_limit variable
-float HybridStepperMotor::velocityOpenloop(float target_velocity)
-{
-  // get current timestamp
-  unsigned long now_us = _micros();
-  // calculate the sample time from last call
-  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
-  // quick fix for strange cases (micros overflow + timestamp not defined)
-  if (Ts <= 0 || Ts > 0.5f)
-    Ts = 1e-3f;
-
-  // calculate the necessary angle to achieve target velocity
-  shaft_angle = _normalizeAngle(shaft_angle + target_velocity * Ts);
-  // for display purposes
-  shaft_velocity = target_velocity;
-
-  // use voltage limit or current limit
-  float Uq = voltage_limit;
-  if (_isset(phase_resistance))
-  {
-    Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit);
-    // recalculate the current
-    current.q = (Uq - fabs(voltage_bemf)) / phase_resistance;
-  }
-
-  // set the maximal allowed voltage (voltage_limit) with the necessary angle
-  setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs));
-
-  // save timestamp for next call
-  open_loop_timestamp = now_us;
-
-  return Uq;
-}
-
-// Function (iterative) generating open loop movement towards the target angle
-// - target_angle - rad
-// it uses voltage_limit and velocity_limit variables
-float HybridStepperMotor::angleOpenloop(float target_angle)
-{
-  // get current timestamp
-  unsigned long now_us = _micros();
-  // calculate the sample time from last call
-  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
-  // quick fix for strange cases (micros overflow + timestamp not defined)
-  if (Ts <= 0 || Ts > 0.5f)
-    Ts = 1e-3f;
-
-  // calculate the necessary angle to move from current position towards target angle
-  // with maximal velocity (velocity_limit)
-  if (abs(target_angle - shaft_angle) > abs(velocity_limit * Ts))
-  {
-    shaft_angle += _sign(target_angle - shaft_angle) * abs(velocity_limit) * Ts;
-    shaft_velocity = velocity_limit;
-  }
-  else
-  {
-    shaft_angle = target_angle;
-    shaft_velocity = 0;
-  }
-
-  // use voltage limit or current limit
-  float Uq = voltage_limit;
-  if (_isset(phase_resistance))
-  {
-    Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit);
-    // recalculate the current
-    current.q = (Uq - fabs(voltage_bemf)) / phase_resistance;
-  }
-  // set the maximal allowed voltage (voltage_limit) with the necessary angle
-  setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs));
-
-  // save timestamp for next call
-  open_loop_timestamp = now_us;
-
-  return Uq;
-}

src/HybridStepperMotor.cpp

@@ -84,30 +84,23 @@ class HybridStepperMotor : public FOCMotor
      */
     void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
 
+    /**
+     * Measure resistance and inductance of a StepperMotor and print results to debug.
+     * If a sensor is available, an estimate of zero electric angle will be reported too.
+     * TODO: determine the correction factor
+     * @param voltage The voltage applied to the motor
+     * @returns 0 for success, >0 for failure
+     */
+    int characteriseMotor(float voltage){
+      return FOCMotor::characteriseMotor(voltage, 1.0f);
+    }
+    
 private:
     int alignCurrentSense();
     /** Sensor alignment to electrical 0 angle of the motor */
     int alignSensor();
     /** Motor and sensor alignment to the sensors absolute 0 angle  */
     int absoluteZeroSearch();
-
-    // Open loop motion control
-    /**
-     * Function (iterative) generating open loop movement for target velocity
-     * it uses voltage_limit variable
-     *
-     * @param target_velocity - rad/s
-     */
-    float velocityOpenloop(float target_velocity);
-    /**
-     * Function (iterative) generating open loop movement towards the target angle
-     * it uses voltage_limit and velocity_limit variables
-     *
-     * @param target_angle - rad
-     */
-    float angleOpenloop(float target_angle);
-    // open loop variables
-    long open_loop_timestamp;
 };
 
 #endif