Inline current sensing and velocity measurments don't match reality

[OiD-W]

Hi again!

I’m going nowhere with the PID tunning and went back to the basics.

I’ve noticed that the velocity value when idle is not zero, it jumps around slowly to some random value. This is not an issue when testing the sensors separately with simplefoc’s AS5600 sensor test

I’ve also seen the current is not what is measured either, graph shows 1A but power supply is around 30ma:

10mOhm shunt with INA240A2 (gain of 50V/V), simplefoc’s inline test also shows odd results..
InlineCurrentSense current_sense0 = InlineCurrentSense(0.01f, 50.0f, 39, 36, _NC);

Any input on how to tackle these issues would be appreciated, thanks!

[OiD-W]

MKS FOC DUAL PLUS v3.3 Inline current sense with INA240A2
AS5600 I2C 400kHz (tried faster and slower
750KV WYE 0.22Ohms (0.182Ohms 4 wire measurement) 0,031mH motors.

I’m getting inconsistent Pole Pair checks
Inconsistent current sense alignments (switch phases, invert them, all currents same magnitude…)
When in the webcontroller i set the target to zero in all motion and torque modes.
When setting the KV and induction parameters to 0 for the phase resistance testing the motor starts spinning with full current and horrendous noise. Setting PID’s to zero does nothing to stop the motor.

I’m really starting to run out of ideas and motivation.

#include <SimpleFOC.h>

#define PowerSupplyVoltage 12;
#define DriVolLim 2;
#define MotVolSenAli 1;
#define MotVolLim 12;
#define MotCurLim 1;
#define Skip_current_align false;
char motor0_id = 'L';
char motor1_id = 'R';
  
BLDCMotor motor0 = BLDCMotor(7, 0.182, 750, 0.03, 0.03);
BLDCMotor motor1 = BLDCMotor(7, 0.182, 750, 0.03, 0.03);

BLDCDriver3PWM driver0 = BLDCDriver3PWM(32, 33, 25);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(26, 27, 14);

MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);

InlineCurrentSense current_sense0  = InlineCurrentSense(0.01f, 50.0f, 39, 36, _NC);
InlineCurrentSense current_sense1  = InlineCurrentSense(0.01f, 50.0f, 35, 34, _NC);

Commander command0 = Commander(Serial);
  
void doMotor0(char* cmd){command0.motor(&motor0, cmd);}
void doMotor1(char* cmd){command0.motor(&motor1, cmd);}
  
void setup() {
  Serial.begin(115200);
  delay(2000);
  Serial.println();
  Serial.println("Start!");
  Serial.println("Starting Serial");
  SimpleFOCDebug::enable(&Serial);

  Wire.setClock(400000);
  Wire.begin(19, 18, (uint32_t)400000);
  Wire1.setClock(400000);
  Wire1.begin(23, 5, (uint32_t)400000);
  
  sensor0.init(&Wire);
  sensor1.init(&Wire1);

  motor0.linkSensor(&sensor0);
  motor1.linkSensor(&sensor1);

  driver0.pwm_frequency = 15000;
  driver0.voltage_power_supply = PowerSupplyVoltage;
  driver0.voltage_limit = DriVolLim;
  driver1.pwm_frequency = 15000;
  driver1.voltage_power_supply = PowerSupplyVoltage;
  driver1.voltage_limit = DriVolLim;

  driver0.init();
  driver1.init();

  motor0.linkDriver(&driver0);
  motor1.linkDriver(&driver1);

  current_sense0.linkDriver(&driver0);
  current_sense1.linkDriver(&driver1);
  
  motor0.torque_controller = TorqueControlType::estimated_current;
  motor0.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor0.voltage_sensor_align = MotVolSenAli;
  motor0.voltage_limit = MotVolLim;
  motor0.current_limit = MotCurLim;
  motor0.useMonitoring(Serial);
  motor0.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE | _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE;
  motor0.monitor_downsample = 10;
  motor0.monitor_start_char = motor0_id;
  motor0.monitor_end_char = motor0_id;
  
  motor1.torque_controller = TorqueControlType::estimated_current;
  motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor1.voltage_sensor_align = MotVolSenAli;
  motor1.voltage_limit = MotVolLim;
  motor1.current_limit = MotCurLim;
  motor1.useMonitoring(Serial);
  motor1.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE | _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE;
  motor1.monitor_downsample = 10;
  motor1.monitor_start_char = motor1_id;
  motor1.monitor_end_char = motor1_id;
  
  command0.verbose = VerboseMode::machine_readable;
  
  motor0.init();
  motor1.init();
  
  current_sense0.init();
  current_sense1.init();

  motor0.linkCurrentSense(&current_sense0);
  motor1.linkCurrentSense(&current_sense1);

  current_sense0.gain_a *=-1;
  current_sense0.gain_b *=-1;
  current_sense1.gain_a *=-1;
  current_sense1.gain_b *=-1;
  
  current_sense0.skip_align = Skip_current_align;
  motor0.initFOC();
  
  current_sense1.skip_align = Skip_current_align;
  motor1.initFOC();
  
  command0.add(motor0_id,doMotor0,"motor 0");
  command0.add(motor1_id,doMotor0,"motor 1");
  
  motor0.loopFOC();
  motor0.move(0);
  motor0.monitor();
  motor0.disable();
  
  motor1.loopFOC();
  motor1.move(0);
  motor1.monitor();
  motor1.disable();

  Serial.println("Starting main loop");
}

void loop() {
  motor0.loopFOC();
  motor0.move();
  motor0.monitor();

  motor1.loopFOC();
  motor1.move();
  motor1.monitor();

  command0.run();
}

Start!
Starting Serial
ESP32-DRV: Configuring 3PWM in group: 0 on timer: 0
ESP32-DRV: Configuring 2 operators.
ESP32-DRV: Configuring 3 comparators.
ESP32-DRV: Configuring 3 generators.
ESP32-DRV: Configuring center-aligned pwm.
ESP32-DRV: Enabling timer: 0
ESP32-DRV: MCPWM configured!
ESP32-DRV: Configuring 3PWM in group: 1 on timer: 0
ESP32-DRV: Configuring 2 operators.
ESP32-DRV: Configuring 3 comparators.
ESP32-DRV: Configuring 3 generators.
ESP32-DRV: Configuring center-aligned pwm.
ESP32-DRV: Enabling timer: 0
ESP32-DRV: MCPWM configured!
MOT:Monitor enabled!
MOT:Monitor enabled!
MOT:Init
MOT:Enable driver.
MOT:Init
MOT:Enable driver.
ESP32-CS: Configuring ADC1 channel 3
ESP32-CS: Configuring fast ADCs
ESP32-CS: Configuring ADC1 channel 0
ESP32-CS: Configuring fast ADCs
ESP32-CS: Configuring ADC1 channel 7
ESP32-CS: Configuring fast ADCs
ESP32-CS: Configuring ADC1 channel 6
ESP32-CS: Configuring fast ADCs
MOT:Align sensor.
MOT:sensor dir: CW
MOT:PP check: OK!
Skip dir calib.
MOT:Zero elec. angle: 2.14
MOT:Align current sense.
CS: Switch B-(C)NC
CS: Inv C
MOT:Success: 4
MOT:Ready.
MOT:Align sensor.
MOT:sensor dir: CCW
MOT:PP check: OK!
Skip dir calib.
MOT:Zero elec. angle: 2.18
MOT:Align current sense.
CS: Switch A-(C)NC
CS: Err B - all currents same magnitude!
ERR-MOT:Align error!
ERR-MOT:Init FOC fail
Starting main loop
L0.0000	0.0000	0.0000	1.5355L
R0.0000	0.0000	0.0024	-4.8044R
L0.0000	0.0000	0.0000	1.5355L
R0.0000	0.0000	-0.0858	-4.8060R

[OiD-W]

Just for a sanity check I tested the current sense separately with the following code:

void loop() {
  //Shunt is 0.01R and gain is 50. 1A=0.01V=>0.5V/A
  //3.3V/50=0.066=6.6A=> +-3.3A
  //0A average is 1910, offset=137
  Serial.print(map(analogRead(34)+137,0,4095,-3300,3300));
  Serial.print("\t");
  Serial.print(map(analogRead(35)+137,0,4095,-3300,3300));
  Serial.print("\t");
  Serial.print(map(analogRead(36)+137,0,4095,-3300,3300));
  Serial.print("\t");
  Serial.println(map(analogRead(39)+137,0,4095,-3300,3300));
  delay(250);
}

Perfect measurements on all 4 channels.

I’ve also taken note of suggestions 2 and 3 from Runger’s comment;

Motor stuttering at low and high speed with openloop

Hey @MORDRET_Pierre-Yves, Welcome to SimpleFOC! A few points I note: The motors you link, in my experience, have around 5-10ohm phase resistance. Not 100mOhm like you write. Did you measure them? But in your sample code you’re not using the resistance value, so it should not matter. You’re using the same voltage limit on motor and driver, which will lead to a lot of overmodulation. Please try with motor.voltage_limit = driver.voltage_limit/2.0f; Please remove the KV value from the mo…

Everything works separately, driver, encoder, current sense…