## Sunday, January 21, 2018

### Control multiple servos using Raspberry Pi 2 Model B, PCA9685, Windows 10 IOT and C#

In this post, I will be showing you how to control servos using Raspberry Pi 2 model B, Windows 10 IOT, C#, PCA9685 and Visual Studio 2017.  This post assumes you have some experience running C# application on a remote device. I will not cover how you connect to Rpi2 via Visual Studio or anything related to deployment. There is also an accompanying youtube video I have created and you can find it here - https://youtu.be/gkWYirX8uHo.  For code and power point check links below.

Servos motors typically turn 0 to 180 degrees. Some servos can go beyond 180 degree as well. The way you control servos is by providing a Pulse Width Modulation (PWM) signal.

Raspberry Pi 2 has one hardware PWM pin and you can do software PWM via any of its GPIO pins.  If you want to control multiple servos then just one PWM hardware pin is not going to cut it and if you use GPIO pins and do PWM via software then you will not have enough GPIO pins for doing other work.  And PWM via software is also not that accurate. However, I am not too concerned about it for my project.

So what are our options if we want to control multiple servos? Enter I2C protocol.  I2C is a protocol that enables master slave communication over SDA and SCL.  Via I2C you can have multiple devices (slaves) connect to our RPi2 (master) without taking up those GPIO pins. What does that mean? For example, if you see any sensor breakout boards that have I2C pins like SDA and SCL then you can connect multiple of those sensors directly to RPi2 via I2C. So we will use I2C pins of RPi2 to connect to PCA9685 chip which has 16 header pins that allow us to control up to 16 servos or 16 LEDs. With I2C and PCA9685 you can connect up to 62 PCA9685 boards and control up to 992 servos.

Let’s talk about PCA9685 and what it is.  The PCA9685 comes on a board typically if you buy from Adafruit or Amazon or any other retailer. I got mine from SunFounder via Amazon. The PCA9685 has 12-bit resolution. What does that mean? It means that it has a resolution of 2^12 = 4096. What does that mean? It means that for each period of your frequency you will have 4096 steps.  So let’s say your PWM frequency is 60Hz and 1/f is period T = 1/60s. T = 17ms. Now each 17ms is divided into 4096 steps. The time is taken per step is 17ms/4096=4.15us. If you want to give a pulse of 0.5ms then figure out how many steps are required.  Total Steps required for 0.5ms pulse = 0.5ms/4.15us = 120steps. Similarly you can find out steps required for 1ms,1.5ms,2ms,2.5ms. Below are the steps for these times.

0.0005 –> 120

0.0010 –> 240

0.0015 –> 361

0.0020 –> 481

0.0025 –> 602

How did I come up with these times and why do we need them? Typically servos turn 0 degrees if you provide a 0.5ms pulse, 1.0 for 60 degree pulse, 1.5ms for 90 degrees and 2.5ms for 180 degrees.  These are not accurate numbers and you find out these times for different servo motors. In our experiment we will fine tune our calculation so these turn servo exactly at a particular angle. Please see the section below where it says fine tuning our calculations so servo turn at our desired angle.

Now your next question is going to be how to turn servo at a specific angle other than 0,60,90 or 180.  In other words, if I want to turn servo at any random angle say 35 degrees, what is the number of steps we need to provide.

602-120 = 482 Total number of steps from 0 to 180 roughly.

480/180 = 2.7 Step per angle.

120 + 2.7x = Steps required to turn our servo at an angle x.

For 35 degrees,

120 + 2.7(35) = 214.5 Steps for 35 degrees.

Let’s summarize, to turn our servo at angle 35 degrees, we need to tell PCA9685 chip to turn the pulse HIGH for 214.5 steps and (4096-214.5) LOW.

Note: These calculations can change based upon your frequency and servo.  I am showing you this based upon Tower Pro SG90 micro servo motor at 60Hz frequency. Why 60Hz? Because this is a analog servo and analog servo you should have frequency ranging from 30Hz to 60Hz.

Since all the math is out of our way we can now focus other things.

1. Block diagram

In the block diagram, the things I haven’t shown are that Host PC and RPi2 are connected to their own keyboard, mouse and monitor.

2. Wiring diagram

The below image is from adafruit.com. Connecting multiple servos is pretty much straightforward.

3. Coding

You are now really impatient to see the working code. Right. Every body seems to use Adafruit library provided in python.  The c# library from Adafruit i.e AdafruitClassLibrary available on nuget doesn’t work out of the box if you create a new UWP application in Visual Studio. Why? Because the it is just not updated and it has issues. So I copied Adafruit I2CBase.cs and PCA9685.cs classes and modified them so they will throw exceptions if anything goes wrong. The main page of the application has few textboxes and buttons to control servos.

You can find the source code at this github link.

Follow the steps to run the application.

1. Ensure that Platform is set to ARM and Build configuration to Debug

2. Start the Visual Studio debugger on RPi2. This you can start by the Device Portal under Debug menu.

3. Run the application and it will deploy to the device and the application will start. (For me debugging doesn’t work and a prompt appears and I close that prompt)

4. Once you have the application running you will see a UI as shown in the figure below.

5. Press InitPCA9685

6. Press 60 the Step 2 textbox and SetPWMFrequency

7. Then enter the desired angle for your servo say 30 degrees.

You can see some logging into the textblock as you are executing actions.

The line of code that actually sets the steps is actually very simple.

_pca9685.SetPin(0, ticks, false);
For 35 degree angle ticks are 214.5 ticks, the code sets the pin 0 (which is where the first servo is connected) to HIGH starting from 0th tick to 214.5 tick and turns it LOW from there on till 4095th tick.
4. Fine Tuning
If you have followed so far and found that servo horns do not go exactly from 0 to 180 then we will tackle it in this section. It either goes past 0 or beyond 180. So let’s try to fix it.
a. Adjusting the lower and upper limit.
Let’s Recap – Do you remember the values 120 and 602? I will wait. Go above and come back. Alright those are the only things that we need to alter. That will determine our angle equation. My values after adjustment are as follows:
125 and 530
The resulting equation to calculate Ticks for a given angle is
125+2.25*angle
Those two values are the biggest things.
b. Placing your servo horns
Placing your servo horns is important. The lower and upper bounds will be affected based upon how you place your servo horn. Normally I place servo horn after providing a zero pulse and mark the position. Sometimes the grove will not fit at the exact zero position. At this time you will have to modify the upper and lower tick values to fine tune that servo.
c. Changing PWM frequency
All of our calculations were based upon 60Hz frequency. If you change this frequency then you will have to change recalculate everything again and do your tuning accordingly.
Notes:
1. The angle of your servo will also depend on how you are mounting your servo horn. I would pulse the servo to zero’s position and then place the servo horn and fine tune the steps.
2. At some steps you will hear the servo is getting stuck. This is due to servo is not in the desired position. Adjust the tick plus or minus one and you will see that it doesn’t occur. This is where we need to tune our calculations. This is a trial and error process.
3. Somehow I cannot debug the application while running using Visual Studio 2017. Hence I had to create UWP UI application with textboxes and buttons. You will need a monitor, keyboard, mouse connected to Rpi2 to enter the angle and visualize what is happening.
4. If you would like to know all the components used in this then please refer the environment section.
Resources:
Below are the resources I have found to be useful while learning this process.
1.  Adafruit Raspberry pi servo driver
2.  Sunfounder wiki
3.  PCA9685 datasheet
4.  AdafruitClassLibrary
5.  Full source code available here
6.  PowerPoint Slides
7.  YouTube Video Link


Environment

1. Windows IOT – 10.0.16299.125
2. Raspberry PI 2 Model B
3. Visual Studio 2017 Update 15.5
4. Power Supply for Raspberry PI 5V 2.1A
5. PCA9685 supplier (SunFounder)