 # Swing Up Control of Reaction Wheel Inverted Pendulum

Hello Everyone,

I am working on a project to stabilize a reaction wheel pendulum. It is a type of inverted pendulum but unlike moving cart type this pendulum has a reaction wheel mounted on the top.Torque is produced by the change of the angular momentum of the reaction wheel. This is a non linear system. To stabilize this system basically there are 3 control algorithms involved;

1. Stabilizing pendulum about the upright position using PID, LQR,LQG etc.(Linear Control)
2. Swinging the pendulum from the stable to the unstable equilibrium(Non linear Control)
3. Switching between the two controllers at a proper angular position

As of now I have done the first part i.e stabilizing about the upright position. I made use of the Encoder and the PID Library developed by fellow Arduino forum members.In the PID control the set point was set as 180 degrees and then the error terms were fed into the algorithm and appropriate gains were found out. Now when I take the pendulum to the upright position it is able to stabilize itself.

Now I have to swing the pendulum from the unstable position to the upright position and then balance it. This is not that easy because the dynamics is non linear. Reading some papers (By Prof. Spong and Furuta)I have found out that using a bang bang controller with a energy based controller we will be able to swing it with appropriate velocity.Then by switching between the stabilizing controller at upright position it can be balanced.

A bang bang controller is basically a on and off controller.Currently I am planning to implement only bang bang and later on integrate it with the energy based controller. To swing up the pendulum using bang bang controller we need to find the rate of change of angle of the pendulum i.e theta dot. So to swing up the pendulum we apply torque in the direction of the swing. If reaction wheel moves clockwise the pendulum moves anticlockwise and vice versa.We assume clockwise angle to be positive.So when the pendulum moves clockwise the rate of change of angle will be positive and negative when moving anticlockwise. Thus the motor driving the reaction wheel can be given voltage and direction from the Motor Driver and Arduino accordingly. The attached image will give you clear insight of what I am planning to do.

I am hoping that my concepts are right but I am having a hard time implementing the algorithms in the hardware.I have to admit that this is my first time I am working extensively on a microcontroller so I am not that great in Arduino programming. I hope my fellow Arduino forum members will help me with this one.Thank you very much. This is the code that I have made for the swing up function.

``````#include <Encoder.h>
#define dT 0.0001
//Variable definition
double prevPos=0,currentPos;
double rate;
const int dirA=8;
const int dirB=12;
const int motorPWM=11; // This will be connected to the enable pin on the H-bridge
Encoder motEnc(2,3);

void setup()
{
Serial.begin(9600);
pinMode(dirA,OUTPUT);
pinMode(dirB,OUTPUT);
pinMode(motorPWM,OUTPUT);
}

void loop()
{
currentPos = motEnc.read()*0.09; // Since the encoder generates 4000 pulses in 1 revolution 4000 pules= 360 degree Hence, 1 pulse=0.09 degree
Serial.println(rate);
rate=(currentPos-prevPos)/dT;
prevPos=currentPos;
if(rate>0){
digitalWrite(dirA,HIGH);
digitalWrite(dirB,LOW);
analogWrite(motorPWM,200);
delay(50);

}
else if(rate<=0){
digitalWrite(dirA,LOW);
digitalWrite(dirB,HIGH);
analogWrite(motorPWM,200);
delay(50);

}

}
`````` Actually the main problem right now is to correctly measure the rate of change of the angle.I know that the way I calculated the rate of change of angle in above code is totally wrong as I have randomly taken a sample time. Below is the latest code that I’ve worked out on to find the rate of change of the angle.

``````#include <Encoder.h>
#include <TimerOne.h>
volatile double prevPosition=0,newPosition;
volatile double rate;
Encoder myEnc(18, 19);
void setup() {
Serial.begin(9600);
Serial.println("Basic Encoder Test:");
Timer1.initialize(4000); // set a timer of length 4000 microseconds (or 4 ms - or 250Hz)
Timer1.attachInterrupt(callback); // attach the service routine here
}
void loop() {

Serial.println(rate);
}

void callback()
{
if(newPosition>4000)// I am using a 1000 PPR encoder with 4X decoding
myEnc.write(0);
if(newPosition<-4000)
myEnc.write(0);
rate=((newPosition-prevPosition)*0.01744*0.09)/0.004;//0.004 second is the sample time and we are converting the velocity in radians per second 1 degree=0.01744 radians and 1 pulse=0.09 degree since 360 degree=4000 pulses
prevPosition=newPosition;
}
``````

This is the another code that I’ve worked out but the problem is when I move the pendulum in anticlockwise direction from the stable direction then it shows a value of 65535.

``````#define encoder0PinA 2
#define encoder0PinB 3
volatile double dP,rate;
volatile double currentPos=0;
volatile unsigned previousPos;
volatile unsigned int encoder0Pos = 0;
volatile unsigned long duration,LastCapTime;
volatile unsigned long CapTime=0;
void setup() {
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, Achange, CHANGE);
// encoder pin on interrupt 1 (pin 3)
attachInterrupt(1, Bchange, CHANGE);
Serial.begin (9600);
}

void loop(){
dP = (double)(currentPos - previousPos);
duration = CapTime - LastCapTime; //Get the period in ms
rate=dP/duration;
Serial.println(rate);
}

void Achange(){
// look for a low-to-high on channel A
// check channel B to see which way encoder is turning
encoder0Pos++;         // CW
}
else {
encoder0Pos-- ;        // CCW
}
}
else   // must be a high-to-low edge on channel A
{
// check channel B to see which way encoder is turning
encoder0Pos++;          // CW
}
else {
encoder0Pos--;          // CCW
}
previousPos=currentPos;
currentPos=encoder0Pos;
LastCapTime = CapTime;
CapTime = millis();
}
}

void Bchange(){
// look for a low-to-high on channel B
// check channel A to see which way encoder is turning
encoder0Pos++;         // CW
}
else {
encoder0Pos--;         // CCW
}
}
// Look for a high-to-low on channel B
else {
// check channel B to see which way encoder is turning
encoder0Pos++;          // CW
}
else {
encoder0Pos--;          // CCW
}
}
previousPos=currentPos;
currentPos=encoder0Pos;
LastCapTime = CapTime;
CapTime = millis();
}
``````

Which one from the above two would be a better approach to find out the rate of change of the pendulum angle? Thank you.

The "best" solution depends on the timing requirements - integer arithmetic is faster than float math. Since the encoder produces discrete steps, at least the position variables can be integers, for fast handling in the ISR (callback). The same for the time, in discrete ms steps.

Your 65535 problem is caused by unsigned variables, where negative values (positions) wrap around to the maximum int value. The solution is simple: use

``````//these are updated in the ISR, must be volatile
volatile int currentPos=0, previousPos=0, encoder0Pos = 0; //int or long as appropriate
volatile unsigned long LastCapTime, CapTime=0;

//everything else not volatile
unsigned long duration; //all times are unsigned long
double dP,rate;
...
``````

Variables updated in the ISR should be volatile, and should be accessed outside the ISR with interrupts disabled.

``````void loop(){
noInterrupts(); //copy volatile values
int idP = currentPos - previousPos;
duration = CapTime - LastCapTime; //Get the period in ms
interrupts();
dP=idP; //possibly lengthy conversion
rate=dP/duration;
Serial.println(rate);
}
``````

Thank you very much for the clear explanation and modification on the previous code. Another thing,Do you think that the previous code that I wrote using the Encoder.h library should work as well?

Now, as we have figured out a way to find the rate of change of angle we can do the swing up of the pendullum.This is the code I have developed using the concept you’ve described. I am not sure if the logic is correct though.

``````#define encoder0PinA 2
#define encoder0PinB 3
const int dirA=8;
const int dirB=12;
const int motorPWM=11; // This will be connected to the enable pin on the H-bridge
volatile int currentPos=0, previousPos=0, encoder0Pos = 0; //int or long as appropriate
volatile unsigned long LastCapTime, CapTime=0;
//everything else not volatile
unsigned long duration; //all times are unsigned long
double dP,rate;
void setup() {
pinMode(dirA,OUTPUT);
pinMode(dirB,OUTPUT);
pinMode(motorPWM,OUTPUT);
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, Achange, CHANGE);
// encoder pin on interrupt 1 (pin 3)
attachInterrupt(1, Bchange, CHANGE);
Serial.begin (9600);
}

void loop(){
noInterrupts(); //copy volatile values
int idP = currentPos - previousPos;
duration = CapTime - LastCapTime; //Get the period in ms
interrupts();
dP=idP; //possibly lengthy conversion
rate=dP/duration;
if(rate>0){ // Algorithm for swing up refer the image for clear explanation we are providing torque in the direction of swing if the reaction wheel mounted to motor shaft moves clockwise then pendulum moves counterclockwise and vice versa
digitalWrite(dirA,HIGH);
digitalWrite(dirB,LOW);
analogWrite(motorPWM,200);
delay(50);

}
else if(rate<=0){
digitalWrite(dirA,LOW);
digitalWrite(dirB,HIGH);
analogWrite(motorPWM,200);
delay(50);

}
}

void Achange(){
// look for a low-to-high on channel A
// check channel B to see which way encoder is turning
encoder0Pos++;         // CW
}
else {
encoder0Pos-- ;        // CCW
}
}
else   // must be a high-to-low edge on channel A
{
// check channel B to see which way encoder is turning
encoder0Pos++;          // CW
}
else {
encoder0Pos--;          // CCW
}
previousPos=currentPos;
currentPos=encoder0Pos;
LastCapTime = CapTime;
CapTime = millis();
}
}

void Bchange(){
// look for a low-to-high on channel B
// check channel A to see which way encoder is turning
encoder0Pos++;         // CW
}
else {
encoder0Pos--;         // CCW
}
}
// Look for a high-to-low on channel B
else {
// check channel B to see which way encoder is turning
encoder0Pos++;          // CW
}
else {
encoder0Pos--;          // CCW
}
}
previousPos=currentPos;
currentPos=encoder0Pos;
LastCapTime = CapTime;
CapTime = millis();
}
``````

I don't have an idea how your pendulum looks like, and what's the purpose/effect of the PWM output. Can you provide a link or picture?

Here is the descriptive image of my setup.

At normal condition the pendulum will be at θ=0 i.e stable equilibrium. We want the pendulum to be erect i.e θ=180(set point). Since the motor torque isn’t high to reach the set point at single turn we have to swing up and then once it reaches nearby θ=180 we switch to the stabilizing control and then stabilize it. In my case I am using PID controller to stabilize it. I have been able to stabilize it by taking it to the upright position. Now when the pendulum is at say 175-185 degrees it can balance itself.

But now I have to swing the pendulum to that position and then switch between the stabilizing controller. But swing up is difficult as the dynamics is non linear.So to make case simpler referring to literature I’ll be using a bang bang controller. Bang bang controller is simply an on-off controller. So how do I use a on off controller to swing up?

You see we have found a way to find the rate of change of the pendulum angle.We will be using that concept to drive the bang bang controller. Refer the image below;

Pendulum angle is measured by 1000 PPR Optical encoder.When the pendulum moves clockwise the rate of change of angle is positive and negative when moves anti-clockwise.So lets say at θ=0, when we move the reaction wheel(mounted to the shaft of DC Motor) anticlockwise for a short time then the pendulum will move clockwise,reach certain max position then begin to fall. That means while falling rate of change of angle is negative. Then we move the reaction wheel clockwise then the pendulum moves anticlockwise.It means we are assisting the pendulum to move anticlockwise when it falls. Then after it reaches max postion then again it begins to move in other direction then the rate of change of angle is positive and then we move the reaction wheel anticlockwise thus it begins to move clockwise and you see the process is iterative till it reaches the upright position. We are simply providing torque in the direction the pendulum is moving.

So basically,

1. When rate of change of angle is greater than or equal to zero move the reaction wheel anticlockwise

2. When rate of change of angle is negative move the reaction wheel clockwise

I hope I have been able to explain the concept.

P.S: In case the link doesn’t work here are the images that I have put as attachments.   Wow, I didn't know that a pendulum can be made swing this way :-)

Now I understand the idea of the bang-bang controller, but I doubt that it will allow to stop the pendulum in upright position. Instead you should use a linear or even nonlinear control algorithm, depending on the equations of the physical model, which only behaves like a bang-bang controller when the output value is clipped at the limits of the usable output range. I cannot help you with the physical model and related math, only with the implementation of an algorithm.

Glad that I was able to provide you a new idea.

Yes you are correct just with a bang bang controller we can't stabilize as the velocity will be very high and at upright position it will not be possible to catch and stabilize. Thus we have to find a way that restricts the velocity to exceed a high value.Actually to avoid the hectic maths in the non linear model they generally incorporate the bang bang controller with the energy based controller. This idea was developed by Prof. Spong and is widely used for inverted pendulum.

In the energy based controller they basically find the energy at every instance by substituting the values of rate of change of angle and other parameters like Moment of inertia,gravity,mass etc. This energy is then compared with the desired energy i.e. the energy at the upright position.

So,to the previous concept of bang bang controller we add the energy concept i.e Check the energy and if energy is less than the desired energy then activate bang bang else stop and then swing it up. If we can develop bang bang controller then the energy based controller will be few extra lines of code to activate it.

Right, it may be a good idea to range-check the error signal, so that the non/linear controller math won't run into numeric instabilities, overflows or /0 errors. But precautions may be required to make that other controller take over in a reasonable initial state, instead of a jump from e.g. zero to the current position.

Well to be frank this was the code I was previously using. This code swings up my pendulum but is not able to catch and stabilize it. However when I take the pendulum to the upright position then it is able to stabilize itself.Because I was able to find appropriate gains for the PID and when in upright position there is no use of the rate of change of angle.

I understand this is very childish approach but I am just beginning my way with microcontrollers. This code is just to show you the overall concept. Now I have to change the code to find the rate of change of angle like we previously discussed. I don’t know weather my logic towards this project is still correct or not.

``````#include <PID_v1.h>
#define dT 0.002
//Variable definition
double prevPos=0;
double currentPos,rate;
#include <Encoder.h>
//Variable definition
double setPoint, input, output;
//Specifying tuning parameters
double Kp=301.5, Ki=30.22, Kd=30.15;
const int dirA=8;
const int dirB=12;
const int motorPWM=11; // This will be connected to the enable pin on the H-bridge
PID myPID(&input, &output, &setPoint, Kp, Ki, Kd, DIRECT);
Encoder motEnc(18,19);

void setup()
{
myPID.SetMode(AUTOMATIC);
Serial.begin(9600);
setPoint=3.14;// I want the wheel mounted to the motor to exactly point 90 degree which is on the paper besides it
pinMode(dirA,OUTPUT);
pinMode(dirB,OUTPUT);
pinMode(motorPWM,OUTPUT);
myPID.SetOutputLimits(-255, 255);
myPID.SetSampleTime(5);
}

void balance(){
myPID.Compute();
if(output>=0){
digitalWrite(dirA,HIGH);
digitalWrite(dirB,LOW);
analogWrite(motorPWM,abs(output));
}
else if(output<0){
digitalWrite(dirA,LOW);
digitalWrite(dirB,HIGH);
analogWrite(motorPWM,output);
}
}
void swing(){

if(rate>0){
digitalWrite(dirA,HIGH);
digitalWrite(dirB,LOW);
analogWrite(motorPWM,200);
delay(50);

}
else if(rate<=0){
digitalWrite(dirA,LOW);
digitalWrite(dirB,HIGH);
analogWrite(motorPWM,200);
delay(50);

}

}
void loop()
{

input = motEnc.read()*0.00157; // Since the encoder generates 4000 pulses in 1 revolution 400 pules= 360 degree Hence, 1 pulse=0.09 degree and 1degree=pi/180 radian
currentPos=input;
rate=(currentPos-prevPos)/dT;
prevPos=currentPos;
if(input > 6.28) motEnc.write(0);
else if(input < -6.28) motEnc.write(0);
Serial.println(input);
if(input > 3.05 && input < 3.23) balance();
else swing();

}
``````

Looks good to me, so far :-)

The determination of the limit constants is up to you. Also the initial internal PID values, when switching from swing to balance. Eventually you can call myPID.Compute() when approaching the set position, even if you you don't use the output value.

Today I tried incorporating the energy based controller with the pendulum. But still it is not able to catch and stabilize in the upright position. I really don’t understand where I am going wrong.I have rechecked the required energy and other terms. They are correct. I substituted them with the parameters like Moment of inertia,mass of pendulum etc that’s why the expression looks kind of wierd.

``````#include <PID_v1.h>
#define dT 0.00001
//Variable definition
double prevPos=0;
double currentPos,rate;
#include <Encoder.h>
//Variable definition
double setPoint, input, output;
double E;
double reqE=2.01;// This is the desired energy of upright position i.e 2mgl and I replaced all the parameters and found the value
//Specifying tuning parameters
double Kp=301.5, Ki=30.15, Kd=34.22;
const int dirA=8;
const int dirB=12;
const int motorPWM=11; // This will be connected to the enable pin on the H-bridge
PID myPID(&input, &output, &setPoint, Kp, Ki, Kd, DIRECT);
Encoder motEnc(2,3);

void setup()
{
myPID.SetMode(AUTOMATIC);
Serial.begin(9600);
setPoint=3.14;//I want the pendulum to remain in the upright postion i.e 180 degrees
pinMode(dirA,OUTPUT);
pinMode(dirB,OUTPUT);
pinMode(motorPWM,OUTPUT);
myPID.SetOutputLimits(-90, 90);
myPID.SetSampleTime(2);
}
void totalEnergy(double x,double y){
/*This is the energy at any instant*/
E=0.010773*y*y+1.0536*(1+cos(x));

}
void balance(){
myPID.Compute();
if(output>=0){
digitalWrite(dirA,HIGH);
digitalWrite(dirB,LOW);
analogWrite(motorPWM,abs(output));
}
else if(output<0){
digitalWrite(dirA,LOW);
digitalWrite(dirB,HIGH);
analogWrite(motorPWM,output);
}
}
void swing(){

totalEnergy(currentPos,rate);

if((rate<0 && E<reqE) ||(rate>=0 && E>=reqE)){
digitalWrite(dirA,HIGH);
digitalWrite(dirB,LOW);
analogWrite(motorPWM,90);
delay(50);

}
else if((rate>=0 && E<reqE) ||(rate<0 && E>=reqE)){
digitalWrite(dirA,LOW);
digitalWrite(dirB,HIGH);
analogWrite(motorPWM,90);
delay(50);

}

}
void loop()
{

input = motEnc.read()*0.00157; // Since the encoder generates 4000 pulses in 1 revolution 4000 pules= 360 degree Hence, 1 pulse=0.09 degree
currentPos=input;
rate=(currentPos-prevPos)/dT;
prevPos=currentPos;
if(input > 6.28) motEnc.write(0);
else if(input < -6.28) motEnc.write(0);
Serial.println(input);
if(input > 3.05 && input < 3.23) balance();
else swing();

}
``````

I still suspect that the problem occurs when the controller is switched. The motor does not react immediately on changes, so that its dynamic behaviour may have to be taken into account as well.

Can you please give some ideas by which the switching can be made more effective? I am like totally lost with this one now.

profsatch: I don't know whether my logic towards this project is still correct or not.

Looking quickly at your code, I am surprised that you seem to be using only one of the two rotary encoders. Is that correct?

I am concerned about the transition from swing-up to balance. As I understand things, your motor will be running at nearly full speed (PWM duty cycle 200/255) when you first swtich to using the balance function in your code. I am not saying that you want to brake the motor and its inertia wheel as soon as you change to balance mode because that would give the pendulum an unwanted kick. I am saying that I think you need to give the transition from swing-up to balance mode more thought.

My feeling as well. A look into the PID library should reveal, which internal variables are used there, in detail which are used to determine the integral portion of the output. These variables should be set to meaningful values before the PID takes over.

Also the capture range around upright position may be important. The swing-up should push the pendulum into that range, without exceeding it at the other side. With the proper formulas at hand it should be possible to determine the pendulum position (angle) at the end of a swing, and to reduce the motor speed as soon as that position exceeds the upright position.

I don’t claim any working knowledge in what you are doing., but my nose is pretty good.

If I understand correctly, your balance program values were determined by starting the pendulum from pretty much a top-dead-center position with very little momentum involved.

Unless you can kick the pendulum up to that position with very little movement, I don’t think you will be able to get it to balance. Your balance program either needs to be able to catch the moving pendulum, or the swing program has to be able to just make it to top-dead-center with very little speed at the top of the swing and use your current balance program.

Archibald:
Looking quickly at your code, I am surprised that you seem to be using only one of the two rotary encoders. Is that correct?

Yes, currently I am not using the encoders in the motor. This is the disadvantage of PID controller. As the reaction wheel velocity is not being taken into account the system is thus less robust. There is another controller called LQR where the state variables are calculated using observers. That controller uses all angle of pendulum, angular velocity of pendulum as well as the reaction wheel.But as of now I am just implementing a simpler PID.

Archibald:
I am saying that I think you need to give the transition from swing-up to balance mode more thought.

Like switching off the motor as it approaches the catching region, say 170 degrees or so?

MikeLittle: Unless you can kick the pendulum up to that position with very little movement, I don't think you will be able to get it to balance. Your balance program either needs to be able to catch the moving pendulum, or the swing program has to be able to just make it to top-dead-center with very little speed at the top of the swing and use your current balance program.

Yes, I found the gains for the stabilizing control at top dead center with very little momentum involved.

So guys I think I have two options now.

1. Change the gains by taking into account the speed of the pendulum

2. Come up with a way that ensures the pendulum reaches the upright position with very less speed so that the stabilizing algorithm can catch and stabilize it.

Another thing guys, the way I am calculating the rate of change of angle is totally vague. I tried discontinuing it but I've got no options. All the previously discussed method of finding the rate of change are not working in the hardware.

An observer is not required for the motor. The encoder returns the rotation speed, thus the momentum to take into account. Unfortunately the motor itself is hard to control, due to the moment of inertia. This is what has to go into the system matrix of the LQR.