Link

Steer by wire
using SimpleFOCShield and Arduino UNO

Connecting everything together

Here is a picture of the setup used in this project. Check the Arduino code to see the pin numbers that are used.

Arduino code

The code for this example is very simple. Since we use two BLDC motors we will need to use all 6 pwm Arduino UNO pins which means that we do not have enough hardware interrupt pins. Therefore we will need to use software interrupts. In this example, we will use PciManager library. It is very simple to use and I definitely recommend it, but it can be any other respectively.

Both motors will be controlled in the torque/voltage control mode and one motor will have encoder as a sensor, the other will be using the magnetic sensor with I2C communication (AS5600). After both motors and sensors are initialized, no configuration is necessary, we can initialize FOC algorithm for both motors initFOC() and we are ready to go to real-time execution.

The smart stuff is placed in the loop() function. 😄

In order to maintain the virtual link in between two motors positions we write the code:

motor1.move( 5*(motor2.shaft_angle - motor1.shaft_angle));
motor2.move( 5*(motor1.shaft_angle - motor2.shaft_angle));

This control algorithm sets the voltage to the motors, proportional to their distance from the position of the other motor. Constant 5 is a scaling gain, it is found empirically and it will differ for each setup. But in general, the higher the number the stiffer the link, it will the more hard to introduce an offset in between motors positions.

And that is it, here is the full code of this example.

#include <SimpleFOC.h>
// software interrupt library
#include <PciManager.h>
#include <PciListenerImp.h>

BLDCMotor motor1 = BLDCMotor(11);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(3, 10, 6, 7);
Encoder encoder1 = Encoder(A2, 2, 500, A0);
void doA1(){encoder1.handleA();}
void doB1(){encoder1.handleB();}
void doI1(){encoder1.handleIndex();}

// encoder interrupt init
PciListenerImp listenerA(encoder1.pinA, doA1);
PciListenerImp listenerB(encoder1.pinB, doB1);
PciListenerImp listenerI(encoder1.index_pin, doI1);

BLDCMotor motor2 =  BLDCMotor( 11);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(9, 11, 5, 8);
MagneticSensorI2C sensor2 = MagneticSensorI2C(0x36, 12, 0x0E, 4);

void setup() {

  // initialise encoder hardware
  encoder1.init();  
  // interrupt initialization
  PciManager.registerListener(&listenerA);
  PciManager.registerListener(&listenerB);
  PciManager.registerListener(&listenerI);
  // link the motor to the sensor
  motor1.linkSensor(&encoder1);

  // init driver
  driver1.init();
  // link driver
  motor1.linkDriver(&driver1);

  // set control loop type to be used
  motor1.controller = MotionControlType::torque;
  // initialise motor
  motor1.init();
  // align encoder and start FOC
  motor1.initFOC();
  
  // initialise magnetic sensor hardware
  sensor2.init();
  // link the motor to the sensor
  motor2.linkSensor(&sensor2);
  // init driver
  driver2.init();
  // link driver
  motor2.linkDriver(&driver2);
  // set control loop type to be used
  motor2.controller = MotionControlType::torque;
  // initialise motor
  motor2.init();
  // align encoder and start FOC
  motor2.initFOC();
  
  Serial.println("Steer by wire ready!");
  _delay(1000);
}

void loop() {
  // iterative setting FOC phase voltage
  motor1.loopFOC();
  motor2.loopFOC();

  // virtual link code
  motor1.move( 5*(motor2.shaft_angle - motor1.shaft_angle));
  motor2.move( 5*(motor1.shaft_angle - motor2.shaft_angle));
  
}

Haptic velocity control
using SimpleFOCShield and Stm32 Nucleo-64

Connecting everything together

Here is a picture of the setup used in this project. Check the Arduino code to see the pin numbers that are used.

Arduino code

In this example we will be using the Nucleo-64 board and two BLDC motors with encoders. Since Nucleo doesn’t have problems with hardware interrupts (every pin can be external interrupt pin), there is no complications in code due to usage of 6 pwm pins. The only difference in between Nucleo and Arduino UNO is that Nucleo pin cannot use its pin 11 for PWM so we need to use pin 13 instead. The rest of the code is very straight forward.

We define two motors and two encoders and link them together.

#include <SimpleFOC.h>

BLDCMotor motor1 = BLDCMotor(11);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 6, 5, 7);
Encoder encoder1 = Encoder(A1, A2, 8192);
void doA1(){encoder1.handleA();}
void doB1(){encoder1.handleB();}


BLDCMotor motor2 = BLDCMotor(11);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 13, 10, 8);
Encoder encoder2 = Encoder(A3, 2, 500);
void doA2(){encoder2.handleA();}
void doB2(){encoder2.handleB();}


void setup() {

  // initialise encoder hardware
  encoder1.init();
  encoder1.enableInterrupts(doA1,doB1);
  
  encoder2.init();
  encoder2.enableInterrupts(doA2,doB2);
  // link the motor to the sensor
  motor1.linkSensor(&encoder1);
  motor2.linkSensor(&encoder2);
  
  // config drivers
  driver1.init();
  driver2.init();

  // link the motor to the driver
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);
}
void loop(){}

Then we define the motion control to be voltage for one motor and the velocity for the other:

// set control loop type to be used
motor1.controller = MotionControlType::torque;
motor2.controller = MotionControlType::velocity;

Additionally we introduce a bit higher degree of filtering by augmenting Tf value and rise a bit the integral gain I as well for better following.

// augment filtering
motor2.LPF_velocity.Tf = 0.02;
// rise I gain
motor2.PID_velocity.I = 40;

And to finish the setup() we just initialize the motors and FOC algorithm.

The smart stuff is again in the the loop() function. The virtual link code looks like this:

// virtual link code
motor1.move(5*(motor2.shaft_velocity/10 - motor1.shaft_angle));
motor2.move(10*motor1.shaft_angle);

This control strategy basically says that the target velocity of the motor2 will be proportional to the position of the motor1. On the other hand, it sets the voltage to the motor1 proportional to the difference in between the difference on the velocity of the motor2 and position of the motor1. This creates a virtual link in between those two variables.

Constant 5 is the scaling gain with the same role as in the previous example. It will just make the motor1 be more or less responsive while following the motor2 velocity and it will want to maintain the difference in 0.

Constant 10 is a bit different. It is a scaling factor which helps to better map velocity to the positon. For example in the example we are using the motor2 has maximal velocity of 60rad/s, but we don’t want our gauge to rotate 10 rotations to show this velocity. we would like it to rotate maximally 1 rotation ~6rad therefore the constant 10. But in your case maybe you will be running a drone motor, which turns with thousands of RPM and you might want to have even larger scaling 100 or even a 1000. On the other hand, maybe you will want to have a very precise slow moving motor which will go under 1 radian/s. And you will want to go to the values of ~0.1 or even less. Therefore this will depend on your application and the precisions you will need to have.

One more interesting thing to notice is that this scaling factor can be variable, so you could change it in real-time as well.

Here is the full example code:

#include <SimpleFOC.h>

BLDCMotor motor1 = BLDCMotor(11);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 6, 5, 7);
Encoder encoder1 = Encoder(A1, A2, 8192);
void doA1(){encoder1.handleA();}
void doB1(){encoder1.handleB();}


BLDCMotor motor2 = BLDCMotor(11);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 13, 10, 8);
Encoder encoder2 = Encoder(A3, 2, 500);
void doA2(){encoder2.handleA();}
void doB2(){encoder2.handleB();}

void setup() {

  // initialise encoder hardware
  encoder1.init();
  encoder1.enableInterrupts(doA1,doB1);
  
  encoder2.init();
  encoder2.enableInterrupts(doA2,doB2);
  // link the motor to the sensor
  motor1.linkSensor(&encoder1);
  motor2.linkSensor(&encoder2);
  
  // config drivers
  driver1.init();
  driver2.init();
  // link the motor to the driver
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);
    
  // set control loop type to be used
  motor1.controller = MotionControlType::torque;
  motor2.controller = MotionControlType::velocity;

  motor2.LPF_velocity.Tf = 0.02;
  motor2.PID_velocity.I = 40;

  // use monitoring with serial for motor init
  // monitoring port
  Serial.begin(115200);
  // enable monitoring
  motor1.useMonitoring(Serial);
  motor2.useMonitoring(Serial);

  // initialise motor
  motor1.init();
  motor2.init();
  // align encoder and start FOC
  motor1.initFOC();
  motor2.initFOC();

  Serial.println("Interactive gauge ready!");
  _delay(1000);
}


void loop() {
  // iterative setting FOC phase voltage
  motor1.loopFOC();
  motor2.loopFOC();

  // iterative function setting the outter loop target
  motor1.move(5*(motor2.shaft_velocity/10 - motor1.shaft_angle));
  motor2.move(10*dead_zone(motor1.shaft_angle));
  
}

float dead_zone(float x){
  return abs(x) < 0.2 ? 0 : x;
}