SimpleFOCStudio closed loop positioning

I am having significant troubles getting closed loop working. With both angle and velocity, the motor makes no attempt to spin. It just sits at the closest cogging point and stays. If I manually force it to the next point, it will not try to go back to the original.

Everything spins with open loop, albeit, not very well.

I have verified that my linear halls print what seem like correct values. (A value of about 6 is correct for a full revolution right?)

I have also tried without SimpleFOCStudio but seem to be unable to get a good PID tune. The motor either just sits and oscillates slightly or screams. Sometimes both.

I have:
Qvadrans 1.1
2205, 14 pole, 27mOhm BLDC motor
2 DRV5055A1 linear halls offset by 90 degrees. They read between 350 and 700 on the adc
A 7v Lipo battery as the power supply
ESP32 C3 as the uart to usb adapter

Code

#include <SimpleFOC.h>
#include <SimpleFOCDrivers.h>
#include <encoders/linearhall/LinearHall.h>


//#define DebugSerial Serial
HardwareSerial Serial3(PB11, PB10);  // uart1
#define DebugSerial Serial3

// Set up the motor and such
BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15);

// Set the pins for the hall sensors
#define LinHall0 PB0
#define LinHall1 PB1

// Set up the hall effect sensors
LinearHall sensor = LinearHall(LinHall0, LinHall1, 1);

//target variable
float target_velocity = 0;

// instantiate the commander
Commander command = Commander(Serial3);
void doMotor(char* cmd) { 
    command.motor(&motor, cmd); 
}

void setup() {

  // Wait for 5 seconds
  delay(5000);

  Serial3.begin(115200);
  //Serial3.println("Hello!");

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 7;
  // 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 = .15;
  driver.init();
  // 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
  // current = voltage / resistance, so try to be well under 1Amp
  // aligning voltage [V]
  motor.voltage_sensor_align = 0.15;
  // Running voltage
  motor.voltage_limit = 0.15;  // [V]
  // Running Current
  //motor.current_limit = 3; // [A]

  // open loop control config
  motor.controller = MotionControlType::velocity_openloop;

  // init motor hardware
  motor.init();

  // Init the sensor
  sensor.init(&motor);

  // The letter (here 'M') you will provide to the SimpleFOCStudio
  command.add('M', doMotor, "motor");

  // tell the motor to use the monitoring
  motor.useMonitoring(Serial3);

  Serial3.println("Motor ready!");
  _delay(1000);
}

void loop() {
  //motor.loopFOC();       
  motor.move();
  motor.monitor();
  command.run();
}

The ESP32 Code incase it borks everything

#include <Arduino.h>

/*

AdcVal  : Temp
650     : 0 C
2003    : 37.5C (99.5 F)
4053    : 100 C (This is the max the adc will read. 
                That adc will cap at the exact voltage that should be produced at 100 C)
*/

// Set the number of loops to make before printing a value
#define NumberOfPointsToAverage 7500

// Set the thermistor pin
#define adcPin A0

// Set the TX and RX for uart
#define UART4_TX_Pin 21
#define UART4_RX_Pin 20

// Turn printing off if using SimpleFOCStudio
bool printTemp = false;

HardwareSerial HardSerial(1);

void setup() {
  // Start the Serial communication at 115200 baud rate
  Serial.begin(115200);
  
  // Begin the UART Com
  HardSerial.begin(115200, SERIAL_8N1, UART4_RX_Pin, UART4_TX_Pin);

  //set the resolution to 12 bits (0-4095)
  analogReadResolution(12);

  delay(1000);

  // Let the user know the code is actually running
  Serial.println("Serial Started");
}

// Initialize the cumulator variables
long int avgVal = 0;
short int loopNum = 0;

void loop() {
  
  if(printTemp) 
  {    
    // Read the analog pin and add it avgVal for later averaging.
    avgVal += analogRead(adcPin);
    // Count the number of loops so we know when to print
    loopNum++;

    // If the target number of loops have occured, print the data
    if(loopNum >= NumberOfPointsToAverage-1){
      // Find the temperature in Celcius from the analog read
      float degreesC = 0.0295*(avgVal/NumberOfPointsToAverage) - 20.054;
      // Print the found values
      Serial.print("Degrees C: "); Serial.println(degreesC,2);
      //Serial.printf("\tADC analog value = %d\n", avgVal/NumberOfPointsToAverage);
      
      // Reset the cumulator variables
      loopNum = 0;
      avgVal = 0;
      }
  }

  // Check if data is available on UART1
  if (HardSerial.available()) {
    // Create a temporary sting
    String stringValueStore = "";
    // While there is data, pull a character and append it to the temp string
    while (HardSerial.available() > 0) {
        stringValueStore += (char)HardSerial.read();
    }
    // Return the receive value
    Serial.print(stringValueStore);
  }

  if (Serial.available()) {
    // Create a temporary sting
    String stringValueStore = "";
    // While there is data, pull a character and append it to the temp string
    while (Serial.available() > 0) {
        stringValueStore += (char)Serial.read();
    }
    // Return the receive value
    HardSerial.print(stringValueStore);
  }
}

For confusing reasons, the driver voltage limit actually needs to be twice the motor limit. Generally you can just set the driver limit to the full 7v and only use the motor limit.

analogRead is insanely slow on STM32, so that’s why your loop rate is bad. Try pasting this in setup():

  RCC->AHB2ENR |= RCC_AHB2ENR_ADC12EN;
  RCC->CCIPR |= RCC_CCIPR_ADC12SEL_0; // ADC use PLL clock
  ADC1->CR &= ~ADC_CR_DEEPPWD; // Exit deep powerdown mode
  ADC1->CR |= ADC_CR_ADVREGEN; // Enable ADC voltage regulator
  delayMicroseconds(20); // Voltage regulator startup time from STM32G431 datasheet
  ADC1->CR |= ADC_CR_ADEN; // Enable ADC

And paste this function anywhere and it should override the default ADC code:

void ReadLinearHalls(int pinA, int pinB, int *a, int *b)
{
  ADC1->SQR1 = 1 | (pinA<<ADC_SQR1_SQ1_Pos);
  while(!(ADC1->ISR & ADC_ISR_ADRDY));
  ADC1->CR |= ADC_CR_ADSTART;
  while(!(ADC1->ISR & ADC_ISR_EOC));
  sensor.lastA = ADC1->DR;
  ADC1->ISR |= ADC_ISR_EOC; // Clear flag

  ADC1->SQR1 = 1 | (pinB<<ADC_SQR1_SQ1_Pos);
  while(!(ADC1->ISR & ADC_ISR_ADRDY));
  ADC1->CR |= ADC_CR_ADSTART;
  while(!(ADC1->ISR & ADC_ISR_EOC));
  sensor.lastB = ADC1->DR;
  ADC1->ISR |= ADC_ISR_EOC; // Clear flag
}

When using this code, the pins passed to the LinearHall constructor are interpreted as ADC channel numbers. Assuming you’re using STM32G431, you’ll need to change to this:

#define LinHall0 15 // PB0 is ADC1 channel 15
#define LinHall1 12 // PB1 is ADC1 channel 12

But beware that this code is untested. I’ve done something similar on STM32G031, but its ADC is a bit different.

Bit of an update.
I am using some code from here now. SimpleFOCGenerator

It appears to be working properly now.

#include <SimpleFOCDrivers.h>
#include <encoders/linearhall/LinearHall.h>


//#define DebugSerial Serial
HardwareSerial Serial3(PB11, PB10);  // uart1
#define DebugSerial Serial3

// Set up the motor and such
BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15);

// Set the pins for the hall sensors
#define LinHall0 PB0
#define LinHall1 PB1

// Set up the hall effect sensors
LinearHall sensor = LinearHall(LinHall0, LinHall1, 1);

//target variable
float target_velocity = 0;

// instantiate the commander
Commander command = Commander(Serial3);
void doMotor(char* cmd) { 
    command.motor(&motor, cmd); 
}

void setup() {

  // Wait for 5 seconds
  delay(5000);

  Serial3.begin(115200);
  //Serial3.println("Hello!");

  // initialize sensor
  sensor.init(&motor);
  // link sensor to motor
  motor.linkSensor(&sensor);

  // set power supply voltage
  driver.voltage_power_supply = 7;
  // set driver voltage limit, this phase voltage
  driver.voltage_limit = .3;
  // initialize driver
  driver.init();
  // link driver to motor
  motor.linkDriver(&driver);

  // set motion control type to velocity
  motor.controller = MotionControlType::velocity;

  // set torque control type to voltage (default)
  motor.torque_controller = TorqueControlType::voltage;

  // set FOC modulation type to sinusoidal
  motor.foc_modulation = FOCModulationType::SinePWM;

  // Motor limits
  motor.voltage_sensor_align = .3;
  motor.voltage_limit = 0.3;

  // use monitoring
  motor.useMonitoring(Serial3);

  // initialize motor
  motor.init();

  // align sensor and start FOC
  motor.initFOC();

  // add command to commander
  command.add('M', doMotor, "target");

  Serial3.println("Motor ready!");
  _delay(1000);
}

int loopCount = 1;
int TimeThing = millis();

void loop() {
  /*
  if(loopCount >= 10000){
    float LoopTimeMs = ((float)(millis()-TimeThing))/((float)loopCount);
    Serial3.printf("MOT: Loop Time\t"); Serial3.println(LoopTimeMs);
    loopCount = 0;
    TimeThing = millis();
  }*/
  // main FOC algorithm function, the higher the execution frequency, the better, don't put delays in the loop
  motor.loopFOC();

  // this function can be run at much lower frequency than loopFOC()
  motor.move();

  // significantly slowing the execution down
  motor.monitor();

  // user communication
  command.run();

  //loopCount++;
}```

I appear to be running a loop rate of 2.8khz which seems miserably slow. I'll try to tune PID first then deal with that.

Good to know. I’m glad others smarter than me have worked all the theory out already.

I remembering reading that and was dreading the implementation. Thank you for the starter code. That makes it significantly easier.