# Calculation of rate of change of Angle

Hello Everyone,

Currently I am working on the balancing of the inverted pendulum. I am using an incremental encoders to measure the angular position (theta) of my pendulum.I am using the Encoder Library that I got from the “Rotary Encoder” post in the Arduino website.

http://www.pjrc.com/teensy/td_libs_Encoder.html

The thing is that I need to find out the rate of change of the angle (theta dot) so that I can implement the control algorithms. I have written a code but it is not giving good results. Can you please help me find the best way to find out the rate of change of the angle.

``````#define dT 0.0001 //Sample Time
double prevPos=0;
double rate;
#include <Encoder.h>
Encoder myEnc(5, 6);

void setup() {
Serial.begin(9600);
Serial.println("Basic Encoder Test:");
}

long oldPosition  = -999;

void loop() {
rate=(newPosition-prevPos)/dT;
prevPos=newPosition;
}
``````

Have you determined that your loop time is 0.0001 seconds?

Your encoder output is unlikely to change by more than one bit within one loop period (unless you put delay in the loop). So consider measuring the time it takes between successive changes of the two output signals. From the specification of your encoder, work out the angle between changes in output to enable you to work out the angular speed.

Thank you for quick explanation. I am just beginning my way in the electronics so I am not that aware of the use of timers and frequency. It would be great if you would give an insight about how to measure the time between do pulses.So that I can measure the rate of change of angle.

Take a look at micros() and millis().

I suggested above that you look for successive changes in the two output signals of your incremental rotary encoder. Your code would work out the direction of rotation. That could work well if you can be sure that your Arduino can loop fast enough to detect every change of the output signals. However it may be better if you continue to use the library with its fast interrupt routine written in assembly language, although I understand there is still a risk of missing changes in the output signals.

If you use the library, you would work out the rate of change of angle from the change in the number of counts, the angular degrees represented by one change in count and the time taken. That way you should get an fairly accurate measure of the rate of change of angle as quickly as possible. That's what you need for the control feedback path of your pendulum.

Thank you for giving me the idea to deal with the problem. This is what I have come up with after some hours of thinking. Is my logic correct in this regard.Or Am I doing some blunder here in the code.Thank you! :)

``````#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();
}
``````

I have not checked your code in detail but it looks very good to me.

I suggest within the loop that you only calculate rate if the encoder position has changed. That way if your pendulum is swinging only very slowly you will not get a load of zero measurements of rate in between genuine measurements of rate. Does that make sense?

Although Arduinos are extremely slow by modern processor standards, your loop may take less than 1 millisecond or only a few milliseconds (depending on whether an encoder change has been detected). I don't really know. I would therefore be inclined to use micros() instead of millis(). Anyway you might as well use micros() as it will work for 70 minutes before you need to be worried about overflow (going back to zero).

Will you be driving a stepper motor or servo from your Arduino to keep your pendulum balanced?

Let us know how you get on. We will all want to see a YouTube video of your pendulum in action!

Archibald: I suggest within the loop that you only calculate rate if the encoder position has changed. That way if your pendulum is swinging only very slowly you will not get a load of zero measurements of rate in between genuine measurements of rate. Does that make sense?

You mean this way,right?

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

[quote author=Archibald link=msg=2813339 ] Will you be driving a stepper motor or servo from your Arduino to keep your pendulum balanced? [/quote] I will be using a DC motor with PID control to keep the pendulum balanced.

[quote author=Archibald link=msg=2813339 ] Let us know how you get on. We will all want to see a YouTube video of your pendulum in action! [/quote] I hope this works out well.Thank you for helping out.:) :) Cheers!

As you’ve already got a loop, I would have automatically used an “if” statement but I can’t see any reason why “while” wouldn’t work.

The problem with my previous code was that when I moved the pendulum in the opposite direction instead of showing - 1 and further values it show 65535. Is there a solution to that one?

Instead I have come up with a code that calculates velocity based on constant sample time approach I am using the timer overflow interrupt for this case. Please do check the code and please tell me if the logic is right?What frequency or sample time would yield me an approximate estimation of the velocity. Thank you.

``````#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;
}
``````

In your old code, I think you need to use variables of type signed int for currentPos and previousPos.

In your new code, I do not understand what your “if” statements are for, at least partly because I have no information on your Encoder library.

I would declare previousPosition and newPosition as signed integers because that is what they are.

As the processor time for integer arithmetic is probably very much quicker than for double variables, I would try to do all calculations of rate of change of angle in terms of encoder counts per 4ms. You could then simply do:
`rate = (newPosition - previousPosition);`(where ‘rate’ is also an integer)
Does that make sense?

Archibald: I would declare previousPosition and newPosition as signed integers because that is what they are.

As the processor time for integer arithmetic is probably very much quicker than for double variables, I would try to do all calculations of rate of change of angle in terms of encoder counts per 4ms. You could then simply do:

Thank you that was a very useful suggestion.I have implemented it in my code.

Archibald: In your new code, I do not understand what your "if" statements are for, at least partly because I have no information on your Encoder library.

Actually I am using a 4000 PPR optical encoder. So when it swings multiple time then it'll be more than 4000. As my set point is 180 degrees I don't want it to be disturbed in multiple rounds of the pendulum so basically I am setting is to 0 once it reaches 4000. That way my pendulum will only measure angle between 0-360 degrees.

Thank you for you guidance Archibald.Actually I am having problem in the swing up algorithm. Here I have made a new post with all the details.

https://forum.arduino.cc/index.php?topic=409005.0 Please please do post there to help me out. Thank you very much.

Just for clarification I understand, in the way they specify rotary encoders, that your encoder is 1000 pulses per revolution (PPR). But that means you will get 4000 counts per revolution.

Surely if your count goes over 4000 you don't want to set the count to zero; you want to subtract 4000 from the count. Similarly if your count goes less than zero you want to add 4000 to the count. That way you will get a number that represents the angle of your pendulum, ensuring for any position that there can only be one number and that will be between 0 and 3999. With the code you posted above, the number representing the angle can be between +4000 and -4000.

To constrain your measurement of angle to the range 0 to 3999, you can throw away your "if" statements and use modulo division by 4000. However, you would need to first add 4000 to ensure the number is not negative. Don't ask me which method is quickest.

However, if the modulo division (or your "if" statements) are performed before calculation of the rate of change of angle, you will get very wrong values for the rate of change of angle when the modulo division (or your "if" statements) take effect. You need to work out the rate of change of angle before constraining the count to 0 to 3999 and you need to do prevPosition=newPosition after constraining the count.

I assume your count is initially zero when the pendulum is at rest non-inverted, so your "set position" will be a count of 2000. However, especially as your pendulum approaches stability in an inverted position, it may be advisable for your code to be aiming for stability rather than aiming for a count of 2000.

Oh! I am sorry my bad. Actually I want to constrain the angle between +360 and -360 degree.Because during swing up the pendulum goes in the anticlockwise direction of the stable position that means the angle will be negative.

`````` if(newPosition>4000)
myEnc.write(newPosition-4000);
``````

Regarding the value being less than -4000. I guess that will not happen with the setup so I'll consider doing the above only. If required I'll use the concepts given by you previously.

I don’t understand that, but anyway you need to calculate rate of change of angle before you constrain the measurement of angle, otherwise you will get a very wrong result.

Thank you very much. I will work out on the suggestions you’ve given.