Build your own robot arm – programming

Now, that the arm is assembled it is time to take it to the next level. It is time to unleash the beast and gain full control of the whole robot arm. By the end of this post you should have an idea of how to program this robot arm to do what you want it to do. To see how I got here please visit my previous article describing the assembly process – Build your own robot arm – assembly.

What you need

Once again, you will need to get some extra hardware to facilitate the parallel control of servos and a solder to assemble the shield.

Servo shield

Lets start with the first step – preparing the servo shield. If you buy servo shield that is ready to go than feel free to skip this part. Main objective here is to end up with assembled servo shield like the one depicted below.


When you order the Adafruit shield, it will arrive like this and you will have to solder it together yourself. If you have previous experience with soldering this should be a piece of cake. If this is your first soldering project, I suggest to watch a few YouTube videos to see, how to go about soldering and assembling Arduino shields. When you feel ready check Adafruits tutorial on how to assemble the shield.


Once you have your shield ready, try connecting one of the servos to it. Keep in mind that you need to match PWM, V+ and GND pins to the wires coming out of your servo.



To make any use of this shield we need to be able to address it programatically. Adafruit released a library to facilitate this available here. I also suggest to browse through library GitHub repo and also see the examples of usage.

Research & development

At this point some research is required. Aim is to find either the edge position of each servo or middle one so you can use them later as a reference point. You need to try out several values to find the sweet spot that suits your build. Based on servo spec the frequency these servos operate at is 50 Hz. When it comes to servo movement the spectrum you need to examine ranges from 0 to 4 096. Values you will arrive at are based on the way you assembled your arm so they may vary from mine.

At this point you may want to utilize following method that simplifies the control of the servo:

float SERVOFREQ = 50;
float pulseconstant;

void setup() {
    pulseconstant = (1000000/SERVOFREQ)/4096;

void servoWrite(uint8_t n, float pulse) {
    float pulsetick = pulse/pulseconstant;
    pwm.setPWM(n, 0, pulsetick);

Values that work for me are as follows:

  • Base servo
    • turns the arm to the right and left
    • middle position at 1 440
  • Height servo
    • changes the height of the gripper
    • middle position at 1 675
  • Length servo
    • changes the reach of the arm
    • middle position at 1 440
  • Grip servo
    • opens and closes the gripper
    • closed position at 2 115

Servo movement is just a matter of changing the value based on the direction you want to move the servo.

Final code

Putting all the pieces together results in a simple sketch like this one. The whole purpose of this sketch is to set the arm into its initial position.

#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define BASE 0
#define HEIGHT 1
#define LENGTH 2
#define GRIP 3

float base_init = 1440;
float height_init = 1675;
float length_init = 1440;
float grip_init = 2155;

float SERVOFREQ = 50;
float pulseconstant; 

void setup() {
    pulseconstant = (1000000/SERVOFREQ)/4096;


    servoWrite(BASE, base_init); 
    servoWrite(HEIGHT, height_init);
    servoWrite(LENGTH, length_init);
    servoWrite(GRIP, grip_init);

void loop() {

void servoWrite(uint8_t n, float pulse) {
    float pulsetick = pulse/pulseconstant;
    pwm.setPWM(n, 0, pulsetick);

What is next?

At this point you should have your robot arm up and operational. Given your programming abilities you can program it to do what you like. But this is not enough (at least for me). This setup is cool and all, but it lacks the element of interactivity. So in my next and final post titled Build your own robot arm – remote control I will explore the options of interactive control of the arm. There are several options to go with like desktop application or mobile app but since I own PlayStation 4, I decided to use my PS4 wireless controller to control the arm. And yes, it is a lot of fun 🙂

Pictures used throughout this post were taken from Feel free to check them out.

2 thoughts on “Build your own robot arm – programming

  1. Thank you!

    I am new to programming so I am struggling with the Ic2 protocol. Would you be able to send me the code above with descriptions next to each line so I can understand what is going on?



    1. Hi Dan,

      sorry for the late response, I was on holidays. It is cool that you started playing with these things. Here are some pointers regarding the final code sample. Don’t forget to include your custom logic into the loop method to actually move the arm from the initial position.

      line 1 – you need to include a dependency on the driver in order to be able to use it in your code
      line 3 – driver instantiation so it can be referenced throughout the code
      line 5-8 – definition of what each pin controls so you can control them in whatever combination you like
      line 10-13 – based on the physical setup, you want to know the boundaries of the movement of each part and also more parts when controlling them so you don’t damage the arm or the servos
      line 15-16 – some constants retrieved from docs to control the setup in a right way
      line 19 – with a little bit of googling, I was able to determine how to calculate the proper pulse value for my build
      line 21-22 – driver startup and configuration
      line 24-27 – this block puts the arm in its initial position (somewhat in the middle) it is helpful to have the exact same starting point when you program or control it remotely
      line 30-31 – you can put a code that gets executed in a loop here controlling movement of the arm
      line 33-36 – moves the servo into a desired position

      Hope it helps and best of luck!

Leave a Reply

Your email address will not be published. Required fields are marked *