Encoder Demo
The rotary inverted pendulum has two encoder. Encoder has a CPR value, which is the value of "counts per revolution". We use Arduino MEGA 2560 to read the encoders, and by using 2 external interrupt pins for each encoder, we could get 4 times resolution, in other words, for a 500 CPR encoder, we could get a resolution of 2000 CPR.
Here is a video explains how this works. The video was made for another project, but by watching this video, you could have a better understanding on how encoder works.
The following is the encoder demo for this rotary inverted pendulum.
This is the demo setup:
Table of hardware connections:
pins of encoder | pins of Arduino | |
encoder 0 | Ch.A | pin 2 (external interrupt 0) |
Ch.B | pin 3 (external interrupt 1) | |
encoder 1 | Ch.A | pin 21 (external interrupt 2) |
Ch.B | pin 20 (external interrupt 3) |
Here I would like to explain some math about encoder readings:
encoder 0 (base encoder)
This is a 500 CPR encoder installed in base. This encoder senses motor rotations. Because our Arduino program reads encoder in 4 times the original resolution, this 500 CPR encoder now becomes:
Also note that there is a pair of pulleys connect the encoder and the motor. The bigger pulley has 60 teeth, the smaller pulley has 20 teeth. Thus, the rotation ratio of motor to encoder is 3:1. So, when the motor rotates one full circle the encoder will output:
Therefore, we have this relation between the angle motor rotates and the encoder output counts:
Which gives us
The above explains this line in the program:
encoder 1 (pendulum encoder)
This is a 1250 CPR encoder. Our program reads it in 4 times the original resolution, hence, it becomes:
And we have this relation between the angle the encoder shaft rotates and the encoder output counts:
Which gives us
The above explains this line in the program:
Timer2 Setups
There is this piece of code for Timer2 setup:
To find out why such settings will lead to a 1ms timer, please refer to MEGA 2560 datasheet, available in DOWNLOAD section.
Eastimate Velocity
This is the program for estimating velocity:
Timer2 overflows every 1ms, thus, each increment of variable t is 1ms. In line 1-3, we recode "angle_previous"; in line 4-6, which is 20ms later, we record "angle_post". Therefore, the average velocity in this past 20ms is:
The above explains line 8 and 9.
Program Codes
This is the Arduino program code: (the file is available in DOWNLOAD section)
encoder_demo.pde
read_encoders.pde