Hi Guys,
I have rebuilt the reaction wheel inverted pendulum with some different parts.
Encoder: [https://shop.iflight-rc.com/as5048a-magnetic-encoder-pro262](https://AS5048A Magnetic Encoder)
I have adjusted the code from GitHub (GitHub - simplefoc/Arduino-FOC-reaction-wheel-inverted-pendulum: Reaction wheel inverted pendulum project based on the Arduino Simple FOC library and SimpleFOC shied.) so I can use the SPI connection the encoder offers. Right now I am trying to adjust the LQR controller and would appreciate some help.
#include <SimpleFOC.h>
// BLDC motor init
BLDCMotor motor = BLDCMotor(11);
// driver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
// Motor encoder init
MagneticSensorSPI encoder = MagneticSensorSPI(12, 14, 0x3FFF);
// pendulum encoder init
MagneticSensorSPI pendulum = MagneticSensorSPI(11, 14, 0x3FFF);
float pendulumoffset = 0;
void setup() {
Serial.begin(115200);
// initialise motor encoder hardware
encoder.init();
// init the pendulum encoder
pendulum.init();
pendulumoffset = pendulum.getAngle();
// set control loop type to be used
motor.controller = MotionControlType::torque;
// link the motor to the encoder
motor.linkSensor(&encoder);
// driver
driver.voltage_power_supply = 12;
driver.init();
// link the driver and the motor
motor.linkDriver(&driver);
// limiting motor movements
//motor.phase_resistance = 11.1; // [Ohm]
//motor.current_limit = 1.5; // [Amps] - if phase resistance defined
// 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++ > 25){
// updating the pendulum angle sensor
// NECESSARY for library versions > v2.2
pendulum.update();
// calculate the pendulum angle
float pendulum_angle = constrainAngle((pendulum.getAngle()-pendulumoffset) + M_PI);
Serial.print(pendulum_angle);
Serial.print("\n");
if( abs(pendulum_angle) < 0.35 ) // if angle small enough stabilize
target_voltage = controllerLQR(pendulum_angle, pendulum.getVelocity(), motor.shaftVelocity());
else // else do swing-up
// sets 45% of the maximal voltage to the motor in order to swing up
target_voltage = -_sign(pendulum.getVelocity())*motor.voltage_limit*0.45;
// set the target voltage to the motor
motor.move(target_voltage);
// restart the counter
loop_count=0;
}
}
// 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, 15, 0.35]
// - x = [pendulum angle, pendulum velocity, motor velocity]'
float u = 40*p_angle + 15*p_vel + 0.35*m_vel;
// 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;
}
With the gains given in the example code the pendulum didn’t even came close to be controlled. I figured my motor could be heavier so played with the pendulum velocity and motor velocity. Right now I use the following gains:
k = [40, 15, 0.35]
x = [pendulum angle, pendulum velocity, motor velocity]
u = 40p_angle + 15p_vel + 0.35*m_vel
If I use this gains the pendulum almost stays in a steady state. I have recorded a video but it I cant upload it to this website. I have created a share link to download the video (Preformatted texthttps://we.tl/t-0wcZyYgu9f).
If I’m not mistaken the original reaction wheel is your project @Antun_Skuric?
Do you have any suggestions how I can get the pendulum to a steady state?
Thanks in advance.
Greetings from Tyrol.
Raphael