Robot Arm Exercise

This tutorial will cover some basic approaches to programming motion sequences with a robot arm

Here’s a zip of the example code 

For some of these examples, you’ll need the Arduino Timer library
you can download that here

We’re using is the Dagu 6 degrees of freedom robotic arm which has 6 hobby servos in the following configuration:
1: Shoulder Rotation
2: Shoulder Angle
3: Elbow Angle
4: Wrist Angle
5: Wrist Rotation
6: Claw Grip


I found that the servo on Joint #2 didn’t have enough torque to lift the arm when fully extended so I replaced it with a beefier one.
It’s hooked up to an Arduino UNO for control and has the servos on pins 8-13.

For this exercise, we also have a Lynx Motion 5 Degrees of freedom arm, graciously loaned by  Gabriella Levine (thanks Gabby!).
Here’s a link to info about the control board.

The main differences are that this only has 5 servos instead of 6 (no wrist rotation), so you’ll need to set that in your code.
Also, the servos are attached to pins 2-6, so you’ll need to specify that in your servo.attach() function.
In uploading code, you’ll need to specify the board as an Arduino Duemillanove.
You’ll also notice that arrangement of the servos are somewhat different on the  two arms so the same angles will produce different results.
So how are we going to control this thing?

Let’s start off by checking out the full range of motion for all the servos:
This program sweeps through each of the servos one by one using forloops and a delay.

This gives us an idea of the arm’s range of motion and it executes movement in a nice, smooth way.  By moving the arm a small amount each step and waiting between steps to give the servo time to move, we avoid the jerky, inaccurate movement that you sometimes get from simply writing a position to a servo.  But this technique has a few problems:

  • We’re only addressing and storing position for 1 servo at a time.  Ideally, we want to be able to tell all the servos where to go independently and have them update their positions together.
  • We’re using delay() to control the timing of the servos’ movement.  That means that the program is literally stopping and waiting before going to the next command.  The servo can still move during the delay because the servo library is using a different timer.  But as soon as we want to do something else like listen for a sensor input or reposition another servo, we’re out of luck.
  • We’re currently using for loops to set and increment  the servos’ positions.  With this setup, we have to define the start and stop position for each movement and can only address one servo at a time.

So how do we fix these issues?
Here’s one approach:

This sketch Allows us to compose a sequence with sets of 6 servo angles stored in a 2D array positions:

We can populate this positions array with arrays of predefined positions  and name them something appropriate :

We also have a waitTimes array — we’ll want one item for each each of the positions in our sequence:

We have a counter variable that we’ll use to increment through the steps of our sequence and then loop back to 0.
To do this, we’ll need to know, how many positions are in the sequence array so we have a numPos variable to store that.  We could just assign this the right number manually but it would be a pain to have to update it each time we change the sequence.  Arduino doesn’t have a direct method for returning the length of an array (ala processing’s array.length()).  So, in our setup routine we’ll use the sizeOf() operator to get the positions[][] array’s total size in bytes and divide that by the size of elements datatype (in this case byte(*)[6]).

We’ll keep track of the 6 servos’ positions with an array of current positions (pos[]) and an array of target positions (tPos[]).  Our servoSpeed variable controls how fast they will move to their targets (i.e. by what amount will it increment).  We also have an array of  state booleans for each servo (hitTarget[]) to keep track of whether each has reach its target and a variable to keep track of how many have reached (numReached).

We’ll use all of these in a servoUpdate() function that we’re using the Timer library to call at a set period (defined by our updateSpeed variable):

Here’s a diagram of what this function is doing:

Notice that our position update code is wrapped in a millis() comparison conditional so that it will only repositions our servos if the wait time for previous position has passed.

Similarly,  our code to increment the counter (i.e. set new target positions), is wrapped in a conditional that checks to see if all the servos have reached their target:

So, we have a give and take between our wait times and our target positions.  There two functions could be separated out as well, as in this example:

In this case, our checkTime() function will increment to new positions after the allotted wait time whether the servos have reached their targets or not and the servoUpdate() function will immediately start moving to the new target positions once they are assigned.  This may sound less convenient, but depending on what else you have going on in your program, it may be desirable have these functions operating more independently.