PlatformIO B-G431-ESC1 Code not working

Hello,

I am trying to program the B-G431-ESC1 through PlatformIO (VSCode) and I am getting the below error

** Programming Started **
Warn : Adding extra erase range, 0x080098c0 .. 0x08009fff
** Programming Finished **
** Verify Started **
** Verified OK **
** Resetting Target **
shutdown command invoked

This is how my platform.io file looks

[env:disco_b_g431b_esc1]
platform = ststm32
board = disco_b_g431b_esc1
framework = arduino
monitor_speed = 115200

build_flags =
    -D PIO_FRAMEWORK_ARDUINO_ENABLE_CDC
    -D PIO_FRAMEWORK_ARDUINO_NANOLIB_FLOAT_PRINTF
    
lib_archive = false

lib_deps=
    askuric/Simple FOC @ ^2.0.2
    SPI
    Wire 

I removed all the code from main.cpp just to check if something is wrong in the code but even with basic code I get the same error. Here is how the main.cpp looks

#include <Arduino.h>

void setup() {
  // put your setup code here, to run once:
  // use monitoring with serial 
  Serial.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:
  Serial.println("Motor ready.");
}

Can someone please help, I am unable to figure out what is going wrong

This looks normal. Before printing to serial, wait 3s in setup(). I am not sure if the board settings do it by default now, but before you needed to platformio to use Serial2 as the default for Serial. Try printing directly to Serial2 instead of the generic Serial, since that is used by the board.

… one day I’ll publish a post which collects all the information on that board in one place.

1 Like

This warning will appear every time, no matter what. Don’t worry about it.

Try again with this commented out.

Try, like @Grizzly said

Serial.begin(115200); 
delay(3000);

This gives Serial Monitor on your pc/ide enough time to get going before the mcu prints to it.

Hi @Kacko @Grizzly

Thank you for your response. I changed Serial to Serial2 and added a delay of 3seconds and I could see the statement getting printed. Thanks for the input.

I am now trying to run the example:- B-G431b-ESC1.ino
I have updated the following parameters in the example
No of pole pairs of my motor is 7
The power supply voltage is 24V

Below is the code that I am using

/** 
 * B-G431B-ESC1 position motion control example with encoder
 *
 */
#include <SimpleFOC.h>

// Motor instance
BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);


// encoder instance
Encoder encoder = Encoder(A_HALL2, A_HALL3, 2048, A_HALL1);

// Interrupt routine intialisation
// channel A and B callbacks
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}
void doIndex(){encoder.handleIndex();}

// instantiate the commander
Commander command = Commander(Serial2);
void doTarget(char* cmd) { command.motion(&motor, cmd); }

void setup() {
  
  // initialize encoder sensor hardware
  encoder.init();
  encoder.enableInterrupts(doA, doB); 

  // link the motor to the sensor
  motor.linkSensor(&encoder);
  
  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);
  // link current sense and the driver
  currentSense.linkDriver(&driver);

  // current sensing
  currentSense.init();
  // no need for aligning
  currentSense.skip_align = true;
  motor.linkCurrentSense(&currentSense);

  // aligning voltage [V]
  motor.voltage_sensor_align = 3;
  // index search velocity [rad/s]
  motor.velocity_index_search = 3;

  // set motion control loop to be used
  motor.controller = MotionControlType::velocity;

  // contoller configuration 
  // default parameters in defaults.h

  // velocity PI controller parameters
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 20;
  // default voltage_power_supply
  motor.voltage_limit = 6;
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;
 
  // velocity low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01;

  // angle P controller
  motor.P_angle.P = 20;
  //  maximal velocity of the position control
  motor.velocity_limit = 4;


  // use monitoring with serial 
  Serial2.begin(115200);
  // comment out if not needed
  motor.useMonitoring(Serial2);
  
  // initialize motor
  motor.init();
  // align encoder and start FOC
  motor.initFOC();

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

  Serial2.println(F("Motor ready."));
  Serial2.println(F("Set the target angle using serial terminal:"));
  _delay(3000);
}

void loop() {
  // main FOC algorithm function

  // Motion control function
  motor.move();

  // function intended to be used with serial plotter to monitor motor variables
  // significantly slowing the execution down!!!!
  // motor.monitor();
  
  // user communication
  command.run();
}

The motor does not rotate and I am getting the following error

MOT: Error: Not found!
MOT: Init FOC failed.
Motor ready.
Set the target angle using serial terminal:

I’m not sure this is the correct close loop code, you may be missing

motor.loopFOC();

as part of the main loop.

Could you please read more carefully what others have done?

Cheers,
Valentine

Hi @Valentine, I used the same code that is provided in the example. I also tried with the change you mentioned
motor.loopFOC();
but that did not help

Now just to go step by step I followed the youtube video tutorial (SimpleFOC 2.0 Tuning Guide - Part 1 - YouTube) , I get the angle and velocity readings proper but the part where he rotates the motor in open loop that does not rotate the motor at my end

here is the code I used after following the video

#include <SimpleFOC.h>

BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);

HallSensor sensor = HallSensor(A_HALL1, A_HALL2, A_HALL3,7);

void doA(){sensor.handleA();}
void doB(){sensor.handleB();}
void doC(){sensor.handleC();}
 
float target = 1.0; //[Volt]

void serialLoop(){
  static String received_chars;
  
  while(Serial2.available()){
    char inChar = (char) Serial2.read();
    received_chars += inChar;
    if(inChar == '\n'){
      target = received_chars.toFloat();
      Serial2.print("Target = "); Serial2.println(target);
      received_chars = "";
    }
  }
}

void setup() {
  Serial2.begin(115200);
  delay(3000);
  // initialise encoder hardware
  sensor.init();
  // hardware interrupt enable
  sensor.enableInterrupts(doA, doB, doC);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  // limit the maximal dc voltage the driver can set
  // as a protection measure for the low-resistance motors
  // this value is fixed on startup
  // driver.voltage_limit = 6;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

  // limiting motor movements
  // limit the voltage to be set to the motor
  // start very low for high resistance motors
  // currnet = resistance*voltage, so try to be well under 1Amp
  motor.voltage_limit = 1;   // [V]
  motor.velocity_limit = 20;
  // open loop control config
  motor.controller = MotionControlType::velocity_openloop;

  // init motor hardware
  motor.init();

  Serial2.println("setup");

}

void loop() {
  serialLoop();

  motor.move(target);

// display the angle and the angular velocity to the terminal
  Serial2.print(sensor.getAngle());
  Serial2.print("\t");
  Serial2.println(sensor.getVelocity());
  
}

Avoid printing to serial at every loop iteration. It will slow everything down and your loop frequency will be too low to move the motor properly.

… and loopFOC() is missing too, as @Valentine already mentioned.

/Chris

@Valentine @Grizzly @Kacko thank you so much for your help guys. This community is really helpful.

So the issue with my code was in Platformio.ini file I was trying different things and hence had commented out the below line
lib_archive = false
I uncommented it and the motor starts to rotate. What does that line actually do?

1 Like

I would love to know what that line does, too!! Crazy how it makes or breaks a program.

@Valentine @Grizzly @Kacko
So the

  1. Hall sensor code works properly I can see the angle and velocity readings however for angles I only see 4 values (15.00,30.00,-15.00,-30.00) Is this correct?
  2. Open loop velocity control works properly

Now I am trying to implement closed-loop FOC but I get the following error. I am tuning the P and I parameters but the motor rotates 90 degrees clockwise and then anti-clockwise and then I get the “Failed to notice movement” error and nothing happens even if i set the target parameter through serial terminal

MOT: Monitor enabled!
MOT: Init
MOT: Enable driver.
MOT: Align sensor.
MOT: Failed to notice movement
MOT: Init FOC failed.
setup

This is the code

#include <SimpleFOC.h>

BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);

HallSensor sensor = HallSensor(A_HALL1, A_HALL2, A_HALL3,7);

void doA(){sensor.handleA();}
void doB(){sensor.handleB();}
void doC(){sensor.handleC();}
 
float target = 0.0; //[Volt]

void serialLoop(){
  static String received_chars;
  
  while(Serial2.available()){
    char inChar = (char) Serial2.read();
    received_chars += inChar;
    if(inChar == '\n'){
      target = received_chars.toFloat();
      Serial2.print("Target = "); Serial2.println(target);
      received_chars = "";
    }
  }
}

void setup() {
  Serial2.begin(115200);
  delay(3000);
  // initialise encoder hardware
  sensor.init();
  // hardware interrupt enable
  sensor.enableInterrupts(doA, doB, doC);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  // limit the maximal dc voltage the driver can set
  // as a protection measure for the low-resistance motors
  // this value is fixed on startup
  // driver.voltage_limit = 6;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

  // limiting motor movements
  // limit the voltage to be set to the motor
  // start very low for high resistance motors
  // currnet = resistance*voltage, so try to be well under 1Amp
  motor.voltage_limit = 2;   // [V]
  motor.velocity_limit = 20;
  motor.voltage_sensor_align = 1;

  motor.linkSensor(&sensor);

  motor.PID_velocity.P = 0.0;
  motor.PID_velocity.I = 0.0;
  motor.LPF_velocity.Tf = 0.01;
  // open loop control config
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::torque;
  motor.useMonitoring(Serial2);

  // init motor hardware
  motor.init();
  motor.initFOC();
  Serial2.println("setup");

}

void loop() {
  serialLoop();

  motor.PID_velocity.P = target;

  motor.loopFOC();
  motor.move(2.0);
  // motor.monitor();
// display the angle and the angular velocity to the terminal
  // Serial2.print(sensor.getAngle());
  // Serial2.print("\t");
  // Serial2.println(sensor.getVelocity());
  
}

I am not sure what is wrong, I checked multiple resources but could not find a solution to this.

Hey check out one of the sketches in my B-G431B-ESC1 repo. Maybe you’ll get some inspiration.

@Valentine @Kacko @Grizzly Sharing the fix here so it might help others. So in closed loop I was always getting the below error, I tried multiple examples but the result was same.

MOT: Monitor enabled!
MOT: Init
MOT: Enable driver.
MOT: Align sensor.
MOT: Failed to notice movement
MOT: Init FOC failed.
setup

So I went ahead and changed the hall sensor combinations in the code and for this combination It started working

HallSensor sensor = HallSensor(A_HALL2, A_HALL3  , A_HALL1 ,7);

It makes the compiler behave differently, and then it can find the hardware specific driver code… we use something called weakly bound functions to have alternative implementations for each MCU type.

Normally the linker should select the non-weak binding over the weak one, but on PlatformIO for so,e reason it doesn’t unless you set this option…

1 Like

I’m taking a course in embedded design right now (STM32 ) and I’ve seen this somewhere. I’ll pay close attention when they go over it. Thanks for the insight.

That probably means that you switched the connections to the hall sensors e.g. soldered the wire of Hall u to the pad for hall sensor v etc.