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.

Video 3
(If you cannot watch YouTube, please click here for YouKu)

The following is the encoder demo for this rotary inverted pendulum.

This is the demo setup:

demosetup

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:

500*4=2000 CPR

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:

2000*3=6000 counts

Therefore, we have this relation between the angle motor rotates and the encoder output counts:

6000/(2*pi) = counts / angle

Which gives us

angle = 2*pi/6000 counts = 0.00105*counts, unit: radian

The above explains this line in the program:

encodercode1

encoder 1 (pendulum encoder)

This is a 1250 CPR encoder. Our program reads it in 4 times the original resolution, hence, it becomes:

1250*4=5000 CPR

And we have this relation between the angle the encoder shaft rotates and the encoder output counts:

5000/(2*pi) = counts / angle

Which gives us

angle = 2*pi/5000 counts = 0.00126*counts, unit: radian

The above explains this line in the program:

encodercode2

Timer2 Setups

There is this piece of code for Timer2 setup:

timer2

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:

estimate

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:

velocity = (angle_post - angle_previous) (radian) / 20 (ms)
= (angle_post - angle_previous) (radian) / (20 (ms) / 1000 (ms/s))
= (angle_post - angle_previous) * 50, unit: rad/s

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

encoderdemo1

read_encoders.pde

encoderdemo2