I have an app with two instances of Motor/Drive/HallSensor
I have my code structured such that I can easily enable each Motor/Drive/Sensor separately (#define USE_MOTOR1 and/or #define USE_MOTOR2) .
Each instance of Motor/Drive/HallSenssor works fine by itself, but when I run with both USE_MOTOR1 and USE_MOTOR2 defined, it seems that the last initialized sensor is the one that works??? (is the driver re-initializing or overwriting something from the previous instance?)
#include <Arduino.h>
#include <SimpleFOC.h>
#include "DRV8301.h"
#include "STM32HWEncoder.h"
#include <SimpleFOC.h>
#define CONTROL_TYPE (MotionControlType::velocity)
#define SUPPLY_V (18)
#define DRIVER_V_LIMIT (4.5)
#define MOTOR_V_LIMIT (2)
#define SENSOR_ALIGN_V (1)
#define COMMANDER
#define CLOSED_LOOP
#define MONITOR
#define HALL
//#define USE_SMOOTHSENSOR
**#define USE_MOTOR1**
**#define USE_MOTOR2**
#ifdef USE_SMOOTHSENSOR
#include "smoothsensor.h"
#endif
#define HB_IO (8)
#ifdef COMMANDER
// commander communication instance
Commander command = Commander(Serial);
#endif
//************************************** MOTOR1 DECLARE *****************************************
#ifdef USE_MOTOR1
#ifdef CLOSED_LOOP
HallSensor sensor1 = HallSensor(12, 14, 15, 15); // PA6, PB9, PB8
#endif // CLOSED_LOOP
DRV8302 gate_driver1 = DRV8302(2, 3, 4, 5, 6, 7); // PA10, PB3, PB5, PB4, PB10, PA8, nFAULT, M_PWM, M_OC, GAIN, OC_ADJ, EN_GATE
BLDCMotor motor1 = BLDCMotor(15, 0.4, 28, 0.0004); // hub
BLDCDriver3PWM driver1 = BLDCDriver3PWM(PC0, PC1, PC2); // DRV8301/2
#ifdef HALL
void doA1(){sensor1.handleA();}
void doB1(){sensor1.handleB();}
void doC1(){sensor1.handleC();}
#endif // HALL
#ifdef COMMANDER
// void doMotor(char* cmd) { command.motor(&motor, cmd); }
void doTarget1(char* cmd) {command.scalar(&motor1.target, cmd);}
void doLimit1(char* cmd) {command.scalar(&motor1.voltage_limit, cmd);}
void doMotor1(char* cmd) { command.motor(&motor1, cmd); }
#endif // COMMANDER
#ifdef USE_SMOOTHSENSOR
SmoothingSensor smooth1 = SmoothingSensor(sensor1, motor);
#endif // USE_SMOOTHSENSOR
#endif // USE_MOTOR1
//************************************** MOTOR2 DECLARE *****************************************
#ifdef USE_MOTOR2
#ifdef CLOSED_LOOP
//HallSensor sensor2 = HallSensor(PB12, PA11, PA12, 15); //
HallSensor sensor2 = HallSensor(PC6, PA11, PA12, 15); // OK, cannot use [PB12, PA11, PA12]
#endif // CLOSED_LOOP
DRV8301 gate_driver2 = DRV8301(PB14, PB15, PB1, PB13, PB2, PB11); // MOSI, MISO, SCLK, CS, EN_GATE, nFAULT
//DRV8301 gate_driver2 = DRV8301(3, 4, 5, 2, 6, 7); // MOSI, MISO, SCLK, CS, EN_GATE, nFAULT
BLDCMotor motor2 = BLDCMotor(15, 0.4, 28, 0.0004); // hub
BLDCDriver3PWM driver2 = BLDCDriver3PWM(PC7, PC8, PC9); // DRV8301/2
#ifdef HALL
void doA2(){sensor2.handleA();}
void doB2(){sensor2.handleB();}
void doC2(){sensor2.handleC();}
#endif // HALL
#ifdef COMMANDER
void doTarget2(char* cmd) {command.scalar(&motor2.target, cmd);}
void doLimit2(char* cmd) {command.scalar(&motor2.voltage_limit, cmd);}
void doMotor2(char* cmd) { command.motor(&motor2, cmd); }
#endif // COMMANDER
#ifdef USE_SMOOTHSENSOR
SmoothingSensor smooth2 = SmoothingSensor(sensor2, motor2);
#endif // USE_SMOOTHSENSOR
#endif // USE_MOTOR2
char msgbuf[256];
void setup() {
Serial.begin(921600); // WARNING: low value like 115200 cause distorted FOC
// for timer analysis
SimpleFOCDebug::enable(&Serial);
//delay(5000);
Serial.printf("enter setup...\n");
pinMode(HB_IO, OUTPUT);
//************************************** MOTOR1 SETUP *****************************************
#ifdef USE_MOTOR1
#ifdef CLOSED_LOOP
#ifdef HALL
sensor1.pullup = Pullup::USE_INTERN;
sensor1.enableInterrupts(doA1, doB1, doC1);
#endif // HALL
sensor1.init();
// link the motor to the sensor
#ifdef USE_SMOOTHSENSOR
#ifdef HALL
smooth1.phase_correction = -_PI_6; // FOR HALL SENSOR
#endif
motor1.linkSensor(&smooth1);
#else
motor1.linkSensor(&sensor1);
#endif
#endif // CLOSED_LOOP
// driver config
// power supply voltage [V]
driver1.voltage_power_supply = SUPPLY_V;
driver1.voltage_limit = DRIVER_V_LIMIT;
driver1.init();
gate_driver1.begin(PWM_INPUT_MODE_3PWM, SHUNT_GAIN_10);
_delay(100);
// link driver
motor1.linkDriver(&driver1);
// link current sense and the driver
#ifdef CLOSED_LOOP
// velocity loop PID
motor1.PID_velocity.P = 0.7;
motor1.PID_velocity.I = 0.7;
motor1.PID_velocity.D = 0.0;
motor1.PID_velocity.output_ramp = 200.0;
motor1.PID_velocity.limit = 1000.0;
// Low pass filtering time constant
motor1.LPF_velocity.Tf = 0.1;
// angle loop PID
motor1.P_angle.P = 7.0;
motor1.P_angle.I = 0.0;
motor1.P_angle.D = 0.0;
motor1.P_angle.output_ramp = 50.0;
motor1.P_angle.limit = 27.0;
// Low pass filtering time constant
motor1.LPF_angle.Tf = 0.0;
#endif // CLOSED_LOOP
// limts
motor1.voltage_sensor_align = SENSOR_ALIGN_V;
//motor.controller = MotionControlType::torque;
motor1.controller = CONTROL_TYPE;
// default voltage_power_supply
motor1.velocity_limit = 27 ;
motor1.voltage_limit = MOTOR_V_LIMIT;
motor1.current_limit = 1000.0;
// set the inital target value
motor1.target = 0;
#ifdef MONITOR
motor1.useMonitoring(Serial);
motor1.monitor_downsample = 0; // disable intially
motor1.monitor_variables = _MON_TARGET | _MON_VOLT_Q | _MON_VOLT_D | _MON_CURR_Q | _MON_CURR_D | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
#endif
#ifdef COMMANDER
// subscribe motor to the commander
command.add('T', doTarget1, "target1");
command.add('L', doLimit1, "voltage limit1");
command.add('M',doMotor1,"motor1");
#endif // COMMANDER
#ifdef USE_MOTOR1
//motor.foc_modulation = SpaceVectorPWM;
// initialise motor
motor1.init();
#ifdef CLOSED_LOOP
// align encoder and start FOC
motor1.initFOC();
#endif // CLOSED_LOOP
#endif // USE_MOTOR1
#endif // USE_MOTOR1
//************************************** MOTOR2 SETUP *****************************************
#ifdef USE_MOTOR2
#ifdef CLOSED_LOOP
#ifdef HALL
sensor2.pullup = Pullup::USE_INTERN;
sensor2.enableInterrupts(doA2, doB2, doC2);
#endif // HALL
sensor2.init();
// link the motor to the sensor
#ifdef USE_SMOOTHSENSOR
#ifdef HALL
smooth2.phase_correction = -_PI_6; // FOR HALL SENSOR
#endif
motor2.linkSensor(&smooth2);
#else
motor2.linkSensor(&sensor2);
#endif
#endif // CLOSED_LOOP
// driver config
// power supply voltage [V]
driver2.voltage_power_supply = SUPPLY_V;
driver2.voltage_limit = DRIVER_V_LIMIT;
driver2.init();
// configure the DRV8301
gate_driver2.begin(PWM_INPUT_MODE_3PWM, SHUNT_GAIN_10);
_delay(100);
int reg1, reg2, reg3, reg4, fault;
gate_driver2.get_regs(®1, ®2, ®3, ®4);
fault = gate_driver2.is_fault();
sprintf(msgbuf, "DRV8301: fault=%x, STATREG1=0x%.4x, STATREG2=0x%.4x, CTRLREG1=0x%.4x, CTRLREG2=0x%.4x", fault, reg1, reg2, reg3, reg4);
Serial.println(msgbuf);
// link driver
motor2.linkDriver(&driver2);
// link current sense and the driver
#ifdef CLOSED_LOOP
// velocity loop PID
motor2.PID_velocity.P = 0.7;
motor2.PID_velocity.I = 0.7;
motor2.PID_velocity.D = 0.0;
motor2.PID_velocity.output_ramp = 200.0;
motor2.PID_velocity.limit = 1000.0;
// Low pass filtering time constant
motor2.LPF_velocity.Tf = 0.1;
// angle loop PID
motor2.P_angle.P = 7.0;
motor2.P_angle.I = 0.0;
motor2.P_angle.D = 0.0;
motor2.P_angle.output_ramp = 50.0;
motor2.P_angle.limit = 27.0;
// Low pass filtering time constant
motor2.LPF_angle.Tf = 0.0;
#endif // CLOSED_LOOP
// limts
motor2.voltage_sensor_align = SENSOR_ALIGN_V;
//motor.controller = MotionControlType::torque;
motor2.controller = CONTROL_TYPE;
// default voltage_power_supply
motor2.velocity_limit = 27 ;
motor2.voltage_limit = MOTOR_V_LIMIT;
motor2.current_limit = 1000.0;
// set the inital target value
motor2.target = 0;
#ifdef MONITOR
motor2.useMonitoring(Serial);
motor2.monitor_downsample = 0; // disable intially
motor2.monitor_variables = _MON_TARGET | _MON_VOLT_Q | _MON_VOLT_D | _MON_CURR_Q | _MON_CURR_D | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
#endif
#ifdef COMMANDER
// subscribe motor to the commander
command.add('U', doTarget2, "target2");
command.add('N', doLimit2, "voltage limit2");
command.add('O',doMotor2,"motor2");
#endif // COMMANDER
#ifdef USE_MOTOR2
//motor.foc_modulation = SpaceVectorPWM;
// initialise motor
motor2.init();
#ifdef CLOSED_LOOP
// align encoder and start FOC
motor2.initFOC();
#endif // CLOSED_LOOP
#endif // USE_MOTOR2
#endif // USE_MOTOR2
Serial.printf("setup complete...\n");
_delay(1000);
}
void loop() {
digitalWrite(HB_IO, 1);
#ifdef CLOSED_LOOP
// iterative setting FOC phase voltage
#ifdef USE_MOTOR1
motor1.loopFOC();
#endif // USE_MOTOR1
#ifdef USE_MOTOR2
motor2.loopFOC();
#endif // USE_MOTOR2
#endif // CLOSED_LOOP
// iterative function setting the outter loop target
#ifdef USE_MOTOR1
motor1.move();
#endif // USE_MOTOR1
#ifdef USE_MOTOR2
motor2.move();
#endif // USE_MOTOR2
#ifdef MONITOR
// motor monitoring
#ifdef USE_MOTOR1
motor1.monitor();
#endif // USE_MOTOR1
#ifdef USE_MOTOR2
motor2.monitor();
#endif // USE_MOTOR2
#endif // MONITOR
#ifdef COMMANDER
// user communication
command.run();
#endif // COMMANDER
digitalWrite(HB_IO, 0);
#ifdef USE_MOTOR1
if (gate_driver1.is_fault())
{
sprintf(msgbuf, "gate driver1 faulted");
}
#endif // USE_MOTOR1
#ifdef USE_MOTOR2
if (gate_driver2.is_fault())
{
sprintf(msgbuf, "gate driver2 faulted");
}
#endif // USE_MOTOR2
}