SimpleFOC Mini with Teensy: trouble with position control example

Anupam_Das · February 15, 2025, 6:19am

Hi All, im following along with the simpleFOC mini tutorial here;

my hardware setup , motor, encoder is identical to the tutorial (GBM4108H-120T
and AMT103) except that im using a Teensy 4.

Im able to get most of the examples working except the closed loop position control. I used the code in the tutorial here:

im getting a buzzing / twitching behavior on the motor. I tried tuning the PID and while that calmed the buzzing down I am unable to change the angle.

To rule out any hardware issues, I did verify open loop position and voltage control works, I also verified the encoder works and is outputting correct values.

Are there any nuances with the teensy and modifications I need to make to the example code. Most of the examples use the arduino.

I do have a ESP32 that I coud try but ideally id like to get simpleFOC to work with the teensy.

Anupam_Das · February 21, 2025, 10:14pm

Still struggling with getting the Teensy to work with closed loop position control. Is the Teensy not the recomended MCU to used with this Library?

My issue is likely with the encoder . Pin assignments seem fine as all pins except 13 are HW interrupt pins. the AMT103 is a very popular encoder for use with SimpleFOC. Im using the recommended settings in the example code. Im at a loss.

Antun_Skuric · February 22, 2025, 7:54am

Hey @Anupam_Das,

Just to clarify, did you try using voltage and velocity control before?
Did they work?

Position control is the most complicated one and there might be several different issues causing it. We do not have enough information yet.

There is a short guide to starting with SimpleFOC: Getting started | Arduino-FOC

To which step are you able to go?

Anupam_Das · February 22, 2025, 8:18pm

Greetings Antun, first, I am so thankful to you for this amazing library.
I have posted further details on the discord channel:

To summerize.

Yes, I have tried the open loop velocity and position code and that works fine. I have also verified my encoder is reading correctly by rotating the motor manually 360 degrees and checking it. That also seems to be perfectly accurate. I have also used the pole-pair utilitiy and landed on 11 for my motor. With all this, I was able to verify step 1 and 2 work perfectly.
Step 3 is where the issues happen. To be honest I did not use the code in Step3, I used the closed loop angle control from the examples. My code is here:
FOCTests/src/main.cpp at master · anupamdas2012/FOCTests · GitHub

I tried to use the same PWM pins from different submodules as mentioned in the docs: https://docs.simplefoc.com/choosing_pwm_pin

I do not have a electronics power supply yet. Regardless, I took care to supply clean 19v using a laptop power supply, I am using the 3v3 output from the teensy to power the Mini board and the 5v output to power the encoder. I also tried to use a 4S lipo but that resulted in similar behavior. I do need to verify that these voltages are what is to be expected using a volt meter. I will do that. It may be there is noise due to powering the sensor and board from the teensy, perhaps i need to isolate power completely?

Finally, when I run the code linked above I do not get any Hardware warnings or errors in the debug. As you can see in the video, the motor tries to “home”, and then freaks out.

I purchased the SimpleFOC mini boards from Ali Express, I purchased several as I know the quality is questionable. Seem all boards I have show the same behavior. Here is a short video of the cogging. https://youtube.com/shorts/9rH5sI6M7tY.

I greatly appreciate your input, and when resolved, I will make a video to perhaps help others who may come across similar problems.

Anupam_Das · February 23, 2025, 4:58am

update: upon strying the code in Step3 I am getting a failed to detect motion error:

code:

github.com/anupamdas2012/FOCTests

src/main.cpp

master

#include <SimpleFOC.h>

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(4, 5, 6, 0);

// encoder instance
Encoder encoder = Encoder(17, 16, 2048);
// channel A and B callbacks
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

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

void setup() { 
  
  // use monitoring with serial 
  Serial.begin(115200);

This file has been truncated. show original

I tried lib_archive = false and true. Same result.

I re-ran tests from step 2 and it seems to succeed, I was able to get the motor to run smoothly when setting target velocity and voltage limit.

i should note setting lib_archive = true removed the high pitch whine and the motor ran smoother.

and step 1 still outputs the correct encoder readings for a 360 rotation.

this is quite strange indeed.

Could it be pin assignment or power supply issues?

I should also mention i am using the v1.0 of the SimpleFoc Mini. and verified the correct pinout connections there.

Antun_Skuric · February 23, 2025, 9:06am

Hey @Anupam_Das,

The line liba_archive=True is necessary, if you don’t set it the code will try to use the Arduino default functions, which are not always implemented for all architectures. For teensy they are though, but the PWM frequency is at 1kHz, hence the high-pitched noise. So the fact that you dont hear the noise any more is a good sign. And especially since you see the TEESNY-DRV outputs in the serial, this means that the correct code is being used.

So yeah, its a bit strange. It should work, if the first two steps did go well. If the open-loop works then the driver is fine, and the power-supply should be fine as well.
If the sensor test works, then the sensor is fine as well, especially since it’s the amt102 encoder, they are very reliable.

Failed to notice movement indicates two things:

if the motor moved, then it is the sensor issue, it did not read any values
if the motor did not move then, there is an issue with the driver (but that seems not to be the case)

Debugging the sensor issues

So then can you verify that the sensor is well fixed and that the inner shaft is not slipping.
Can you also verify that for one full rotation of the sensor you have exactly 2pi ~ 6.28 rad read?
Last thing that you can try is to run your example in the velocity_openloop mode (or angle_openloop). The init will not pass but that is not a big deal, you just need to enable the motor (motor.enable()) after the initFOC call. And then in the main loop you can print the motor position encoder.getAngle() after the loopFOC and move call.
You should be able to see coherent sensor readings even though the motor is controlled in the open-loop. If you don’t see any sign of movements in the sensor readings that probably means the sensor is somehow not initialised well.

Anupam_Das · February 26, 2025, 8:41am

Greetings @Antun_Skuric . First, thanks for your continued support.

I have been able to verify that my sensor and motor work together without any mechanical or electrical interference. I created a test where in open loop velocity control I stop the motor when the encoder reads 6.39 pi radians using this code:FOCTests/src/main.cpp at 9ff89c8e3b8be26db9f7c6213663f8fbf6d4dc96 · anupamdas2012/FOCTests · GitHub

video: https://youtube.com/shorts/TPYyIhh7bJM

Then i attempted closed loop velocity control using this step in this tutorial: https://www.youtube.com/watch?v=ENGGh0ajE2M&t=258s

No matter what value I type in, as soon as my I term is >0 I get chattering in the motor:

code:FOCTests/src/main.cpp at d8a96fef70d7937d170d192b7c600cb7fda2b6a5 · anupamdas2012/FOCTests · GitHub

Note: P and I are initially 0 and use the command input to tune P. I have also set I = 1 and then tried to tune P. Setting LPF_velocity.Tf = 0.1; smooths out the ocolations

So clearly, something specific inside the closed loop mode is causing issues. I wonder if there are any additional build flags that would be useful?

Antun_Skuric · February 26, 2025, 2:17pm

So you do no longer have the issue Failed to notice movement. That is great.

Did you try using voltage torque control?
Does it behave well?
It does not use any PIDs so this at least removes the gain tuning factor from the equation.

Anupam_Das · February 26, 2025, 2:50pm

ah yes I did try voltage torque control using
motor.torque_controller = TorqueControlType::foc_current;
motor.torque_controller = TorqueControlType::voltage; (since its a Mini)

that works. Its when I go to the velocity control using PID i get shaking like in the video ( ```
motor.controller = MotionControlType::velocity;

I did not use ```
motor.PID_velocity.output_ramp = 1000;
``` but i'll try that.

I'll also try setting motor.voltage_limit = 1. 

Perhaps the PID controller is asking too much from the motor causing spikes?

Anupam_Das · February 26, 2025, 6:28pm

i found this thread that may be related to my issue. Seems I am not the only one that has trouble with closed loop vibrations. I will try the suggestions here;/ Gimbal motor overheating - #6 by Valentine

Antun_Skuric · February 26, 2025, 8:13pm

Ok that’s great. So it’s either the PID tuning or the velocity measurement that’s problematic.

As teensy boards are very fast the velocity calculation can be pretty noisy. You can down sample the velocity calculation using the encoder.min_elapsed_time value.it determines what is the minimal elapsed time after which the sensor will calculate the new velocity value. By default is 100us if I’m not mistaking. You can set it to a few milliseconds.

encoder.min_elapsed_time=0.002; //2ms

This will make your velocity calculation much better. Then you can tune your PIDs afterwards.

Anupam_Das · February 28, 2025, 12:38am

@Antun_Skuric I was hopeful for the min_elapsed_time to calm down the motor but seems to have made no difference. Here is my latest code:

github.com/anupamdas2012/FOCTests

src/main.cpp

master


      
          
          void setup() { 
            
            // use monitoring with serial 
            Serial.begin(115200);
            // enable more verbose output for debugging
            // comment out if not needed
            SimpleFOCDebug::enable(&Serial);
          
          
            encoder.min_elapsed_time=0.001; //2ms
          
            // initialize encoder sensor hardware
            encoder.init();
            encoder.enableInterrupts(doA, doB); 
            // link the motor to the sensor
            motor.linkSensor(&encoder);
            //motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // default _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE
            motor.monitor_downsample = 100; // default 10
          
            // driver config

I have a stm32 bluepill board and tried that, but it is a pain to set it up to upload and debug. So I gave up on that.

Perhaps I need to just try a basic arduino board like a mega just to see if I can get it to work.

Antun_Skuric · February 28, 2025, 7:38am

Hi @Anupam_Das,

Yeah teensy is not at popular as the other platforms with simplefoc but there are many people that are using it.

It’s very strange that the velocity loop does not work if the voltage loop work. Did you plot the velocity, is it noisy?

Are you sure that the encoder is well fixed to the shaft and not slipping?

Anupam_Das · February 28, 2025, 8:46am

the teensy is very powerful and small formfactor, so I am not giving up yet. It is indeed strange that the voltage loop would work yet the velocity loop does not. I will plot the velocity and see if it is noisy.

Mechanically ,its impossible for the encoder to slip. I created a rigid shaft that mates directly with the encoder without the friction fit that the AMT103 comes with. As stated earlier, I verified that the encoder is working by having the motor rotate 360 and printing out the encoder values and the encoder also read exactly 360.

I could try 6PWM on the teensy but im guessing if 3PWM isnt working right than 6PWM wouldnt be much better.

Antun_Skuric · February 28, 2025, 9:27am

Yeah, is the voltage loop works, that means that the encoder and driver are working well. There is no point in changing the MCU or the sensor or the driver, everything works.

There are two things that are different when you change from the voltage to the velocity loop

the loop is closed over the velocity calculation which might be noisy
the PID parameters can be bad making for an unstable behavior.

There are no other real reasons why would it not work.

I imagine that the vibration behavior does not happen if using only a small P value right?
If it does, this probably means that the velocity calculations is noisy. Then increase the encoder elapsed time parameter even more, but I guess that you’ve tried this right?

If the motor is stable for small P values and then becomes unstable for higher values, then it might be an issue of choosing the appropriate PID values.

What you can also tey is to down sample the motion control loop using the motor.motion_downsample parameter. Try setting it to 5-10 or so. This will run the motor move each 5-10 loops, making the time between the calls a bit longer and possibly making the PID tuning a bit easier.

Anupam_Das · March 3, 2025, 9:55am

Hello Antun. Quick update. So indeed it was a PID tuning issue. I was able to get stable PID velocity control but only for higher speeds. At lower speeds the rotation is unstable. I am still new to all of this but apparently low speed jitters/cogging is a common problem with non-obvious solutions. What comes to mind is to turn off PID below a threshold…etc. However with this specific encoder/bldc motor combo, im surprised im unable to get good behavior at lower speeds because This is the exact same motor and encoder you used in your self balancing robot and surely in your robot the wheel is very smooth across all ranges of wheel speeds. The only thing different in my setup is that im using a mini which does not have current sensing. . Ive tuned the PID the best I could, I’ll still need to exports some plots and post on here but if you have any initial thoughts let me know. Here is the latest code. For target speeds < 20 I start to get ocelations. FOCTests/src/main.cpp at master · anupamdas2012/FOCTests · GitHub

Antun_Skuric · March 3, 2025, 10:15am

Nice!
I’m glad to hear that!

So 20rad per second is not really low speed, you should be able to get good velocity control loop all the way down to 1rad per second even less.

Id say it’s still the issue of the velocity measurement, please try to plot it somehow

If that’s the issue you should be able to get better control if you tey using higher encoder.min_elapsed_time (try setting up to 10-20ms) and motor.motion_downsample (try setting 10 up to 150)

For the PID tuning, start with only P. If the velocity is not noisy, you should easily have vers table behavior (not very good following of the target velocity) but very stable. Then add some I slowly.

Good luck

Anupam_Das · March 4, 2025, 9:41am

Greetings Antun,
So, turns out I was using the 5v pin on my Teensy to power the encoder. I switch that to using the 3v3 pin and practically all of the noise went away and now close loop velocity is silky smooth across all RPM range. The PID tuning parameters from your examples and suggestions now are starting to make sense and show the behavior i would expect where as before there was just noise in the system. I know many of your examples use regulated logic voltage from the MCU, but in a “production” environment is it recommended to use a separate regulated power source for logic instead of using logic power from the MCU?

This was a gigantic hurdle, thanks for all your help.
On to Closed loop angle control!

Anupam_Das · March 5, 2025, 3:56am

@Antun_Skuric I have posted a video of my learnings and prototype so far, hopefully it is helpful to the community.