 # Swing Up Control of Reaction Wheel Inverted Pendulum

The rate of change can be determined in two ways, by number of pulses in constant time intervals, or from the time for a constant pulse count.

profsatch:
All the previously discussed method of finding the rate of change are not working in the hardware.

In my view, until you can measure the angle reliably at fairly frequent intervals you are wasting your time.

Do you have any idea why your measurements are not working “in the hardware”?

I suggest you start a new very simple sketch something like this to check your encoder (not debugged):

``````#include <Encoder.h>

Encoder enc(2,3);

void setup()
{
Serial.begin(9600);
}

void loop()
{
delay(1000);                       // print measurement every second
}
``````

Then rotate your pendulum slowly by hand and check that you get near enough 4000 counts for a complete revolution.

Then rotate your pendulum fast by hand and see if you still get 4000 counts for a complete revolution. That will check whether the Arduino processor is fast enough to deal with the rather rapid pulse rate from the encoder’s two outputs.

Obviously other code you have within the loop will put more demand on processor time.

``````void callback()
{
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
if(newPosition>4000)// I am using a 1000 PPR encoder with 4X decoding
{
newPosition -= 4000;
myEnc.write(newPosition);
}
else if(newPosition<-4000)
{
newPosition += 4000;
myEnc.write(newPosition);
}
prevPosition=newPosition;
}
``````

Problems:
If the position wraps around at a full turn, the controller won’t be able to hold that position, because it is adjusted by ±4000 whenever it crosses that limit.
If the position range is extended to 2 or more full turns, the positions are no more unique, so that the controller must be given the setpoint next the current position.
These considerations apply to both the bang-bang and PID controller!

Interrupts are not nested but serialized, i.e. they always have precedence over other code. That’s why interrupt handlers should be as short as possible, so that they don’t delay other interrupts too much, possibly causing lost events.

So let’s assume that the pendulum doesn’t take a full turn i.e the encoder values doesn’t exceed 4000 or less than -4000. Then the following code should be able to give the rate in a fair manner, isn’t it? OR I can’t just access the values of the ISR in the loop like this?

``````#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()
{
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
if(newPosition>4000)// I am using a 1000 PPR encoder with 4X decoding
{
newPosition -= 4000;
myEnc.write(newPosition);
}
else if(newPosition<-4000)
{
newPosition += 4000;
myEnc.write(newPosition);
}
prevPosition=newPosition;
}
``````

If the Encoder.h library uses Interrupts to decode the position. Like I am calling an ISR after timer overflows say 4 ms and then inside the ISR there is decoding of the pulses from the encoders using interrupts. Will this not affect the calculation?

I am really sorry for asking such questions. But as I am not very used to interrupts it is confusing me.

Archibald: Then rotate your pendulum slowly by hand and check that you get near enough 4000 counts for a complete revolution.

Then rotate your pendulum fast by hand and see if you still get 4000 counts for a complete revolution. That will check whether the Arduino processor is fast enough to deal with the rather rapid pulse rate from the encoder's two outputs.

This was one of the first task I did.When doing multiple times at different rates (fast as well as slow) I was able to get very good encoder values reading in the Serial monitor.

In fact the code I just posted above i.e using Encoder.h library and the timer overflow interrupt was able to give me the values of rates in the Serial monitor.While moving the pendulum clock wise the values would be positive and while the pendulum was falling it would be negative.

The problem right now is the inability to use that rate for the swing up I guess.

When a timer and encoder interrupt occur at the same time, the results from the timer ISR may be 1 tick behind. But the tick is not lost, it only will result in a higher rate (by 1) in the next interval. This systematic uncertainty of 1/2 tick cannot be avoided.

Don't forget to protect all references to the volatile variables, outside the ISR, by noInterrupts() and interrupts(), so that multi-byte variables are read properly.

``````noInterrupts();
float tempRate = rate;
interrupts();
Serial.print(tempRate);
...
``````

And avoid floating point calculations whenever possible, in detail in an ISR. A rate in pulses/ms instead of per second may allow to use int values.

Consider varying the motor PWM duty cycle during swing-up.

To give the PID balance mode the best chance of stabilising the pendulum, ideally you want no rotation of the motor when the mode switches over to balance mode.

Perhaps someone experienced in PID control can say whether the integral term relates to the mechanical characteristics of this pendulum system or whether the integral coefficient should be zero.

I was able to use the the rate of change of angle calculated by sampling position every 10 ms to swing up the reaction wheel pendulum. Like previous codes I used the bang bang energy controller with the energy based controller for the swing up.

``````#include <Encoder.h>
#include <PID_v1.h>
#include <TimerOne.h>
const int dirA=8;
const int dirB=12;
const int motorPWM=4;
volatile  long int prevPosition=0,newPosition;// int calucation is faster than double so
double setPoint, input, output;
double Kp=350.5, Ki=35.15, Kd=55.20;// Gains of the PID controller
PID myPID(&input, &output, &setPoint, Kp, Ki, Kd, DIRECT);
double E;
double reqE=4.2;
volatile double rate;
Encoder myEnc(2,3);
void setup() {
Serial.begin(9600);
Serial.println("Basic Encoder Test:");
pinMode(dirA,OUTPUT);
pinMode(dirB,OUTPUT);
pinMode(motorPWM,OUTPUT);
Timer1.initialize(10000); // set a timer of length 10000 microseconds (or 10 ms )
Timer1.attachInterrupt(callback); // attach the service routine here
setPoint=3.14;
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-150, 150);
myPID.SetSampleTime(5);
}

void loop() {
input = newPosition*0.00157; // Since the encoder generates 4000 pulses in 1 revolution 4000 pules= 360 degree Hence, 1 pulse=0.09 degree
Serial.println(input);
if(input > 2.96 && input < 3.31) balance(); // Switch the controller between 170 and 190 degrees
else
{
totalEnergy(rate,input); // Finding the energy at particular instant

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

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

}

}
}

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 callback()
{
rate=((newPosition-prevPosition)*0.00157)/0.01;//0.01 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
if(newPosition>4000)// I am using a 1000 PPR encoder with 4X decoding
{
newPosition -= 4000;
myEnc.write(newPosition);
}
else if(newPosition<-4000)
{
newPosition += 4000;
myEnc.write(newPosition);
}
prevPosition=newPosition;
}

double totalEnergy(double y,double z){
/*This is the energy at any instant*/

E=0.010773*y*y+1.0536*(1+cos(z));
return E;
}
``````

Everything seems good except that the pendulum is not able to stabilize itself after swing up. I tried varying the PWM during swing up and no lucks so far.

How do I effectively switch between the swing up and the stabilizing PID controller so that my pendulum is able to stabilize itself about the upright position right after the swing up? Thank you very much.

From the mathematical VP the function value and (at least) the 1st derivative of both functions (controllers) must match, for a smooth transition. You'll have to look into the PID code and find out which variables are involved in the calculations, and set these to satisfy the condition.

Or you let the PID run in parallel, but only watch its output during swing up. Perhaps this parallel operation happens already in AUTOMATIC mode? As soon as the PID output deviates from its OutputLimits, you know that the PID is ready for take over.

profsatch: I tried varying the PWM during swing up and no lucks so far.

The reasons I suggested varying the PWM duty cycle were: 1) it may help to get the top of the swing of the pendulum very close to the inverted position without the pendulum going over the top and continuing down the other side. 2) it may help to reduce the speed of rotation of the reaction wheel so when the 'balance mode' is entered the PID controller can start with the reaction wheel not rotating fast.

DrDiettrich: From the mathematical VP the function value and (at least) the 1st derivative of both functions (controllers) must match, for a smooth transition. You'll have to look into the PID code and find out which variables are involved in the calculations, and set these to satisfy the condition.

Or you let the PID run in parallel, but only watch its output during swing up. Perhaps this parallel operation happens already in AUTOMATIC mode? As soon as the PID output deviates from its OutputLimits, you know that the PID is ready for take over.

If you run the PID in parallel, the integral term could be large because it will have integrated from the initial position. As questioned above, should the integral coefficient be set to zero?

Is it worth using the PID library or would it be simpler to code the coefficients directly as angle and rate-of-change-of-angle are already being measured?

I remain concerned that the PID controller may be taking over at a time when the reaction wheel is rotating fast. That would limit the amount of feedback correction that could be applied in one direction.

Also you don't want the PID controller to suddenly change the speed of the motor when it takes over control from the swing-up mode. Whether or not running in parallel, I would say that you want the PID to take over control when the PWM output of the PID controller is close to the PWM being applied to the motor by the swing-up control mode (including polarity). Does that make sense?

When a controller allows to setOutputLimits, it may be as clever as to manage the internal variables appropriately in such extreme conditions. As already mentioned, a look into the controller code may reveal more. As a general solution the I and D factors could be set to zero during swing-up, and set differently when the PID takes over control.

Hi profsatch,