Hello,
SimpleFOCMini is really easy to use.
On the other hand, “B-G431B-ESC1” is really very complex, it really lacks “SPI” support for me …
After a lot of reading of your documents (thank you) and time spent testing,
-I now have an “angle” mode that works well.
-a “velocity” mode that works perfectly,
-However, I haven’t managed to get a stable “torque” mode…
Too bad, for this project I need to maintain an adjustable tension on a cable.
It’s for lab rats that need to be relieved of 40% of their weight by a suspension system, to measure muscle loss.
For the moment I’ve got what I want to do via the “velocity” mode by limiting “motor.current_limit” in “setup”.
In my opinion, it would have been preferable to achieve this using the “torque” mode.
If you see any errors in my example, please don’t hesitate to advise me.
The current code:
/*
Sensor zero offset is:
21:07:12.763 -> 1.2046
21:07:12.763 -> Sensor natural direction is:
21:07:12.763 -> Direction::CW
21:07:12.763 -> To use these values provide them to the: motor.initFOC(offset, direction)
21:07:13.758 -> If motor is not moving the alignment procedure was not successfull!!
*/
#include<Arduino.h>
#include <SimpleFOC.h>
// BLDC motor instance BLDCMotor(polepairs, (R), (KV))
BLDCMotor motor = BLDCMotor(11, 5.57, 197);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
// https://docs.simplefoc.com/low_side_current_sense
// LowsideCurrentSense constructor
// - shunt_resistor - shunt resistor value
// - gain - current-sense op-amp gain
// - phA - A phase adc pin
// - phB - B phase adc pin
// - phC - C phase adc pin (optional)
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f / 7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
//sensor @PWM
MagneticSensorPWM sensor = MagneticSensorPWM(PB6, 3, 920); //3, 932 OK pour moi frafa
// instantiate the commander
Commander command = Commander(Serial);
// SimpleFOCStudio
void doMotor(char* cmd) {
command.motor(&motor, cmd);
}
// target
void onTarget(char* cmd) {
command.target(&motor, cmd);
}
//▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬setup▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬
void setup() {
// 1(Amp)/1023(Val Pot) = 0,000977517
float map_pot = analogRead(A_POTENTIOMETER) * 0.000977517;
// initialise magnetic sensor hardware
sensor.init();
// driver config
// pwm frequency to be used [Hz]
// for atmega328 fixed to 32kHz
// esp32/stm32/teensy configurable
driver.pwm_frequency = 100000; // 100000
// power supply voltage [V]
driver.voltage_power_supply = 12;
// Max DC voltage allowed - default voltage_power_supply
driver.voltage_limit = 12; // [V]
// init driver
if (driver.init()) Serial.println("Driver init success!");
else {
Serial.println("Driver init failed!");
return;
}
// link the motor to the sensor
motor.linkSensor(&sensor);
// 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);
//▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬SimpleFOCStudio_config▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬
// torque mode
motor.torque_controller = TorqueControlType::foc_current;
// Control loop type
// MotionControlType::torque - torque control
// MotionControlType::velocity - velocity motion control
// MotionControlType::angle - position/angle motion control
//motor.controller = MotionControlType::angle; // angle velocity torque
//motor.controller = MotionControlType::torque; // angle velocity torque
motor.controller = MotionControlType::velocity; // angle velocity torque
motor.motion_downsample = 0.0; // https://docs.simplefoc.com/bldcmotor
// pwm modulation settings
motor.foc_modulation = FOCModulationType::SinePWM;
motor.modulation_centered = 1.0;
// velocity loop PID
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 20.0;
motor.PID_velocity.D = 0.001;
motor.PID_velocity.output_ramp = 1000.0;
//motor.PID_velocity.limit = 1.0; // ? current_limit [Amps]
// Low pass filtering time constant
motor.LPF_velocity.Tf = 0.01;
// angle loop PID
motor.P_angle.P = 14.0;
motor.P_angle.I = 0.0;
motor.P_angle.D = 0.0;
motor.P_angle.output_ramp = 10000.0;
//motor.P_angle.limit = 50.0; // ? velocity_limit [rad/s]
// Low pass filtering time constant
motor.LPF_angle.Tf = 0.001;
// current q loop PID
motor.PID_current_q.P = 0.1;
motor.PID_current_q.I = 30.0;
motor.PID_current_q.D = 0.0;
motor.PID_current_q.output_ramp = 0.0;
motor.PID_current_q.limit = 12.0;
// Low pass filtering time constant
motor.LPF_current_q.Tf = 0.005;
// current d loop PID
motor.PID_current_d.P = 0.1;
motor.PID_current_d.I = 30.0;
motor.PID_current_d.D = 0.0;
motor.PID_current_d.output_ramp = 0.0;
motor.PID_current_d.limit = 12.0;
// Low pass filtering time constant
motor.LPF_current_d.Tf = 0.005;
// Limits
motor.velocity_limit = 50.0; // [rad/s] cca ~500rpm
motor.voltage_limit = 12.0; // [Volts]
//motor.current_limit = 1.0f; // 1.0 [Amps] - if phase resistance defined
motor.current_limit = map_pot; // 1.0 [Amps] - if phase resistance defined
// sensor zero offset - home position
motor.sensor_offset = 0.0;
// general settings
// motor phase resistance // I_max = V_dc/R
//motor.phase_resistance = 5.57; // 5.7 5.57 [Ohm]
// motor KV rating [rpm/V]
//motor.KV_rating = 197; // [rpm/Volt] - default not set
// direction
motor.sensor_direction = Direction::CW;
//▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬End_SimpleFOCStudio_config▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬
// monitoring port
Serial.begin(115200);
//delay(1000);
// tell the motor to use the monitoring
motor.useMonitoring(Serial);
motor.monitor_downsample = 0; // 0 = disable monitor at first - optional
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// https://docs.simplefoc.com/theory_corner
// https://github.com/simplefoc/Arduino-FOC/blob/dev/src/common/defaults.h
// https://docs.simplefoc.com/cheetsheet/options_reference
// https://docs.simplefoc.com/commander_interface
// https://docs.simplefoc.com/commander_motor
// https://docs.simplefoc.com/commander_target
// https://docs.simplefoc.com/studio
// add the motor to the commander interface
// The letter (here 'M') you will provide to the SimpleFOCStudio
command.decimal_places = 4; // default 3
command.add('M', doMotor, "motor");
Serial.println(F("Motor ready."));
Serial.println(F("Set the target current using serial terminal:"));
// T
command.add('T', onTarget, "Target setting");
// set the inital target value
motor.target = 10;
Serial.println("A_POTENTIOMETER Limit Amps:" + String(map_pot) + " [Amps]");
_delay(1000);
} // End_setup
//▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬End_setup▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬
//▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬loop▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬
void loop() {
// main FOC algorithm function
motor.loopFOC();
// Motion control function
motor.move();
// function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!!
motor.monitor();
// user communication
command.run();
} // End_loop
//▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬End_loop▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬▬
