I have a FOC setup using STM32G474, DRV8302 board, and a 42BLS02 motor (though I;ve tried many different motors).
Everything works great when I don’t use current sensing, but when I enable current sensing, via:
LowsideCurrentSense current_sense = LowsideCurrentSense(0.005, 40, PA0, PA1, _NC);
I get this:
Current sense init success!
MOT: Monitor enabled!
MOT: Init
MOT: Enable driver.
MOT: Align sensor.
MOT: sensor_direction==CW
MOT: PP check: OK!
MOT: Zero elec. angle: 0.29
MOT: Align current sense.
MOT: Align error!
MOT: Init FOC failed.
I double, and tripple checked the wiring and everything is hooked up. I’ve verified that there is a good, strong current signal on the PA0 and PA1 inputs.
What gives?
Whole app:
#include <Arduino.h>
#include <SimpleFOC.h>
#include “STM32HWEncoder.h”
#include <SimpleFOC.h>
#include “Wire.h”
#include “AS5600.h”
#include “spi.h”
#define CONTROL_TYPE (MotionControlType::velocity)
#define SUPPLY_V (12)
#define DRIVER_V_LIMIT (12)
#define MOTOR_V_LIMIT (6)
#define MOTOR_VEL_LIMIT (1000)
#define SENSOR_ALIGN_V (1)
#define MOTOR_PP (4)
#define MOTOR_RES (0.34)
#define MOTOR_K (250)
#define MOTOR_IND (0.000520)
//#define MOD_FREQ (64000)
#define COMMANDER
#define CURSENS
#define CLOSED_LOOP
#define MONITOR
#define DEB_RETRIG_THRUS_INIT_US (500000)
#define DEB_RETRIG_THRS_SUS_US (100000)
#define HB_IO (PA5)
STM32HWEncoder sensor = STM32HWEncoder(1024, PA_11_ALT2, PA_12_ALT1); // nucleo32-g431
// motor
BLDCMotor motor = BLDCMotor(MOTOR_PP, MOTOR_RES, MOTOR_K, MOTOR_IND); // uni motor
// driver
BLDCDriver3PWM driver = BLDCDriver3PWM(PC0, PC1, PC2, PC3); // nucleo64-g474
LowsideCurrentSense current_sense = LowsideCurrentSense(0.005, 40, PA0, PA1, _NC);
// commander communication instance
Commander command = Commander(Serial);
// void doMotor(char* cmd) { command.motor(&motor, cmd); }
void doTarget(char* cmd) {command.scalar(&motor.target, cmd);}
void doLimit(char* cmd) {command.scalar(&motor.voltage_limit, cmd);}
void doMotor(char* cmd) { command.motor(&motor, cmd); }
void doInduct(char* cmd) { command.scalar(&motor.phase_inductance, cmd); }
void doMyval1(char* cmd) { command.scalar(&g_myval1, cmd); }
void doMyval2(char* cmd) { command.scalar(&g_myval2, cmd); }
void doscl(char* cmd) { command.scalar(&g_scl, cmd); }
void dovlim(char* cmd) { command.scalar(&vlim_slop, cmd); }
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”);
sensor.init();
// link the motor to the sensor
// driver config
// power supply voltage [V]
driver.voltage_power_supply = SUPPLY_V;
driver.voltage_limit = DRIVER_V_LIMIT;
driver.init();
// link driver
motor.linkDriver(&driver);
// link current sense and the driver
current_sense.linkDriver(&driver);
// current sense init and linking
if (current_sense.init())
Serial.println(“Current sense init success!”);
else{
Serial.println(“Current sense init failed!”);
return;
}
motor.linkCurrentSense(¤t_sense);
// velocity loop PID
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 5;
motor.PID_velocity.D = 0.0;
motor.PID_velocity.output_ramp = 0.0;
motor.PID_velocity.limit = 1000.0;
// Low pass filtering time constant
motor.LPF_velocity.Tf = 0.01;
// angle loop PID
motor.P_angle.P = 40.0;
motor.P_angle.I = 0.0;
motor.P_angle.D = 0.0;
motor.P_angle.output_ramp = 0.0;
motor.P_angle.limit = MOTOR_VEL_LIMIT;
// Low pass filtering time constant
motor.LPF_angle.Tf = 0.0;
motor.torque_controller = TorqueControlType::foc_current;
// current q loop PID 1/40, 1/40
motor.PID_current_q.P = 0.2;
motor.PID_current_q.I = 5;
motor.PID_current_q.D = 0.0;
motor.PID_current_q.output_ramp = 0;
motor.PID_current_q.limit = 30;
// Low pass filtering time constant
motor.LPF_current_q.Tf = 0.01;
// current d loop PID
motor.PID_current_d.P = 0.2;
motor.PID_current_d.I = 5;
motor.PID_current_d.D = 0.0;
motor.PID_current_d.output_ramp = 0;
motor.PID_current_d.limit = 30;
// Low pass filtering time constant
motor.LPF_current_d.Tf = 0.01;
// limts
motor.voltage_sensor_align = SENSOR_ALIGN_V;
motor.controller = CONTROL_TYPE;
// default voltage_power_supply
motor.velocity_limit = MOTOR_VEL_LIMIT;
motor.voltage_limit = MOTOR_V_LIMIT;
motor.current_limit = 1000.0;
// set the inital target value
motor.target = 0;
#ifdef MONITOR
motor.useMonitoring(Serial);
motor.monitor_downsample = 0; // disable intially
motor.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
//motor.foc_modulation = SpaceVectorPWM;
// initialise motor
motor.init();
motor.initFOC();
command.add(‘M’,doMotor,“motor”);
Serial.printf(“setup complete…\n”);
_delay(1000);
}
void loop()
{
static int loopct = 0;
// iterative setting FOC phase voltage
motor.loopFOC();
// iterative function setting the outter loop target
motor.move();
// motor monitoring
motor.monitor();
// user communication
command.run();
++loopct;
}