Code adaptation for storm32 BGC v1.31 - 3 x BLDC motors with SimpleFOClibrary

[runger]

@Richard

The Steval-GMBL I messed with for a bit, but could only get desired results of blinking the LED using HAL, so I tried remapping the pins in the St Nucleo F303RE files without success. Not sure I want to head down the rabbit hole of writing everything for FOC using HAL just yet.

That’s not needed, you can get it working using the generic F303RE or possibly the nucleo 303RE board config… unfortunately I can’t tell you anymore because it seems ArduinoIDE does not remember this information from when I last ran these sketches.

I have the following in my sketch which may help you, but its been a long time since I ran it so YMMV:

#include "arduino.h"
#include "Wire.h"
#include "SPI.h"
#include <SimpleFOC.h>
#include "SimpleFOCDrivers.h"
#include "encoders/as5048a/MagneticSensorAS5048A.h"


#define V_PSU 8
#define V_LIMIT 8
#define V_INIT_LIMIT 8



// GMBL02V1 pins 
#define PIN_LED    PA7
#define PIN_MOSI   PC12
#define PIN_MISO   PC11
#define PIN_SCK    PC10
#define PIN_nCS1   PB8
#define PIN_nCS2   PB9
#define PIN_nCS3   PB10
#define PIN_M1_U   PC0
#define PIN_M1_V   PC1
#define PIN_M1_W   PC2
#define PIN_M1_EN  PC13
#define PIN_M1_FLT PC3
#define PIN_M1_SNS PB0
#define PIN_M2_U   PA0
#define PIN_M2_V   PA1
#define PIN_M2_W   PA2
#define PIN_M2_EN  PC14
#define PIN_M2_FLT PA3
#define PIN_M2_SNS PB11
#define PIN_M3_U   PC6
#define PIN_M3_V   PC7
#define PIN_M3_W   PC8
#define PIN_M3_EN  PC15
#define PIN_M3_FLT PC9
#define PIN_M3_SNS PB14


SPISettings myspisettings(1000000, BitOrder::MSBFIRST, SPI_MODE1);


MagneticSensorAS5048A sensor1(PIN_nCS1, false, myspisettings);
BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 =  BLDCDriver3PWM(PIN_M1_U, PIN_M1_V, PIN_M1_W, PIN_M1_EN);


MagneticSensorAS5048A sensor2(PIN_nCS2, false, myspisettings);
BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver2 =  BLDCDriver3PWM(PIN_M2_U, PIN_M2_V, PIN_M2_W, PIN_M2_EN);


MagneticSensorAS5048A sensor3(PIN_nCS3, false, myspisettings);
BLDCMotor motor3 = BLDCMotor(7);
BLDCDriver3PWM driver3 =  BLDCDriver3PWM(PIN_M3_U, PIN_M3_V, PIN_M3_W, PIN_M3_EN);

Commander command = Commander(Serial);





float target1, target2, target3;


void loadAlignment() {
	// TODO load from EEPROM/FLASH
}



void initMotors() {

	pinMode(PIN_nCS1, OUTPUT);
	digitalWrite(PIN_nCS1, 1);
	sensor1.init();
	motor1.linkSensor(&sensor1);
  delay(10);

	pinMode(PIN_nCS2, OUTPUT);
	digitalWrite(PIN_nCS2, 1);
	sensor2.init();
	motor2.linkSensor(&sensor2);
  delay(10);

	pinMode(PIN_nCS3, OUTPUT);
	digitalWrite(PIN_nCS3, 1);
	sensor3.init();
	motor3.linkSensor(&sensor3);
  delay(10);

	driver1.voltage_limit = V_LIMIT;
	driver1.voltage_power_supply = V_PSU;
	driver1.init();
	motor1.linkDriver(&driver1);

	driver2.voltage_limit = V_LIMIT;
	driver2.voltage_power_supply = V_PSU;
	driver2.init();
	motor2.linkDriver(&driver2);

	driver3.voltage_limit = V_LIMIT;
	driver3.voltage_power_supply = V_PSU;
	driver3.init();
	motor3.linkDriver(&driver3);

	motor1.voltage_limit = motor2.voltage_limit = motor3.voltage_limit = V_LIMIT;
	motor1.voltage_sensor_align = motor2.voltage_sensor_align = motor3.voltage_sensor_align = V_INIT_LIMIT;
	motor1.controller = motor2.controller = motor3.controller = MotionControlType::angle;
	motor1.PID_velocity.P = motor2.PID_velocity.P = motor3.PID_velocity.P = 0.2;
	motor1.PID_velocity.I = motor2.PID_velocity.I = motor3.PID_velocity.I = 10;
	motor1.PID_velocity.D = motor2.PID_velocity.D = motor3.PID_velocity.D = 0;
	motor1.PID_velocity.output_ramp = motor2.PID_velocity.output_ramp = motor3.PID_velocity.output_ramp = 1000;
	motor1.LPF_velocity.Tf = motor2.LPF_velocity.Tf = motor3.LPF_velocity.Tf = 0.01;
	motor1.P_angle.P = motor2.P_angle.P = motor3.P_angle.P = 20;

	motor1.init();
	motor1.initFOC();
	motor2.init();
	motor2.initFOC();
	motor3.init();
	motor3.initFOC();
	// TODO store motor initialization params and only re-init if we can't load them (and provide a command to reset them)

	motor1.sensor_offset = motor1.shaftAngle();
	motor2.sensor_offset = motor2.shaftAngle();
	motor3.sensor_offset = motor3.shaftAngle();
	target1 = target2 = target3 = 0.0;

  motor1.disable();
  motor2.disable();
  motor3.disable();

}



void doTarget1(char* cmd) { command.scalar(&target1, cmd); }
void doTarget2(char* cmd) { command.scalar(&target2, cmd); }
void doTarget3(char* cmd) { command.scalar(&target3, cmd); }
void doTargetAll(char* cmd) { doTarget1(cmd); doTarget2(cmd); doTarget3(cmd); }
void doMotor1(char* cmd) { command.motor(&motor1, cmd); }
void doMotor2(char* cmd) { command.motor(&motor2, cmd); }
void doMotor3(char* cmd) { command.motor(&motor3, cmd); }
void setMotorZeros(char* cmd) {
	motor1.sensor_offset = motor1.shaftAngle();
	motor2.sensor_offset = motor2.shaftAngle();
	motor3.sensor_offset = motor3.shaftAngle();
	target1 = target2 = target3 = 0.0;
	Serial.println("Set zero position.");
}
void startStop(char* cmd) {
	int motnum = 0;
	if (strlen(cmd)>1) motnum = cmd[0] - '0';
	if (motnum==0||motnum==1) {
		if (motor1.enabled)
			motor1.disable();
		else
			motor1.enable();
	}

	if (motnum==0||motnum==2) {
		if (motor2.enabled)
			motor2.disable();
		else
			motor2.enable();
	}

	if (motnum==0||motnum==3) {
		if (motor3.enabled)
			motor3.disable();
		else
			motor3.enable();
	}
}
void storeAlignment(char* cmd) {
	// TODO store to EEPROM/FLASH
}


void setup() {


	pinMode(PIN_LED, OUTPUT);
	digitalWrite(PIN_LED, 1); // switch on lED for initialization

	Serial.begin(115200);
	while (!Serial);

	SPI.setMOSI(PIN_MOSI);
	SPI.setMISO(PIN_MISO);
	SPI.setSCLK(PIN_SCK);

	Serial.println("Motor initialization...");
	initMotors();

	command.add('a', doTarget1, "target1 position");
	command.add('b', doTarget2, "target2 position");
	command.add('c', doTarget3, "target3 position");
	command.add('d', doTargetAll, "all motors to position");
	command.add('s', startStop, "enable/disable motors");
	command.add(' ', startStop, "enable/disable motors");

	command.add('A', doMotor1, "motor 1 commands");
	command.add('B', doMotor2, "motor 2 commands");
	command.add('C', doMotor3, "motor 3 commands");
	command.add('Z', setMotorZeros, "set zero positions");
	command.add('W', storeAlignment, "store zero positions");

	Serial.println("Motor initialization complete.");
	digitalWrite(PIN_LED,0);
}

long last;

void loop() {

	motor1.loopFOC();
	motor1.move(target1);
	motor2.loopFOC();
	motor2.move(target2);
	motor3.loopFOC();
	motor3.move(target3);

	command.run();
	// TODO report status & position on Serial
  	// read it quickly or we could lose turn count
	float a1 = motor1.shaft_angle;
	float a2 = motor2.shaft_angle;
	float a3 = motor3.shaft_angle;

	if (sensor1.isErrorFlag()) {
		Serial.println("S1ERR");
    sensor1.clearErrorFlag();    
  }
	if (sensor2.isErrorFlag()) {
		Serial.println("S2ERR");
    sensor2.clearErrorFlag();
  }
	if (sensor3.isErrorFlag()) {
		Serial.println("S3ERR");
    sensor3.clearErrorFlag();
  }

	// print once per second, to keep it readable
	long ts = millis();
	if ((ts-last)>=1000) { 
		last = ts;
		Serial.print(a1,4);
		Serial.print("\t");
		Serial.print(a2,4);
		Serial.print("\t");
		Serial.println(a3,4);
	}
}

[Richard]

Hey Runger,
Sorry for not replying sooner. Thanks for providing that code. I did find the issue with the Storm32 CLK line with a scope. It was more attenuated than expected, but it was present.

However my project did move on to needing 4 BLDC with FOC, which is working great on an ESP32D, with 4 AS5048a sensors using SPI and 4 DRV8313s.

I figured I’d mention that here, incase that solution is helpful to others going down the 3+ BLDC motor route and struggling with the STorm32.

Thanks again for all the help!

[MostHated]

@runger

However, its MCU is not really powerful enough to run 3 motors with SimpleFOC, and so the performance you will get won’t be great.

Man, that’s a bummer. I just received a few of these from aliexpress. I should have done a bit more reading up on them first. I figured that since they were designed for 3 gimbal motors, it should be no problem. I guess you can’t have your cake and… well, you know the rest.