I am having the exact same problem, except that I am using hall sensors. Every time I try to implement any form of current sensing the run freezes.
Arduino IDE 1.8.16
Arduino_Core_STM32 V2.2.0
SimpleFOC Library 2.2.2
Board: Discovery → B-G431B-ESC1
Example code:
/**
B-G431B-ESC1 position motion control example with encoder
*/
#include <SimpleFOC.h>
// Motor instance
BLDCMotor motor = BLDCMotor(7, 0.15, 1550);
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);
//InlineCurrentSense currentSense = InlineCurrentSense(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);
HallSensor sensor = HallSensor(A_HALL1, A_HALL2, A_HALL3, 7 );
// Interrupt routine intialisation
// channel A, B and C callbacks
void doA() {
sensor.handleA();
}
void doB() {
sensor.handleB();
}
void doC() {
sensor.handleC();
}
//void doA(){encoder.handleA();}
//void doB(){encoder.handleB();}
//void doIndex(){encoder.handleIndex();}
// instantiate the commander
Commander command = Commander(Serial);
//void doTarget(char* cmd) { command.motion(&motor, cmd); }
void onMotor(char* cmd) {
command.motor(&motor, cmd);
}
/*float target_current = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_current, cmd); }*/
// voltage set point variable
float target_voltage = 1;
void doTarget(char* cmd) {
command.scalar(&target_voltage, cmd);
}
void calcKV(char* cmd) {
// calculate the KV
Serial.println(motor.shaft_velocity / motor.target * 30.0f / _PI);
}
void setup() {
// initialize encoder sensor hardware
//encoder.init();
//encoder.enableInterrupts(doA, doB);
// initialize encoder sensor hardware
sensor.pullup;
sensor.init();
sensor.enableInterrupts(doA, doB, doC);
// link the motor to the sensor
//motor.linkSensor(&encoder);
// link the motor to the sensor
motor.linkSensor(&sensor);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
driver.pwm_frequency = 20000;
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(¤tSense);
// aligning voltage [V]
motor.voltage_sensor_align = 3;
// index search velocity [rad/s]
motor.velocity_index_search = 3;
// control loop type and torque mode
motor.torque_controller = TorqueControlType::voltage;
motor.controller = MotionControlType::torque;
motor.motion_downsample = 0.0;
// velocity loop PID
motor.PID_velocity.P = 0.1;
motor.PID_velocity.I = 1.0;
motor.PID_velocity.D = 0.0;
motor.PID_velocity.output_ramp = 100.0;
motor.PID_velocity.limit = 2.0;
// Low pass filtering time constant
motor.LPF_velocity.Tf = 0.1;
// angle loop PID
motor.P_angle.P = 20.0;
motor.P_angle.I = 0.0;
motor.P_angle.D = 0.0;
motor.P_angle.output_ramp = 0.0;
motor.P_angle.limit = 50.0;
// Low pass filtering time constant
motor.LPF_angle.Tf = 0.0;
// current q loop PID
motor.PID_current_q.P = 3.0;
motor.PID_current_q.I = 300.0;
motor.PID_current_q.D = 0.0;
motor.PID_current_q.output_ramp = 100;
motor.PID_current_q.limit = 6.0;
// Low pass filtering time constant
motor.LPF_current_q.Tf = 0.005;
// current d loop PID
motor.PID_current_d.P = 3.0;
motor.PID_current_d.I = 300.0;
motor.PID_current_d.D = 0.0;
motor.PID_current_d.output_ramp = 100;
motor.PID_current_d.limit = 6.0;
// Low pass filtering time constant
motor.LPF_current_d.Tf = 0.005;
// Limits
motor.velocity_limit = 500.0;
motor.voltage_limit = 6.0;
motor.current_limit = 2.0;
// sensor zero offset - home position
motor.sensor_offset = 0.0;
// general settings
// motor phase resistance
motor.phase_resistance = 0.15;
// pwm modulation settings
motor.foc_modulation = FOCModulationType::SinePWM;
motor.modulation_centered = 1.0;
// use monitoring with serial
Serial.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial);
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC(0.0, Direction::CCW);
// set the inital target value
//motor.target = 1;
// add target command T
//command.add('T', doTarget, "target angle");
// define the motor id
//command.add('T', doTarget, "target current");
command.add('A', onMotor, "motor");
//Serial.println(F("Motor ready."));
//Serial.println(F("Set the target angle using serial terminal:"));
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V."));
// add target command T
command.add('T', doTarget, "target voltage");
command.add('K', calcKV, "calculate KV rating");
Serial.println(F("Motor ready."));
Serial.println(F("Set the target voltage : - commnad T"));
Serial.println(F("Calculate the motor KV : - command K"));
_delay(1000);
}
void loop() {
// main FOC algorithm function
// iterative setting FOC phase voltage
motor.loopFOC();
// Motion control function
//motor.move();
motor.move(target_voltage);
// motor.move(target_current);
//Serial.println("test");
// function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!!
//motor.monitor();
// user communication
command.run();
//PhaseCurrent_s currents = currentSense.getPhaseCurrents();
//float current_magnitude = currentSense.getDCCurrent();
//Serial.print(currents.a*1000); // milli Amps
//Serial.print("\t");
//Serial.print(currents.b*1000); // milli Amps
// Serial.print("\t");
//Serial.println(currents.c*1000); // milli Amps
//Serial.print("\t");
//Serial.println(current_magnitude*1000); // milli Amps
}
Cheers!
There are probably a few patches to the variant to make it simpler to use e.g. enabling opamp by default (as described above).
