[FEATURE] Foc_current mode and Current feed forward

[Candas1]

At the moment in voltage mode, if information like phase_resistance, phase_inductance and KV_rating are provided, q and d currents are used as target and the q and d voltages are estimated.

The foc_current could benefit from that and use those values as feedforward for the q and d PI controllers.
The q and d voltage estimation could be made a separate function not to duplicate the code.

That would probably improve the dynamics of the foc current control.

[Candas1]

This estimation function can also be reused in angle mode and velocity mode

Now it looks like that:
Vq = phase_resistance * Iq + BEMF
Vd = - SPEED * L * Iq

It could be later updated for field weakening in this way:
Vq = phase_resistance * Iq + BEMF + SPEED * L * Id
Vd = phase_resistance * Id - SPEED * L * Iq

Later, for salient motors like IPMSM, it could be updated to handle Ld and Lq:
Vq = phase_resistance * Iq + BEMF + SPEED * Ld * Id
Vd = phase_resistance * Id - SPEED * Lq * Iq

[Candas1]

I think I am wrong about the feedforward:

Only those values are being added as feedforward term after the PI output:

• Vq += BEMF + SPEED * Ld * Id
• Vd -= SPEED * Lq * Iq
  That could be added before the PID output ramp limiting.

That technique is called decoupling.
Vesc seems to give different options.

• only BEMF decoupling : Vq += BEMF
• cross decoupling : Vq += SPEED * Ld * Id and Vd -= SPEED * Lq * Iq
• both : Vq += BEMF + SPEED * Ld * Id and Vd -= SPEED * Lq * Iq

phase_resistance * Iq and phase_resistance * Id might be already covered by the PI controller.

[Candas1]

So something like that could be done.
In move() function for voltage mode

• if phase_resistance is available, Vq = phase_resistance * Iq and Vd = phase_resistance * Id

In loopfoc() function for all modes:

• if phase_inductance is available Vq += SPEED * Ld * Id
• if KV_rating is available Vq += BEMF
• if phase_inductance is available Vd -= SPEED * Lq * Iq

[Candas1]

Nice video about the cross decoupling:
https://www.youtube.com/watch?v=bOr_EgJUakI

[Candas1]

Nice article, and the benefit we could expect:

[askuric]

Makes perfect sense!
I think that the library has matured enough to have this as a feature. Feedforward addition looks great.

[askuric]

So I've spent some time on this subject recently and I've got a few observations.

1. For FOC current approach the decoupling only has the effect when it comes to rapid transitions, it helps feedforward some of the voltage that is going to be necessary to "find" by the PI controller. But in general steady state does not change and there is no direct influence on in by decoupling. Basically, the d-axis decoupling voltage will be more or less the same one that will be found by the d-axis PI controller that will try to keep the d-axis current equal to zero.
2. For voltage control (when estimating current through the phase resistance, inductance and the BEMF), the decoupling has a direct impact on the steady state. By default this approach always sets the d-axis voltage to 0. By using the decoupling term this is no longer the case and it applies the d-axis voltage which helps to better align the current vector. This works very well.
3. The control diagram that you've shown does not really use BEMF as a decoupling term but the flux likange which is proportional to it, but usully lover. So uisng the full BEMF term as a decoupling term might lead to some negative effects.

So what I would propose is this:

1. We extend the torque control strategies to this:
   • voltage - only voltage | no decoupling
   • estimated_current - current estimated using motor parameters| uses decoupling
   • dc_current - current measured only in q | decoupling if motor params available
   • foc_current - current measured d and q | decoupling if motor params available
2. Add more motor parameters
   • phase_inductance_q and phase_inductance_d
   • flux_linkage
   • Maybe even create a FOCMotorParams structure that can be provided in the constructor
     • pole paris
     • resistance
     • inductance
     • flux linkage
3. The overall loopFOC function could something like this:

// Iterative function looping FOC algorithm, setting Uq on the Motor
// The faster it can be run the better
void BLDCMotor::loopFOC() {
  
  // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
  //                 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();
  switch (torque_controller) {
    case TorqueControlType::voltage:
      // use voltage 
      voltage.q = current_sp;
      // d voltage component is zero
      voltage.d = 0;

      // estimate the current as zero
      current.q = 0;
      current.d = 0;
      break;
    case TorqueControlType::estimated_current:
      if(!_isset(phase_resistance)) return;

      // estimate the current
      current.q = (voltage.q - voltage_bemf)/phase_resistance;
      current.d = 0;

      // 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;

      // set the voltage that corresponds to the current
      voltage.q = current_sp*phase_resistance + voltage_bemf;
      voltage.d = 0;
      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 component is zero
      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);
      break;
    default:
      // no torque control selected
      SIMPLEFOC_DEBUG("MOT: no torque control selected!");
      break;
  }

  // if KV rating is available calculate the back-emf voltage
  if (_isset(KV_rating) && _isset(flux_linkage)){
    float voltage_phase_linkage = shaft_velocity/(KV_rating*_SQRT2)/_RPM_TO_RADS*flux_linkage;
    voltage.q += voltage_phase_linkage;
  }

  // set d-component (lag compensation if known inductance)
  if(_isset(phase_inductance_q)){
    float lag = shaft_velocity*pole_pairs;
    voltage.d -= current.q*lag*phase_inductance_q;
    if (_isset(phase_inductance_d)) voltage.q += current.d*lag*phase_inductance_d;
  }

  // limit the voltage
  voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
  voltage.d = _constrain(voltage.d, -voltage_limit, voltage_limit);

  // set the phase voltage - FOC heart function :)
  setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
}

What do you think?

[agri-pat]

I had a quick play around with your proposed loop function.

With the exception of some terrible current reading issues I'm having, which this helped with to some degree, it worked great. If you add the motor target as an additional voltage feedforward the motor runs quite well with all PID gains set to zero.

Hopefully the end result will be tighter tracking of setpoints as the PID is only needing to adjust for errors outside of the feedforward values.

Pretty impressive. If only I could get the same performance improvements in velocity calculations which are non zero when the motor driver is off 🤣

[Candas1]

What do you think?

I like it but I can't test it for now.

Thanks for the feedback @agri-pat

[dzid26]

@askuric
In voltage control, if you add bemf here voltage.q += voltage_phase_linkage;, then it should not be added second time here voltage.q = current_sp*phase_resistance + voltage_bemf;.

Why is KV multiplied with flux_linkage? KV_rating*_SQRT2)/_RPM_TO_RADS*flux_linkage They are the same kind of thing but just represented differently. I think:

• KV is expressed Vrms measure line to line - this is for convenience because it can be measured using voltmeter attached to two tabs and spinning motor with a drill
• flux linkage - is probably Vpp line to (virtual) neutral - so depending how it's measured and what is motor config (delta/wye) it needs to be converted differently

Please note, I wrote it quickly, so I might be hallucinating this post big time.
https://www.mathworks.com/help/mcb/ref/surfacemountpmsm.html

[askuric]

Hey guys,

Its been a while.
So this feature is now implemented in the dev branch and will be soon in the next release v2.4.0.

Arduino-FOC/src/common/base_classes/FOCMotor.cpp

Lines 602 to 658 in 22e19c7

	switch (torque_controller) {
	case TorqueControlType::voltage:
	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 = estimateBEMF(shaft_velocity);
	// 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_dq.q)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance_dq.q, -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) + feed_forward_voltage.q;
	// d voltage - lag compensation
	if(_isset(phase_inductance_dq.q)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance_dq.q, -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);
	// calculate the phase voltages
	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_dq.q)) voltage.d = _constrain( voltage.d - current.q*shaft_velocity*pole_pairs*phase_inductance_dq.q, -voltage_limit, voltage_limit);
	// q voltage - cross coupling compensation - TODO verify
	if(_isset(phase_inductance_dq.d)) voltage.q = _constrain( voltage.q + current.d*shaft_velocity*pole_pairs*phase_inductance_dq.d, -voltage_limit, voltage_limit);
	break;
	default:
	// no torque control selected
	SIMPLEFOC_MOTOR_ERROR("no torque control selected!");
	break;
	}

We now have:

• current, voltage and velocity feed-forwards
• we have lag compensation in estimated_current, dc_current and foc_current
• we have cross coupling compensation for foc_currents