hi, i have been using the SimpleFOC driver with an Arduino mega. i’ve been really enjoying all the documentation available already, but i haven’t seen this problem yet. please forgive me if it is already noted somewhere.
my application uses several motors, and i would like to combine the setup calls into one function to simplify my code. i’ve got this working in the void setup() statement (it goes through the process and aligns the sensor, etc) but when the code goes to the void loop() statement, the motor doesn’t move.
if i bypass my function with the same code, the motor does move, so i am theorizing that it’s a problem with the function. i am thinking that the “void function” statement is canceling the driver/motor/foc init calls at the end of the function execution.
is there a way to make this work? i put the code below. thanks for your help!!
#include <SimpleFOC.h>
// setup motor and driver declarations and pin locations
BLDCMotor motor1 = BLDCMotor(11);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(5, 9, 6, 8); // arduino pins for the first driver
// declare encoder instance
MagneticSensorI2C sensor1 = MagneticSensorI2C(MT6701_I2C);
// declare current sensor instance
// shunt resistor value
// gain value
// pins phase A,B, (C optional)
InlineCurrentSense current_sense1 = InlineCurrentSense(0.01f, 50.0f, A0, A2);
float power_supply_voltage = 24;
float driver_voltage = 24;
float voltage_limit = 8;
float velocity_limit = 10;
float target_velocity = 2;
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
void setup() {
Serial.begin(115200); // To PC
// configure i2c
Wire.begin();
Wire.setClock(400000);
motorsetup(motor1, driver1, sensor1, current_sense1, power_supply_voltage, driver_voltage, voltage_limit, velocity_limit);
motor1.useMonitoring(Serial);
command.add('T', doTarget, "target velocity");
Serial.println(F("Motor ready."));
Serial.println(F("Set the target velocity using serial terminal:"));
} // close setup statement
void loop() {
// main FOC algorithm function - iterative setting of the FOC phase voltage
motor1.loopFOC();
// motion control function
motor1.move(target_velocity);
command.run(); // reads serial instance form constructor
}
and the function code
void motorsetup(BLDCMotor motorid, BLDCDriver3PWM driverid, MagneticSensorI2C sensorid, InlineCurrentSense current_senseid, float power_supply_voltage, float driver_voltage, float voltage_limit,
float velocity_limit){
// enable the debugging output
SimpleFOCDebug::enable(&Serial);
// initialise magnetic sensor hardware
sensorid.init();
Serial.println("Sensor ready");
motorid.linkSensor(&sensorid);
// power supply voltage [V]
driverid.voltage_power_supply = power_supply_voltage;
driverid.voltage_limit = driver_voltage;
driverid.init();
if (!driverid.init()) {
Serial.println("Driver init failed!");
return;
}
// link motor & driver
motorid.linkDriver(&driverid);
// aligning voltage
motorid.voltage_sensor_align = 3;
// link the driver with the current sense
current_senseid.linkDriver(&driverid);
// set control loop type to be used
motorid.torque_controller = TorqueControlType::voltage;
motorid.controller = MotionControlType::velocity;
// motor PID tuning
motorid.PID_velocity.P = 0.2;
motorid.PID_velocity.I = 20;
motorid.PID_velocity.D = 0;
// default voltage_power_supply
motorid.voltage_limit = voltage_limit; // set limit to keep below 2A = 2*phase_resistance
motorid.P_angle.P = 10; // default is P=20
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
motorid.PID_velocity.output_ramp = 1000;
// velocity low pass filtering
// default 5ms - try different values to see what is the best.
// the lower the less filtered
motorid.LPF_velocity.Tf = 0.01f;
//motorid.velocity_limit = velocity_limit;
// calculate the filter time constant
// based on the max velocity you need
// and the rule of thumb for the cutoff frequency
float max_velocity = 100.0; // rad/s
float motor_frequency_hz = max_velocity / (2 * PI); // ~16 Hz
// velocity low pass filtering time constant
motorid.LPF_velocity.Tf = 1.0 / (2.0 * PI * motor_frequency_hz * 5); // ~80 Hz
// initialise motor
if (!motorid.init()) {
Serial.println("Motor 1 init failed!");
return;
}
if (!current_senseid.init()) {
Serial.println("Current sense 1 init failed.");
return;
}
// for SimpleFOCShield v2.01/v2.0.2 invert phase b gain
current_senseid.gain_b *= -1;
// skip alignment
current_senseid.skip_align = true;
// align encoder and start FOC
// link the current sense to the motor
motorid.linkCurrentSense(¤t_senseid);
Serial.println("Current sense 1 ready.");
// align sensor and start FOC
if(!motorid.initFOC()){
Serial.println("FOC 1 init failed!");
return;
}
Serial.println("motor setup loop complete");
}