Hi,
I am having a bit of a problem with two motors on one MCU. I haven’t had the chance to debug anything yet, so feel free to chip in with any ideas for deeper analysis.
MCU: STM32 Black Pill STM32F401CCU6
Driver: BTS7960
Motor: Hoverboard, 250W, Hall sensors
The code runs perfectly fine with one motor, but not with two (pls ignore the I2C code - not used yet, the commands are being sent via serial commander).
#include <SimpleFOC.h>
#include "SimpleFOCDrivers.h"
#include "comms/i2c/I2CCommander.h"
#define SERIAL_COMMANDER
#define MOTOR1
//#define MOTOR2
#define PP 15
#define HZ 5000
#define V_PSU 12 //driver1.voltage_power_supply
#define V_LIM 12 //driver1.voltage_limit
#define V_MOT_LIM 6 //motor1.voltage_limit
#define V_MOT_LIM_INIT 6 //motor1.voltage_limit
#define V_MOT_SENS_ALIGN 3 //motor1.voltage_sensor_align
#define MOT_P 0.2f
#define MOT_I 2
#define MOT_D 0
#define MOT_PID_RAMP 1000
#define MOT_LPF 0.05f // expected 0.005-0.01f (in sec)
#define SDA_PIN PB4
#define SCL_PIN PA8
int rcvnr = 0;
int serial_loop = 0;
//int a = 0;
TwoWire Wire2(SDA_PIN, SCL_PIN);
// I2C commander instance
uint8_t i2c_addr = 0x61; // can be anything you choose
I2CCommander commander(&Wire2);
void onReceive(int numBytes) {
commander.onReceive(numBytes);
rcvnr++;
}
void onRequest() {
commander.onRequest(); }
#ifdef SERIAL_COMMANDER
// velocity set point variable
float target_velocity = 0;
float target_P = 0;
float target_I = 0;
float target_D = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
#endif
#ifdef MOTOR1
BLDCMotor motor1 = BLDCMotor(PP);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(PB0, PA7, PA6, PB1);
HallSensor sensor1 = HallSensor(PB12, PB13, PB14, PP);
void doA1(){sensor1.handleA();}
void doB1(){sensor1.handleB();}
void doC1(){sensor1.handleC();}
#endif
#ifdef MOTOR2
BLDCMotor motor2 = BLDCMotor(PP);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(PA2, PA1, PA0, PA3);
HallSensor sensor2 = HallSensor(PB8, PB7, PB6, PP);
void doA2(){sensor2.handleA();}
void doB2(){sensor2.handleB();}
void doC2(){sensor2.handleC();}
#endif
void setup() {
delay(2000);
Serial.begin(115200);
SimpleFOCDebug::enable();
#ifdef MOTOR1
sensor1.pullup = Pullup::USE_INTERN;
sensor1.init();
sensor1.enableInterrupts(doA1, doB1, doC1);
motor1.linkSensor(&sensor1);
driver1.pwm_frequency = HZ;
driver1.voltage_power_supply = V_PSU;
driver1.voltage_limit = V_LIM;
driver1.init();
motor1.linkDriver(&driver1);
motor1.voltage_limit = V_MOT_LIM_INIT;
motor1.voltage_sensor_align = V_MOT_SENS_ALIGN;
motor1.controller = MotionControlType::velocity;
motor1.PID_velocity.P = MOT_P;
motor1.PID_velocity.I = MOT_I;
motor1.PID_velocity.D = MOT_D;
motor1.PID_velocity.output_ramp = MOT_PID_RAMP;
motor1.LPF_velocity.Tf = MOT_LPF;
delay(2000);
Serial.println("Motor 1");
motor1.useMonitoring(Serial) ;
motor1.init();
motor1.initFOC(); // motor1.initFOC(2.1, CCW);
motor1.voltage_limit = V_MOT_LIM;
#endif
#ifdef MOTOR2
sensor2.pullup = Pullup::USE_INTERN;
sensor2.init();
sensor2.enableInterrupts(doA2, doB2, doC2);
motor2.linkSensor(&sensor2);
driver2.pwm_frequency = HZ;
driver2.voltage_power_supply = V_PSU;
driver2.voltage_limit = V_LIM;
driver2.init();
motor2.linkDriver(&driver2);
motor2.voltage_limit = V_MOT_LIM_INIT;
motor2.voltage_sensor_align = V_MOT_SENS_ALIGN;
motor2.controller = MotionControlType::velocity;
motor2.PID_velocity.P = MOT_P;
motor2.PID_velocity.I = MOT_I;
motor2.PID_velocity.D = MOT_D;
motor2.PID_velocity.output_ramp = MOT_PID_RAMP;
motor2.LPF_velocity.Tf = MOT_LPF;
Serial.println("Motor 2");
motor2.useMonitoring(Serial) ;
motor2.init();
motor2.initFOC(); //motor2.initFOC(0.13, CCW);
motor2.voltage_limit = V_MOT_LIM;
#endif
digitalWrite(SDA_PIN, LOW);
digitalWrite(SCL_PIN, LOW);
Wire2.begin(i2c_addr, true);
#ifdef MOTOR1
commander.addMotor(&motor1);
#endif
#ifdef MOTOR2
commander.addMotor(&motor2);
#endif
commander.init(i2c_addr);
Wire2.onReceive(onReceive);
Wire2.onRequest(onRequest);
#ifdef SERIAL_COMMANDER
// add target command T
command.add('T', doTarget, "target voltage");
#endif
Serial.println("Motor ready!");
delay(1000);
}
void loop() {
command.run();
#ifdef MOTOR1
motor1.loopFOC();
motor1.move(target_velocity);
#endif
#ifdef MOTOR2
motor2.loopFOC();
motor2.move(target_velocity);
#endif
if(serial_loop==100) {
// serial output
serial_loop = 0;
}
serial_loop++;
//motor1.monitor();
}
When I add the second motor and send T1 over the commander, both motors run a lot faster than 1rad/s and stop.
While a single motor is enabled, T1 makes the enabled motor run at 1 rad/sec.
Any ideas on what to look into here are welcome.
Is it possible that this behavior can be caused by my “weird” choice of pins for the phases?