Teensy 4.0 FOC Current Control with AS5048A magnetic encoder


I am trying to get the Torque control option working, with the FOC Current Example using a Teensy 4.0.

I got the basic examples working, SPI AS5048 working with the teensy and the open loop position and velocity examples working.

Firstly I wanted to know if the FOC Current example is compatible with the Magnetic encoders and the only example is under the standard ‘encoder’. My test code where I have tried to implement this myself gives me the error of:

MOT: Monitor enabled!
MOT: Init
MOT: Enable driver.
MOT: Align sensor.
MOT: sensor_direction==CW
MOT: PP check: OK!
MOT: Zero elec. angle: 1.41
MOT: Align current sense.
CUR: No driver linked!
MOT: Align error!
MOT: Init FOC failed.
Motor ready.

Here is my code (*Written by myself so most likely an init issue by how I have called stuff)

  • I have called sensor.init() twice for the magnetic sensor but this seemed to also work for the other examples so I kept it as is.
#include <SimpleFOC.h>

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
//BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); //UNO Pinout
BLDCDriver3PWM driver = BLDCDriver3PWM(4, 6, 5, 7); //Teensy Pinout

//MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, 10);
MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
// current sensor
InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2);

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

void setup() { 
  // initialise magnetic sensor hardware
  // link the motor to the sensor

  // initialise the current sensing
  // for SimpleFOCShield v2.01/v2.0.2
  current_sense.gain_b *= -1;
  //current_sense.skip_align = true;
  // set torque mode:
  motor.torque_controller = TorqueControlType::foc_current; 
  // set motion control loop to be used
  motor.controller = MotionControlType::torque;

  // current sense init hardware
  // link the current sense to the motor
  // foc current control parameters (Arduino UNO/Mega)
  motor.PID_current_q.P = 5;
  motor.PID_current_q.I= 300;
  motor.PID_current_d.P= 5;
  motor.PID_current_d.I = 300;
  motor.LPF_current_q.Tf = 0.01; 
  motor.LPF_current_d.Tf = 0.01; 

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  // link driver

  // use monitoring with serial 
  // comment out if not needed
  // initialize motor
  // align sensor and start FOC
  // initialise magnetic sensor hardware
  // add target command T
  command.add('T', doTarget, "target current");

  Serial.println(F("Motor ready."));

void loop() {

  float current_magnitude = current_sense.getDCCurrent();
    Serial.print(current_magnitude*1000); // milli Amps
    //Magnetic Encoder
    // display the angle and the angular velocity to the terminal

  // main FOC algorithm function

  // Motion control function

  // user communication

Any help would be appreciated, especially any stupid errors on my part.

Overall I am creating a CubeSat reaction wheel, so hopefully an IMU angle reading can be fed into the position control method and rotational velocity can be fed into the velocity control method for attitude control and de-tumbling.

I am only doing a 1-axis reaction wheel and have access to a 1-dof setup/testing system.

Here is my setup:

The AS5048 works correctly, so the SPI pinout works, the purple and blue wires are A0 and A2 respectively and I have connected the GND and 3.3V between boards as the teensy is 3.3V logic level, and have changed over the 0 Ohm resistor underneath.

Thank you,

And here is the underside of my SimpleFOCshield V2.0.4.

I think you also need:


before motor.initFOC() is called

Thank you Owen for you reply,

I added the lines of code before the initFOC and now the motor starts up, however it seems to just ramp up to full speed without any warning.
Also when I enter the target current (assuming mA but have also tried it as A) T150mA (I have also tried T1) for example it doesn’t change or give a reasonable reading for the current and still just spins out of control.

When using the Target Velocity example I have much more control of the motor.

Any ideas as to why it could be ramping up out of control and how to keep it within reasonable values?

Thank you,

The target is in Amps.
Without load, the torque will quickly overcome the friction and speed fast.
In my case with a hoverboard motor it will start to spin at around 0.42A, and will go to full speed already at 0.5A.
I can only really appreciate the change of torque by applying load.

Thank you, so would adding mass to the motor in the form of a reaction wheel structure be enough to apply a load? in the order of a few hundred grams?


Also make sure the current sense works properly before using foc_current.
If current sense doesn’t work and measured Q axis current is 0A, even if the torque target is only 0.1A, the PI controller will increase the output voltage to reach 0.1A which will never happen.

For a cube sat / reaction wheel i would have thought velocity control (perhaps with current limit) was more appropriate?

Most of the time you want a steady velocity.

This will also mean you won’t need to worry about going to max speed as speed will be your target (not current). Maybe you want to control the current limit with an outer loop.

Perhaps someone else can chime in on what inner/outer loops might be appropriate (or whether your current approach is best!)

Hi Owen,

I want to be able to change the direction that the CubeSat is pointing, and relating the speed and inertia of the wheel and CubeSat, the thing I can change is the torque produced by the wheel to obtain this change in rotational speed of the CubeSat which is the same thing as the current in this case. That’s the current approach I am aiming for but I’m welcome to any easier ideas.

For de-tumbling the CubeSat, I’m thinking of a speed control but feeding sensor data from an IMU (Combining the Acc and Gyro data) to control the speed or come up with an equation that relates the two.

This is example code I’m using to set a reference speed and measure the current magnitude, position of the motor (not really needed here), and the measured speed.

Here is the serial output

Current, Position (Rad), Velocity (Rads-1)
133.91 2924.42 23.38
137.35 2924.42 23.38
133.91 2924.42 16.37
133.91 2924.42 16.37
175.02 2924.43 28.06
158.47 2924.43 28.06
167.04 2924.43 18.71
170.84 2924.43 18.71
159.37 2924.43 18.71
157.69 2924.43 18.71
154.69 2924.43 11.69
177.96 2924.44 11.69
148.87 2924.44 23.38
157.69 2924.44 23.38

The current does seem to go all over there place, I’m assuming as the sensor measurements can be noisy, do you have any recommendations to mitigate this?

Here is the output value for the current from the power supply. It is a solid state power supply as I had issues with the other types and relays.

  • Harvey

Can you visualize DQ currents in simplefocstudio