Setting current _sense.gain_b *= -1 breaks FOC_current control on a FOCShield v2.0.4 with an ESP32?

I am controlling a GM3506 motor with a SimpleFOCShield v2.0.4 and an ESP32

I have modified the current_control example from motion_control → torque_control → encoder. My board uses the AS5147 magnetic sensor but the magnetic_sensor folder did not have a current_control example so I used the one from encoder and changed it to use the AS5147.

The code also implements the note from the Inline Current Sense chapter that mentions to initialize and link the current sensor after the motor has been initialized.

The full code looks like this:

 * Torque control example using current control loop.
 * Modified to use the AS5147_SPI position sensor and 
 * incorporated the feedback from:
#include <SimpleFOC.h>

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 17);

// magnetic sensor instance - SPI
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 5);

// current sensor
InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, 34, 35);

// current set point variable
float target_current = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_current, cmd); }

void setup() {

  // initialize sensor hardware
  // link the motor to the sensor

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  // link driver
  // link current sense and the driver

  // set torque mode:
  motor.torque_controller = TorqueControlType::foc_current;
  // set motion control loop to be used
  motor.controller = MotionControlType::torque;

  // foc currnet control parameters (stm/esp/due/teensy)
  motor.PID_current_q.P = 5;
  motor.PID_current_q.I= 1000;
  motor.PID_current_d.P= 5;
  motor.PID_current_d.I = 1000;
  motor.LPF_current_q.Tf = 0.002f; // 1ms default
  motor.LPF_current_d.Tf = 0.002f; // 1ms default

  motor.voltage_limit = 12;
  motor.velocity_limit = 20; // rad/s - default 20

  // These values came from the find_sensor_offset_and_direction utility function
  // These zero_electrical_angle is always a bit different when running the utility function, but I think that's ok as long as
  // data is collected and replayed using the exact same value.
  motor.sensor_direction = Direction::CW;
  motor.zero_electric_angle = 3.3755;

  // use monitoring with serial
  // comment out if not needed
  motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D | _MON_TARGET;
  motor.monitor_downsample = 100; // default 10

  // initialize motor

  // current sense init hardware

  //current_sense.gain_b *=-1;

  // link the current sense to the motor

  // align sensor and start FOC

  // add target command T
  command.add('T', doTarget, "target current");

  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target current using serial terminal:"));

void loop() {

  // main FOC algorithm function
  // the faster you run this function the better
  // Arduino UNO loop  ~1kHz
  // Bluepill loop ~10kHz

  // Motion control function
  // velocity, position or torque (defined in motor.controller)
  // this function can be run at much lower frequency than loopFOC() function
  // You can also use motor.move() and set the in the code


  // user communication;

The issue is with the

current_sense.gain_b *=-1;

If I leave that line in as per the wiki, and this thread. The motor moves in very erratic motions.

Using motor.monitor() to print _MON_CURR_Q | _MON_CURR_D | _MON_TARGET at 100 downsampling shows the following currents, even with the target set to 0:

0.0000	28.4857	-5.9302
0.0000	-2427.9917	90.9723
0.0000	-244.3403	289.7103
0.0000	-137.3981	346.5026
0.0000	98.0400	40.6432

With this line commented out (i.e. gain_b on default) the motor.monitor() looks like this:

0.0000	-2.0771	2.1727
0.0000	3.2162	-1.0990
0.0000	2.6143	-0.4463
0.0000	-4.7762	2.0652
0.0000	3.6826	-1.1429

with target set to 0.035 it looks like this:

0.0350	35.4835	1.1050
0.0350	34.4964	1.0065
0.0350	32.5896	-0.2117
0.0350	38.8085	-3.5964
0.0350	30.0751	0.6035

Q and D currents are much more reasonable and track the desired values very closely when the current_sense.gain_b is not inverted.

I can leave the current_sense.gain_b untouched, but I am trying to get a good reading of the Q current because I am trying to implement the anticogging algorithm by Piccoli et al. I am wondering now which current_sense.gain_b to use for the data acquisition stage.

I think the issue may have been simply caused by the PID gains being too high, though I am not sure why this worked with the non-inverted gain_b.

Hey @InfiniteArray,

So as we already discussed the shield has the b gain inverted in hardware. So in the docs I’ve tried made sure to point out at as many places as possible to invert the gain_b due to this reason. However, using the SimpleFOClibrary this is not technically necessary as the SimpleFOC by default does the calibration and alignment of the current sense and the driver that might include the changing the gain polarity if necessary and changing the order of the analog pins if they do not correspond to the ones of the driver. Here are the docs about it (although very short ones :smile: ).

This alignment procedure will be executed wether you do the inversion current_sense.gain_b*=-1 or not. If you want to avoid the current sense alignement you can set the falg:


So I suppose that in your case the align fails when the gain is inverted already and if you do not invert the gain the align executes correctly. I am not sure why this would happen, but it might have something to do with the esp32 ADC which is not the best, or even just some bug in the alignment procedure.

So what you can try to verify this theory is to invert the gain and avoid the align procedure before the call of the motor.initFOC().


This should pass.
In theory if you remove the line current_sense.skip_align=true; or both lines, SimpleFOC library should be able to autodetect (if the alignment is not disabled) the gain issue and all of them should lead to the same result.

Could you please share the library output in the serial terminal for some of these cases?
This might help determine what’s causing the issue.