I tried motor driver using Arduino simplefoc. When I set control type to "velocity openloop" or "angle openloop", it runs well. But for "velocity" and "angle" type, the motor cannot go to the set point. It spins cw then ccw then cw again and again.
The motor and driver I used is SparkFun IoT Brushless Motor Driver v10. I connect it to mac pro m2 and don't change anything(all as factory setting).
I created an arduino sketch, set "SparkFun ESP32 Thing Plus C" as board type. The source code as following.
#include <Wire.h>
#include <SimpleFOC.h>
#include <SparkFun_TMAG5273_Arduino_Library.h>
#define RAD2DEGREES(v) ((v * 4068) / 71)
#define DEG2RADIANS(v) ((v * 71) / 4068)
//GPIO
#define auxBtn2 13
#define auxBtn1 14
//driver
#define uh16 16
#define ul17 17
#define vh18 18
#define wh19 19
#define vl23 23
#define wl33 33
#define diag 34 // <-TMC6300
#define vio 05 // ->TMC6300, reset to standby(low)/enable(high)
#define i2cint 04 // <-TMAG5273
#define i2cscl 22 // <-TMAG5273
#define i2csda 21 // <-TMAG5273
#define i2caddr TMAG5273_I2C_ADDRESS_INITIAL
#define rgbLED 02 // ->WS2812
#define curSense 32 // <-MCP6021
#define curSenseU 35 // <-INA240
#define curSenseV 36 // <-INA240
#define curSenseW 39 // <-INA240
// I2C default address
uint8_t i2cAddress = TMAG5273_I2C_ADDRESS_INITIAL;
// Set constants for setting up device
uint8_t conversionAverage = TMAG5273_X32_CONVERSION;
uint8_t magneticChannel = TMAG5273_XYX_ENABLE;
uint8_t angleCalculation = TMAG5273_XY_ANGLE_CALCULATION;
float angleCalculationVal = 0;
TMAG5273 _sensor; // Initialize hall-effect sensor
void initMySensorCallback(){
// do the init
}
float readMySensorCallback(){
if((_sensor.getMagneticChannel() != 0) && (_sensor.getAngleEn() != 0)) // Checks if mag channels are on - turns on in setup
{
angleCalculationVal = _sensor.getAngleResult();
//sensor.update();
}
else
{
Serial.println("Mag Channels disabled, stopping..");
while(1);
}
return DEG2RADIANS(angleCalculationVal);
}
GenericSensor sensor = GenericSensor(readMySensorCallback, initMySensorCallback);
// inline current sensor instance
// InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET)
InlineCurrentSense current_sense = InlineCurrentSense((float)0.01, (float)20.0, curSenseU, curSenseV, curSenseW);
// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
BLDCMotor motor = BLDCMotor(7); // BLDCMotor(11, 10.5, 120)
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver6PWM driver = BLDCDriver6PWM(uh16, ul17, vh18, vl23, wh19, wl33);
static float zero_rad = 0.0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&motor.target, cmd); }
void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); }
void doVelocity(char* cmd) { command.scalar(&motor.velocity_limit, cmd); }
void doMotor(char* cmd){ command.motor(&motor, cmd); }
void doStop(char* cmd) { motor.target = motor.shaft_angle; }
void doZero(char* cmd) { zero_rad = sensor.getAngle(); motor.sensor_offset = 0; motor.shaft_angle = 0; }
struct Button{
const uint8_t PIN;
uint32_t numberKeyPresses;
bool pressed;
};
Button aux1 = {auxBtn1, 0, false};
Button aux2 = {auxBtn2, 0, false};
void IRAM_ATTR isrbtn1(){
aux1.pressed = true;
//target_velocity = target_velocity*(-1);
Serial.println("Changing directions.. ");
}
void IRAM_ATTR isrbtn2(){
aux2.numberKeyPresses++;
aux2.pressed = true;
if((aux2.numberKeyPresses % 2) == 0)
{
//target_velocity = 0;
//motor.disable();
Serial.println("Stopping motor.. ");
}
else
{
//target_velocity = 5;
//motor.enable();
Serial.println("Starting motor.. ");
}
}
void setup() {
// configure i2C hall sensor
//Wire.setClock(400000);
//Wire.setPins(18, 19);
Wire.begin();
Serial.begin(115200);
pinMode(aux1.PIN, INPUT_PULLUP); // Sets pin 14 on the ESP32 as an interrupt
attachInterrupt(aux1.PIN, isrbtn1, FALLING); // Triggers when aux1 is pulled to GND (button pressed)
pinMode(aux2.PIN, INPUT_PULLUP); // Sets pin 13 on the ESP32 as an interrupt
attachInterrupt(aux2.PIN, isrbtn2, FALLING); // Triggers when aux2 is pulled to GND (button pressed)
// If begin is successful (0), then start example
if(_sensor.begin(i2cAddress, Wire) == true) {
//Serial.println("Begin");
}
else { // Otherwise, infinite loop
Serial.println("Hall sensor failed to setup - Freezing code.");
while(1); // Runs forever
}
// Set the device at 32x average mode
_sensor.setConvAvg(conversionAverage);
// Choose new angle to calculate from
// Can calculate angles between XYX, YXY, YZY, and XZX
_sensor.setMagneticChannel(magneticChannel);
// Enable the angle calculation register
// Can choose between XY, YZ, or XZ priority
_sensor.setAngleEn(angleCalculation);
// initialise magnetic sensor hardware
sensor.init();
motor.linkSensor(&sensor);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 3.3;
driver.pwm_frequency = 20000;
driver.voltage_limit = 4;
// limit the maximal dc voltage the driver can set
// as a protection measure for the low-resistance motors
// this value is fixed on startup
//driver.voltage_limit = 6;
driver.init();
// link current sense and driver
current_sense.linkDriver(&driver);
// link the motor and the driver
motor.linkDriver(&driver);
// limiting motor movements
// limit the voltage to be set to the motor
// start very low for high resistance motors
// currnet = resistance*voltage, so try to be well under 1Amp
//motor.voltage_limit = 3; // [V]
// limit/set the velocity of the transition in between
// target angles
//motor.velocity_limit = 5; // [rad/s] cca 50rpm
// set control loop type to be used
motor.torque_controller = TorqueControlType::foc_current;
//motor.controller = MotionControlType::angle;
motor.controller = MotionControlType::velocity;
// controller configuration based on the control type
motor.PID_velocity.P = 0.05;
motor.PID_velocity.I = 1;
motor.PID_velocity.D = 0;
// default voltage_power_supply
motor.voltage_limit = 4;
// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01;
// angle loop controller
motor.P_angle.P = 20;
// angle loop velocity limit
motor.velocity_limit = 20;
// add current limit
// motor.phase_resistance = 3.52 // [Ohm]
// motor.current_limit = 2; // [Amps] - if phase resistance defined
// comment out if not needed
motor.useMonitoring(Serial);
motor.monitor_downsample = 0; // // initially disable the real-time monitor //100; // set downsampling can be even more > 100
//motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // set monitoring of d and q currents
motor.init();
// current sense init hardware
current_sense.init();
// link the current sense to the motor
motor.linkCurrentSense(¤t_sense);
// invert phase b gain
//current_sense.gain_b *=-1;
// skip alignment
//current_sense.skip_align = true;
// align sensor and start FOC
motor.initFOC();
// set the initial motor target
// motor.target = 0.2; // Amps - if phase resistance defined
motor.target = 0; // Volts
// add target command T
// command.add('T', doTarget, "target current"); // - if phase resistance defined
//command.add('T', doTarget, "target voltage");
command.add('M', doMotor, "motor control");
command.add('S', doStop, "stop motor");
command.add('Z', doZero, "set zero");
//command.add('L', doLimit, "voltage limit");
//command.add('V', doVelocity, "movement velocity");
//command.verbose = VerboseMode::nothing; // disable commander output to serial
Serial.println(F("Motor ready."));
Serial.println(F("Set the target using serial terminal:"));
_delay(1000);
}
void loop() {
// Button Press ISR
if(aux1.pressed){
aux1.pressed = false;
}
// Turning motor on and off
if(aux2.pressed){
aux2.pressed = false;
}
// main FOC algorithm function
motor.loopFOC();
// Motion control function
motor.move();
// display the currents
motor.monitor();
// user communication
command.run();
}
The esp32(by Espressif Systems) version is 2.0.14, and simplefoc version is 2.3.1. I tried a whole day to adjust luxuriant PID settings using simplefoc studio (I forked one to make it work with macOS m1/m2 natively, https://github.com/chengyingyuan/SimpleFOCStudio) without success.
I tried motor driver using Arduino simplefoc. When I set control type to "velocity openloop" or "angle openloop", it runs well. But for "velocity" and "angle" type, the motor cannot go to the set point. It spins cw then ccw then cw again and again.
The motor and driver I used is SparkFun IoT Brushless Motor Driver v10. I connect it to mac pro m2 and don't change anything(all as factory setting).
I created an arduino sketch, set "SparkFun ESP32 Thing Plus C" as board type. The source code as following.
The esp32(by Espressif Systems) version is 2.0.14, and simplefoc version is 2.3.1. I tried a whole day to adjust luxuriant PID settings using simplefoc studio (I forked one to make it work with macOS m1/m2 natively, https://github.com/chengyingyuan/SimpleFOCStudio) without success.
Are there any hints?