Is there an example for simplefoc angle controlling?

[chengyingyuan]

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(&current_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.

Are there any hints?

[Hiren017]

Here you go:

#include <Wire.h>
#include <SparkFun_TMAG5273_Arduino_Library.h>
#include <SimpleFOC.h>

#define UH 16
#define UL 17
#define VH 18
#define VL 23
#define WH 19
#define WL 33

BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(UH, UL, VH, VL, WH, WL);

class TMAG5273_Sensor : public Sensor {
public:
TMAG5273_Sensor(TMAG5273* sensor)
: tmag_sensor(sensor) {}

float getSensorAngle() override {
// Get the angle from the TMAG5273 sensor and convert to radians
float angle_deg = tmag_sensor->getAngleResult(); // Get angle in degrees
return (angle_deg * PI) / 180.0; // Convert degrees to radians
}

private:
TMAG5273* tmag_sensor;
};

TMAG5273 sensor;
TMAG5273_Sensor tmag_sensor_wrapper(&sensor);

unsigned long previousMillis = 0;
const long interval = 1000;

uint16_t position_count = 0;

float target_angle_deg = 0.0;
float target_angle_rad = 0.0;
float current_angle = 0.0;

// 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;

bool dataread = false;

void readsensor(void) {
if ((sensor.getMagneticChannel() != 0) && (sensor.getAngleEn() != 0)) // Checks if mag channels are on - turns on in setup
{
current_angle = sensor.getAngleResult();
Serial.printf("current position : %0.2f°\n", current_angle);
} else {
Serial.println("Mag Channels disabled, stopping..");
while (1);
}
}

void setup() {
Wire.begin();
// Start serial communication at 115200 baud
Serial.begin(115200);
delay(2000);
if (sensor.begin(i2cAddress, Wire) == true) {
Serial.println("Begin");
} else {
Serial.println("Device failed to setup - Freezing code.");
while (1)
;
}

sensor.setConvAvg(conversionAverage);
sensor.setMagneticChannel(magneticChannel);
sensor.setAngleEn(angleCalculation);

driver.voltage_power_supply = 5.0;
driver.pwm_frequency = 32000;
driver.voltage_limit = 11.0;
driver.init();
motor.linkSensor(&tmag_sensor_wrapper);
motor.linkDriver(&driver);
motor.voltage_limit = 7.4;
motor.velocity_limit = 10.0;
motor.controller = MotionControlType::angle;
motor.PID_velocity.P = 0.005f;
motor.PID_velocity.I = 0.5;
motor.PID_velocity.D = 0;
motor.PID_velocity.output_ramp = 1000;
motor.LPF_velocity.Tf = 0.01f;
motor.P_angle.P = 20;
motor.init();
motor.initFOC();
motor.enable();
}

void loop() {

if (Serial.available() > 0) {
String inputString = Serial.readStringUntil('\n');
target_angle_deg = inputString.toFloat();
target_angle_rad = ((target_angle_deg * PI) / 180.0) * (-1);
Serial.printf("Target Angle value = %0.2f°\n", target_angle_deg);
Serial.flush();
}

unsigned long currentMillis = millis();

if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis;
readsensor();
}

motor.loopFOC();
motor.move(target_angle_rad);
}