Hello everyone,
I’m running into issues with running foc_current , with velocity control beyond a certain limit (150 rad/s, approx. 1500 RPM). I have run the same motor and controller using ST’s work-bench and motor pilot and I could reach speeds of up to 4500 RPM. Almost three times the difference.
Few things, I noticed in my troubleshooting efforts: The loop rate in ST’s motor pilot is at 20kHz (PWM also runs at 20kHz), while SimpleFOC is at 23kHz (with PWM at 20kHz) (with reading encoder values in ABI mode, 1000 PPR using AS5047P). However, I notice the loop rate goes down by 0.2kHz in SimpleFOC with every 10 rad/s increase in speed. Which is not the case in ST’s motor pilot. At the same time, if I tried SPI on SimpleFOC, the loop rate is low, at 3.8kHz, but it stays consistent with speed. Not sure what’s causing this drop in loop rate with ABI mode in SimpleFOC?
Another thing I noticed, (by accident), if I ground one of the current sense phases, SimpleFOC works fine, There’s negligible difference in performance. Which is great and makes me thing it’s robust. And if I tried the same on ST’s motor pilot, it can hardly keep the motor stable and cannot maintain speed. But, this is making me wonder, if there’s something in SimpleFOC that’s limiting the performance of the motor to keep things safe for FOC control ?
Where can I start, to debug, or troubleshoot? I’m interested in the performance I’m getting through ST’s motor pilot, but through SimpleFOC. I have tried playing with motor.motion_downsample and the suggestions laid down here, like using SPI at 10MHz, adjusting zero_angle etc. , but to not much avail (hardly a 10rad/s improvement). I also see the CORDIC _sin and _cos being used, but it’s not helping in improving the performance. Any guidance would be helpful.
Are you using the regular Encoder class? That will slow it down as speed increases due to large number of interrupts. STM32 has the ability to count encoder pulses in hardware without interrupts if you use the STM32HWEncoder class.
Are you using LowSideCurrentSense or InlineCurrentSense? I noticed the other day that the B_G431B_ESC1 example uses low side, but the hardware is inline (EDIT: I was wrong, hardware is low side).
That is odd that grounding a phase would be an improvement.
Oh, let me try the STM32HWEncoder class. I saw the regular Encoder class mentioned about hardware interrupts, so took it as it is. But the hardware interrupts should be a big improvement. I’ll let you know how it goes.
I’m using LowSideCurrentSense. I thought the hardware was also lowside though, I can pull up the schematic of the B-G4310ESC later and send it to you.
I just tried the STM32HWEncoder class and it did indeed improve the velocity, thank you for pointing that out. It went from 150 rad/s to 200 rad/s max. But it’s still halfway to ST’s 4500 RPM.
Any idea on what else I can try.
You should try to run your control loop in sync with your PWM. To do this, you can create an interrupt on the timer what generates the PWM, and in the ISR put motor.loopFOC. This should result in a noticeable improvement in maximum motor speed.
This could be because SimpleFOC was not measuring that phase current, or it was close to zero at that time. In theory this should not be the case, and what should happen is the control loop tries to regulate the current higher as it thinks the current is always lower than the desired value. This will trip the hardware overcurrent protection, or something will blow up if the overcurrent protection fails.
Setting up the interrupts depends on the timer configuration. Here’s the way I do it in my custom firmware. You might need to heavily modify this depending on your setup. This code works on STM32CubeIDE. It is not very optimized and probably also quite ugly with a mix of HAL and direct register access.
// timer initialisation code before here
HAL_NVIC_SetPriority(TIM1_CC_IRQn, 0, 0); // setup the interrupt in the NVIC, at highest priority
HAL_NVIC_EnableIRQ(TIM1_CC_IRQn);
TIM1->CCR4 = 1; // channel 4 of timer used to control when the interrupt fires
TIM1->DIER |= TIM_DIER_CC4IE; // enable the main loop interrupt
// this function is called by stm32 hal when interrupt is fired, you may need to change this to your own function
void TIM1_CC_IRQHandler(void) {
__HAL_TIM_CLEAR_IT(&htim1, TIM_IT_CC4); // clear the interrupt from the timer
loopfoc(); // my custom code
}
It might be difficult to achieve this kind of performance with SimpleFOC. You are asking for 94500 ERPM (1.575KHz), which is a very high speed. I am able to get 125000 ERPM on my motor with custom firmware, which runs the FOC loop at 50KHz. You should consider using custom firmware if SimpleFOC can’t achieve the desired performance.
FOC loop of 50kHz is awesome! and 125000 ERPM is .
I’m open to looking into a custom firmware, or a hybrid between simpleFOC and custom. Any guidance on where to start? Do you have your firmware posted anywhere.
Hi @runger , it’s always a pleasure to hear from you.
I didn’t do anything particularly. Just noticed that the _sin and _cos were being used and figured they were probably referencing the CORDIC system. Please correct me if anything. And if I need to do anything to set it up.
Unfortunately, it went down. It’s now at 16kHz. But it stays consistent now. In contrast to how it was going down every time I increased the speed. So, that seems to be a plus. But there’s probably something else that’s slowing down the loop.
It’s a bit ugly, with all my troubleshooting.
#include <SimpleFOC.h>
#include "SimpleFOCDrivers.h"
#include "encoders/stm32hwencoder/STM32HWEncoder.h"
#include "Arduino.h"
#include "Wire.h"
#include "SPI.h"
BLDCMotor motor = BLDCMotor(21);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
Encoder sensor = Encoder(A_HALL1, A_HALL2, 1000);
// MagneticSensorSPI sensor = MagneticSensorSPI(PA15, 14, 0x3FFE);
//Hardware Encoder for STM32
STM32HWEncoder encoder = STM32HWEncoder(1000, PB6, PB_7_ALT1); // TIM4
float targetSpeed = 0;
// void doA(){sensor.handleA();}
// void doB(){sensor.handleB();}
Commander command = Commander(Serial);
// void doTarget(char* cmd) { command.scalar(&motor.velocity_limit, cmd); }
void doMotor(char* cmd) { command.motor(&motor, cmd); }
float normal_zero_angle = 0;
static int i = 0;
void setup() {
// init magnetic sensor hardware
pinMode(PB9, OUTPUT);
// sensor.init();
// sensor.enableInterrupts(doA, doB);
encoder._pinA = PB_6;
encoder._pinB = PB_7_ALT1;
encoder.init();
// motor.linkSensor(&sensor);
motor.linkSensor(&encoder);
driver.voltage_power_supply = 54;
// driver.pwm_frequency = 25000;
driver.init();
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);
// aligning voltage [V]
motor.voltage_sensor_align = 3;
// index search velocity [rad/s]
// Control loop setup
motor.controller = MotionControlType::velocity;
motor.torque_controller = TorqueControlType::foc_current;
// motor.motion_downsample = 30.0;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// default voltage_power_supply
motor.voltage_limit = 54;
motor.LPF_velocity.Tf = 0.01;//0.01; // velocity low pass filtering time constant ****BRENT = 0.01
motor.P_angle.P = 30; // P value of PID_angle ****BRENT = 30
motor.P_angle.output_ramp = 100;
// motor.PID_velocity.P = 0.8;
motor.current_limit = 6; // in Amps. ****BRENT = 0.5 but he also had his own control loop
motor.velocity_limit = 500; // [rad/s] 5 rad/s cca 50rpm ****BRENT = 19. He also had a limit set on the angle
motor.feed_forward_velocity = 4;
Serial.begin(115200);
command.add('M',doMotor,"motor");
// command.add('T', doTarget, "motor.velocity_limit");
motor.useMonitoring(Serial);
motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
motor.init();
motor.initFOC();
Serial.println(motor.zero_electric_angle);
normal_zero_angle = motor.zero_electric_angle;
Serial.println("Motor ready!");
_delay(1000);
}
void loop() {
//
// i ++;
// if(i%10 == 0){
// // motor.zero_electric_angle=-0.0000007*pow(motor.shaft_velocity_sp,2) + 0.0001*motor.shaft_velocity_sp + normal_zero_angle + 0.2;
// motor.zero_electric_angle=normal_zero_angle + 0.01*motor.shaft_velocity;
// digitalToggle(PB9);
// i = 0;
// }
motor.loopFOC();
motor.move();
// motor.monitor();
command.run();
}
Yeah. But I’m hoping to get the 4500RPM we saw with ST’s Motor Pilot.
My firmware is closed source for now, because it seems competitive with commercial solutions so I might want to sell it in the future. I’m not sure whether to sell open source or closed source and what would be the most successful way, so until I decide that it will stay closed source.
However my motor driver hardware is open source as it can’t compete with the commercial stuff yet, and so I don’t think I will sell any of my current designs. You can find it here.
That’s still quite good, with CORDIC it should go up a bit more.
It’s 10x your desired erpms of 94500… that’s perhaps still a bit low but not far off…
I think the next thing to add is more motor parameters - resistance, KV and inductance and see if tuning these a bit helps you reach more speed.
I’d also try torque-current mode before velocity mode, and try to tune the current PIDs really well.
With the motor voltage limit equal to the driver voltage limit you’re already over-modulating a lot… normally it should be 1/2 the driver limit to avoid over-modulation…
I just tried it and it’s now at 17.8kHz and went upto 230 rad/s . The PWM frequency is at 25kHz (would decreasing it to 20kHz be helpful?)
When you have a moment, can you elaborate on this a little (or share a link or video referring to this). Just curious. I used to think lowering it means limiting the motor performance.
Yeah, I was planning to look into the same. Ways of avoiding some code that can improve the loop rate. But, making the code more effecient will only give 0.1 to 1kHz of improvement. There’s something fundamental we’ll have to change, in order to get to 4500 RPM (from current 2200 RPM)
On another note, wanted to figure why we need this here in the loopFOC function. We are already getting shalf_angle and shaft_velocity inside of move() function. I tried skipping it, and the motor wouldn’t move. Is there another efficient way of doing it by chance?
For a AS5047P sensor which has a PPR of 1000, does the above value of 1000 look correct. I noticed in one place where CPR was reference and was 4*PPR. Should it be 4000 instead?
This page explains about the voltage Field Oriented Control | Arduino-FOC
The motor voltage gets multiplied by sin/cos so total range is doubled. The wave is centered around driver voltage/2. SpaceVectorPWM allows using a bit higher voltage by adding a triangle wave to the sine wave, which keeps it within range while generating the same magnetic field in the coils. Reminds me of how a reuleaux triangle can roll despite not being round
The call to sensor.update in loopFOC is what allows it to continue advancing the magnetic field without running all the PID loops in motor.move. The shaft angle retrieved in move() doesn’t change unless sensor.update is called.
There’s not much inefficiency aside from the overhead of doing things in a modular way. If you strip all the code down to direct calls without virtual functions and such, and run at a fixed loop rate instead of calling micros() all over the place, it would be a fair bit faster. But obviously custom and not very useful to other people then.
Basically motor limit acts on Uq (or Iq) where the voltage can be positive or negative, while driver limit acts on the absolute positive only 0-PSU voltage. So the motor limit has twice the effective range if you take the negative voltage into account, so setting motor.limit = driver.limit/2 is the setting that gets you full range without over modulation.
Later you could try setting it to be equal to the loop frequency or 2x the loop frequency to see if aligning them makes any difference but I don’t think it will be critical. If you care about this then the method Andrew suggested of running loopFOC in an interrupt in sync with the PWM is the better suggestion.
The sensor update method refreshes the value from the sensor and buffers it internally until you call update again. It means the different parts of SimpleFOC work with the same angle value during one iteration. It also keeps track of the full rotations so you have to call it regularly.
In the case of the HWEncoder it is just copying the timer register value to the sensor member field, and the performance impact is negligible.
Thank you @dekutree64 and @runger for clarifying all the aspects. It’s starting to make sense now.
I just tried motion_downsampling (to 30) and bringing the PWM frequency down to 20kHz. The loop rate went up to 24.4kHz !! I was hoping this will get it close to the 4500 RPM, but the speed only increased by a little (from 230 rad/s to 250 rad/s).
Interesting. Maybe phase lag? I’m not sure if foc_current inherently compensates for it or not. There’s a line of code in the foc_current case of loopFOC that’s commented out due to lack of testing, which applies an offset based on motor.phase_inductance. You could uncomment it and give it a try.
Hey @mizzi_labs,
This is a very interesting topic.
I was wondering if you tried using the torque control loop instead of the velocity control. Setting a 1-2 amp current target (or some other value for which you’re sure that the motor can accelerate to its max velocity) and measuring thr achieved velocity.
The velocity achieved in the torque mode is probably the best that you can do with Simplefoc, as the other modes add more complexity and possibly more lag.
Also, I’m wondering if the STM motor control suite uses flux weakening. Do you have any info about that?