I have already rebuilt a pendulum with magnetic sensors AS5600, an Arduino nano, BLDC motor 2804 100KV and driver L6234. You can find a short video here after and the adapted code that I improved a little to make the swing-up more efficient .
Because it is working well, I thought that would be obvious to replace the pendulum encoder with an IMU6050. As said I would like to remove the encoder axis and get the pendulum standing vertical on the desk with two contact points aligned. In the Github it was indicated that we “could use the IMU and measure the inclination by combining the accelerometer and the gyro measurement”. So, I started by install the IMU on the pendulum but unfortunately I am struggling a lot with libraries to make working it with the simpleFOC library and also to set the system. Is someone has already done this or could advise me for modifying the code?
#include <SimpleFOC.h>
// Motor sensor
MagneticSensorAnalog sensorA = MagneticSensorAnalog(A7, 14, 1020); // motor axis
// Pendulum sensor
MagneticSensorI2C sensorB = MagneticSensorI2C(AS5600_I2C); // pendulum axis
// BLDC motor init
BLDCMotor motor = BLDCMotor(7);
// driver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); // 3 phase & enable
float LowPosOffset = 0; // self detect vertical
// function constraining the angle in between -pi and pi, in degrees -180 and 180
float constrainAngle(float x){
x = fmod(x + M_PI, _2PI);
if (x < 0)
x += _2PI;
return x - M_PI;
}
// LQR stabilization controller functions
// calculating the voltage that needs to be set to the motor in order to stabilize the pendulum
float controllerLQR(float p_angle, float p_vel, float m_vel){
// if angle controllable
// calculate the control law
// LQR controller u = k*x
// - k = [40, 7, 0.3]
// - x = [pendulum angle, pendulum velocity, motor velocity]'
float u = 60*p_angle + 7*p_vel + 0.3*m_vel; //40;7;0.3
// limit the voltage set to the motor
if(abs(u) > motor.voltage_limit*0.7) u = _sign(u)*motor.voltage_limit*0.7;
return u;
}
void setup() {
// monitoring port
Serial.begin(115200);
// initialise magnetic sensor hardware
sensorA.init();
sensorB.init();
//
// link the motor to the sensor
motor.linkSensor(&sensorA);
// set control loop type to be used
motor.controller = MotionControlType::torque;
// link the motor to the encoder
motor.linkSensor(&sensorA);
// driver
driver.voltage_power_supply = 12;
driver.init();
// link the driver and the motor
motor.linkDriver(&driver);
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// sensor bottom position
sensorB.update();
LowPosOffset = PI - sensorB.getAngle();
}
// loop downsampling counter
long loop_count = 0;
float target_voltage;
void loop() {
// ~1ms
motor.loopFOC();
// control loop each ~20ms
if(loop_count++ > 20){
// updating the pendulum angle sensor
// NECESSARY for library versions > v2.2
sensorB.update();
// calculate the pendulum angle
float pendulum_angle = constrainAngle(sensorB.getAngle() + LowPosOffset);
if( abs(pendulum_angle) < 0.4 ) // if angle small enough stabilize
target_voltage = controllerLQR(pendulum_angle, sensorB.getVelocity(), motor.shaftVelocity());
else // else do swing-up in both directions
// target_voltage = -_sign(sensorB.getVelocity())*motor.voltage_limit*0.4;
if (_sign(sensorB.getVelocity()) == 0){
target_voltage = - motor.voltage_limit*0.3;
}
else{
target_voltage = motor.voltage_limit*0.3;
}
// set the target voltage to the motor
motor.move(target_voltage);
// restart the counter
loop_count=0;
}
}
Nicely working pendulum, the video has already been is suggestions for me on youtube
So in terms of using an imu, in my mind the biggest issue is the low memory of the Arduino Nano. As simplefoc is relatively large library and imu6050 ones are too, I imagine that you’re having hard time to include both?
I’ve tried several ways of using IMUs with for balancing:
Calculating the angle only from the accelerometers
Fusing accelerometer with a gyro using a simple kalman or complementary filter
Using the imu6050’s integrated dmp
In my setups the best choice was always to use the DMP. Like in the SimpleFOCbalancer. However it requires using a specialised library which is realtively heavy in terms of the memory usage. So it might require you to use another microcontroller.
Otherwise I’d suggest you to use a complementary filter and use the i2c directly to read the gyro and acceleration data. This should work good enough. But you’ll need to tune your filters a bit.
Here is a quick read that might be interesting.
Another issue that I’ve had a lot is due to the electromagnetic fields that are generated by the motor and their influence on the I2C communication. I’ve had a lot of them and I do not really have a good solution to it. Having your motor as far as possible from the imu and the mcu and reducing the i2c communication cable length usually helps though.
Many thanks for your prompt answer, advice, and links. About memory issues I agree with you that was challenging with the Arduino Nano. I suppose that I should definitely change the hardware and move to a more powerful board like ESP32 for instance.
It seems to me that inertia wheel pendulums are particularly sensitive to tiny measurement oscillations and requires a good filtering of the angles data. However, filters slow down too much the process on Arduino and jeopardize the motor control when you jump to the calculating angle loop, that is finally degrading all.
I will go trough the links and let you knows if I succeed to use this nice SimpleFOC library because my final goal would be to build a dual axis self-balancing Inverted stick.
I have a totally different application and precision is not so important in my case, but I noticed that calibrating the IMU first makes a huge difference, regarding the accuracy of derived angles.
Also we are really lucky to have the all time champion of inverted pendulums and unstable systems at the forum that we can ask for opinion @Rem666
@Rem666 we’d love to hear about your software stack that you use in your projects.
As far as I could understand, you are not using the IMU6050’s dmp, right?
Yes, I don’t use dmp. Because reading data from the MPU6050 is very simple. I tried it once - it works well, so I usually use it that way.
But I’ve used dmp before for other projects (without SimpleFOC). This is also a great option.
Here is the last code that I tried based on the MPU6050_tockn library that I needed modify with millis()/32 because the frequency of timer 0 was changed with simpleFOC. It doesn’t use dmp and looks to work but I never succeeded stabilize correctly whatever the settings. A more surprising behaves is when I uncomment the serial.print lines the system works much better. But up to now I have not found the reason in the code.
// Draft of inertia wheel pendulum with IMU6050 link to the pendulum and MPU6050_tockn library modified
#include <SimpleFOC.h>
#include <MPU6050_tockn.h> // modified: millis()/32 in MPU6050_tockn.cpp
#include <Wire.h> // already done by SimpleFOC with magnetic sensor if I2C used
MPU6050 mpu6050(Wire);
// Motor sensor
MagneticSensorAnalog sensorA = MagneticSensorAnalog(A7, 14, 1020);
// Pendulum sensor
//MagneticSensorI2C sensorB = MagneticSensorI2C(AS5600_I2C); // simplified configuration constructor for AS5600 12-bit sensor
// BLDC motor init
BLDCMotor motor = BLDCMotor(7);
// driver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
// function constraining the angle in between -pi and pi, in degrees -180 and 180
float constrainAngle(float x){
x = fmod(x + M_PI, _2PI);
if (x < 0)
x += _2PI;
return x - M_PI; //
}
// LQR stabilization controller functions
// calculating the voltage that needs to be set to the motor in order to stabilize the pendulum
float controllerLQR(float p_angle, float p_vel, float m_vel){
// if angle controllable
// calculate the control law
// LQR controller u = k*x
// - k = [40, 7, 0.3]
// - x = [pendulum angle, pendulum velocity, motor velocity]'
float u = 60*p_angle + 7*p_vel + 0.5*m_vel; //40;7;0.3 // 0.05
// limit the voltage set to the motor
if(abs(u) > motor.voltage_limit*0.9) u = _sign(u)*motor.voltage_limit*0.9; //0.7
return u;
}
float LowPosOffset = 0;
void setup() {
// monitoring port
Serial.begin(115200);
// IMU 6050 initialise
Wire.begin();
mpu6050.begin();
mpu6050.calcGyroOffsets(true); //-2.56, 1.84, 0.89
mpu6050.update();
LowPosOffset = mpu6050.getAngleX(); // get vertical when pendulum is in reverse pos
if (LowPosOffset>0)
LowPosOffset = LowPosOffset - 180;
else
LowPosOffset = LowPosOffset + 180;
// initialise magnetic sensor hardware
sensorA.init();
// link the motor to the sensor
motor.linkSensor(&sensorA);
// set control loop type to be used
motor.controller = MotionControlType::torque;
// link the motor to the encoder
motor.linkSensor(&sensorA);
// driver
driver.voltage_power_supply = 12;
driver.init();
// link the driver and the motor
motor.linkDriver(&driver);
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
}
// loop downsampling counter
long loop_count = 0;
float target_voltage;
void loop() {
// ~1ms
motor.loopFOC();
// control loop each ~25ms
if(loop_count++ > 15){ //20
// updating the pendulum angle sensor
mpu6050.update();
// calculate the pendulum angle
float AngleRadX = (mpu6050.getAngleX()-LowPosOffset)*71/4068; // convert deg to rad
float pendulum_angle = constrainAngle(AngleRadX);
float pendulum_vel = mpu6050.getGyroX()*71/4068; // convert vel deg to rad
// Serial.print("\tpendulum_angle : ");
// Serial.print(pendulum_angle); //pendulum_angle
// Serial.print("\tpendulum_vel : ");
// Serial.println(pendulum_vel); // pendulum_velocity
if( abs(pendulum_angle) < 0.3 ) {// if angle small enough stabilize
target_voltage = controllerLQR(pendulum_angle, pendulum_vel, motor.shaftVelocity());
// Serial.print("\ttarget_voltage : ");
// Serial.println(target_voltage);
}
else { // else do swing-up
// target_voltage = 0;
if (_sign(pendulum_vel) == 1){
target_voltage = - motor.voltage_limit*0.35;
}
else{
target_voltage = + motor.voltage_limit*0.35;
}
}
// set the target voltage to the motor
motor.move(target_voltage);
// restart the counter
loop_count=0;
}
}
I don’t use this library, but it looks like it’s practically the same as reading the data directly.
Serial.print is slow, which is probably why commenting it out the balancing improves.
Anyway, there is no big difference in how the tilt angle is obtained, from the encoder or from the IMU - the angle will be the same.
