SimpleFOC with MKS Dual FOC plus + ESP32 and current sense

hardware support
, , , ,
1

Hi, again.
I’m back and still fighting setting up motors.

Makerbase SimpleFOC Dual Plus with an ESP32
and
BL4825O motors (Removed the PCB, 5PP 1.5Ohm)
with
AS5600 magnetic encoders (The white square version)

I’ve tested the encoders, motors and current sense individually.
I’ve also tested them together.
PWM voltage tested and checked ABC phases.
All seems ok until I try to do the init.currentsense() (0.1Ohm and x50 gain INA181A2)

BLDCDriver3PWM driver0 = BLDCDriver3PWM(32, 33, 25);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(26, 27, 14);
InlineCurrentSense current_sense0 = InlineCurrentSense(0.01f, 50.0f, _NC, 39, 36);
InlineCurrentSense current_sense1 = InlineCurrentSense(0.01f, 50.0f, _NC, 35, 34);

I keep getting stuck at the current sense:

motor0.initFOC
MOT: Align sensor.
MOT: sensor_direction==CCW
MOT: PP check: OK!
MOT: Zero elec. angle: 4.67
MOT: Align current sense.
CS: Err B - all currents same magnitude!
MOT: Align error!
MOT: Init FOC failed.

motor1.initFOC
MOT: Align sensor.
MOT: sensor_direction==CCW
MOT: PP check: OK!
MOT: Zero elec. angle: 4.88
MOT: Align current sense.
CS: Err B - all currents same magnitude!
MOT: Align error!
MOT: Init FOC failed.

And once every now and then (playing with the Motor/Driver voltage/current limits I can reach success and the motor turns (sometimes smooth, often vibrating and getting quite hot)

19:07:46.317 -> motor0.initFOC
19:07:46.317 -> MOT: Align sensor.
19:07:48.512 -> MOT: sensor_direction==CCW
19:07:48.512 -> MOT: PP check: OK!
19:07:49.257 -> MOT: Zero elec. angle: 5.06
19:07:49.452 -> MOT: Align current sense.
19:07:51.647 -> CS: Err B - all currents same magnitude!
19:07:51.647 -> MOT: Align error!
19:07:51.647 -> MOT: Init FOC failed.
19:07:51.647 -> 
19:07:51.647 -> motor1.initFOC
19:07:51.647 -> MOT: Align sensor.
19:07:53.841 -> MOT: sensor_direction==CCW
19:07:53.841 -> MOT: PP check: OK!
19:07:54.585 -> MOT: Zero elec. angle: 4.88
19:07:54.779 -> MOT: Align current sense.
19:07:55.876 -> CS: Switch A-B
19:07:56.972 -> CS: Inv C
19:07:56.972 -> MOT: Success: 4
19:07:56.972 -> MOT: Ready.

Full code below (added ifdefs to test one motor at a time or both):

#include <SimpleFOC.h>

#define Enable_M0
#define Enable_M1
#define pinsetting INPUT

#define DriVolLim 3;
#define MotVolSenAli 3;
#define MotVolLim 12;
#define MotCurLim 1;

#ifdef Enable_M0
  MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
  TwoWire I2Cone = TwoWire(0);
#endif
#ifdef Enable_M1
  MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
  TwoWire I2Ctwo = TwoWire(1);
#endif

#ifdef Enable_M0
  BLDCDriver3PWM driver0 = BLDCDriver3PWM(32, 33, 25);
#endif
#ifdef Enable_M1
  BLDCDriver3PWM driver1 = BLDCDriver3PWM(26, 27, 14);
#endif

#ifdef Enable_M0
  InlineCurrentSense current_sense0  = InlineCurrentSense(0.01f, 50.0f, _NC, 39, 36);
#endif
#ifdef Enable_M1
  InlineCurrentSense current_sense1  = InlineCurrentSense(0.01f, 50.0f, _NC, 35, 34);
#endif

#ifdef Enable_M0
  BLDCMotor motor0 = BLDCMotor(5, 1);
#endif
#ifdef Enable_M1
  BLDCMotor motor1 = BLDCMotor(5, 1);
#endif


//instantiate commander
#ifdef Enable_M0
  Commander command0 = Commander(Serial);
  void doMotor0(char* cmd){command0.motor(&motor0, cmd);}
#endif
#ifdef Enable_M1
  Commander command1 = Commander(Serial);
  void doMotor1(char* cmd){command1.motor(&motor1, cmd);}
#endif

void setup() {
  delay(2000);
  // init the serial port
  Serial.begin(115200);
  delay(4000);
  Serial.println();
  Serial.println("Start!");
  delay(1000);

  // enable the debugging output
  SimpleFOCDebug::enable(&Serial);

  
  #ifdef Enable_M0
    I2Cone.begin(19, 18, 400000UL);
  #endif
  #ifdef Enable_M1
    I2Ctwo.begin(23, 5, 400000UL);
  #endif

  Serial.println("Sensor.init");
  #ifdef Enable_M0
    sensor0.init(&I2Cone);
  #endif
  #ifdef Enable_M1
    sensor1.init(&I2Ctwo);
  #endif

  Serial.println("motor.link");
  #ifdef Enable_M0
    motor0.linkSensor(&sensor0);
  #endif
  #ifdef Enable_M1
    motor1.linkSensor(&sensor1);
  #endif

  #ifdef Enable_M0
    //driver0.pwm_frequency = 20000;
    driver0.voltage_power_supply = 12;
    driver0.voltage_limit = DriVolLim;
  #endif
  #ifdef Enable_M1
    //driver1.pwm_frequency = 20000;
    driver1.voltage_power_supply = 12;
    driver1.voltage_limit = DriVolLim;
  #endif
  
  Serial.println("driver.init");
  #ifdef Enable_M0
    driver0.init();
  #endif
  #ifdef Enable_M1
    driver1.init();
  #endif

  Serial.println("motor.link");
  #ifdef Enable_M0
    motor0.linkDriver(&driver0);
  #endif
  #ifdef Enable_M1
    motor1.linkDriver(&driver1);
  #endif

  Serial.println("current.link");
  #ifdef Enable_M0
    current_sense0.linkDriver(&driver0);
  #endif
  #ifdef Enable_M1
    current_sense1.linkDriver(&driver1);
  #endif
  
  #ifdef Enable_M0
    motor0.controller = MotionControlType::velocity;
    motor0.voltage_sensor_align = MotVolSenAli;
    motor0.useMonitoring(Serial);
    motor0.voltage_limit = MotVolLim;
    motor0.current_limit = MotCurLim;
  #endif
  #ifdef Enable_M1
    motor1.controller = MotionControlType::velocity;
    motor1.voltage_sensor_align = MotVolSenAli;
    motor1.useMonitoring(Serial);
    motor1.voltage_limit = MotVolLim;
    motor1.current_limit = MotCurLim;
  #endif

  Serial.println("motor.init");
  #ifdef Enable_M0
    motor0.init();
  #endif
  #ifdef Enable_M1
    motor1.init();
  #endif

  Serial.println("currentsense.init");
  #ifdef Enable_M0
    current_sense0.init();
  #endif
  #ifdef Enable_M1
    current_sense1.init();
  #endif

  Serial.println("");
  #ifdef Enable_M0
    motor0.linkCurrentSense(&current_sense0);
  #endif
  #ifdef Enable_M1
    motor1.linkCurrentSense(&current_sense1);
  #endif
  
  //current_sense0.gain_b *=-1;
  //current_sense1.gain_c *=-1;
  // skip alignment
  //current_sense0.skip_align = true;
  //current_sense1.skip_align = true;

  #ifdef Enable_M0
    Serial.println("");
    Serial.println("motor0.initFOC");
    motor0.initFOC();
  #endif
  #ifdef Enable_M1
    Serial.println("");
    Serial.println("motor1.initFOC");
    motor1.initFOC();
  #endif

  #ifdef Enable_M0
    command0.add('M',doMotor0,"motor0");
  #endif
  #ifdef Enable_M1
    command1.add('M',doMotor1,"motor1");
  #endif

  delay(1000);  
}

void loop() {
  // monitoring function outputting motor variables to the serial terminal 
  //#ifdef Enable_M0
  //motor0.monitor();
  //#endif
  //#ifdef Enable_M1
  //motor1.monitor();
  //#endif
  
  // FOC algorithm function
  #ifdef Enable_M0
  motor0.loopFOC();
  #endif
  #ifdef Enable_M1
  motor1.loopFOC();
  #endif

  // velocity control loop function
  // setting the target velocity to 2rad/s
  #ifdef Enable_M0
  motor0.move(2);
  #endif
  #ifdef Enable_M1
  motor1.move(2);
  #endif

  // significantly slowing the execution down
  //#ifdef Enable_M0
  //#ifdef Enable_M0
  //motor0.monitor();
  //#endif
  //#ifdef Enable_M1
  //motor1.monitor();
  //#endif

  // read user commands
  #ifdef Enable_M0
  command0.run();
  #endif
  #ifdef Enable_M1
  command1.run();
  #endif
  //commander1.run();

  //driver0.setPwm(2,4,6);
  //driver1.setPwm(2,4,6);
}

I’m lost, I’ve been at this for a couple of weeks. Any ideas?

Thanks :green_heart:

2

Have you tried with current_sense.skip_align = true? Reading through the code around that error message in CurrentSense.cpp, it looks like it will be a pain to debug exactly what’s going on, and in most cases you don’t need the alignment procedure anyway.