Build a 5-Axis Robot Arm With Arduino and DynamixShield

In this instructable I will show you how to quickly and easily build a 5-axis robot arm using commercial off-the-shelf components, and control it using an Arduino with a DynamixShield. The DynamixShield is a new add-on for either the Arduino Due, Zero, or Mega that I am promoting for my kickstarter campaign. It is a really cool shield that performs level shifting for the newer 3.3 volt Arduino systems. It has numerous, modular Grove connectors to allow you to quickly plug-in different grove sensors and motors. It also has standard three-pin connectors for analog, digitial, and pulse width modulation signals, making it easy to use with standard servos. Best of all though, is that it has a built in controller for Dynamixel smart servos. This allows you to control Dynamixel AX, MX, and XL servos directly from the shield. When you combine this with the grove modules, it becomes easy to build complicated robots with just 3 or 4 connections.

In this instructable I will use the DynamixShield to control a robot arm made from Bioloid frame components and Dynamixel AX-12 motors. It will have a pressure sensor on its claw to detect when it has fully gripped something. The arm will be controlled using two joystick grove modules. So it will take a total of five connections to the DynamixShield for this entire robot. Lets get started!

Step 1: Parts List

Below is a list of the parts that you will need. I have included links to where I purchased them as well.

Parts List

1. Bioloid Nuts & Bolts bag, Qty: 1, Unit Price: $23.40

2. Bioloid F1 Frame, Qty: 1, Unit Price: $1.49

3. Bioloid F2 Frame, Qty: 1, Unit Price: $1.49

4. Bioloid F3 Frame, Qty: 6, Unit Price: $1.49

5. Bioloid F4 Frame, Qty: 1, Unit Price: $1.75

6. Bioloid F10 Frame, Qty: 3, Unit Price: $1.25

7. Bioloid FP04 Frame, Qty 1, Unit Price: $6.95

8. RobotGeek large workbench, Qty 1, Unit Price: $15.95

9. Grove thumb joystick, Qty: 2, Unit Price: $6.99

10. PhantomX Parallel AX-12 Gripper without servo, Qty: 1, Unit Price: $24.95

11. Force Sensing Resistor with Female connector, Qty 1, Unit Price: $5.95

12. 220K resistor, Qty: 1 pack, Unit Price: $1.50

13. Dynamixel AX-12A smart servo 6-pack, Qty: 1, Unit Price: $224.50

14. Dynamixel 150 mm servo connectors. Qty: 1 pack of 10, Unit Price: $12.90

15. 3-pin extenders, Qty: 1 pack, Unit Price: $2.95

16. 12V, 5A power supply. Qty: 1, Unit Price: $19.95

17. Either an Arduino Due, Mega, or Zero. Qty: 1, Unit Prices: $41.11 for Due, $39.97 for a Mega 2560, or $49.40 for Zero.

18. A DynamixShield that matches the Arduino you choose. Unit Price: ~$35

So with all of these parts you are looking at around $450 depending on which Arduino board you choose to use.

Step 2: Assemble the Base

Use the bottom half of the FP04 Bioloid frame and connect it to the large RobotGeek workbench. Use three S3 bolts and three N1 nuts. Place the frame about 7 holes from the edge of the workbench and in the center. Be sure to put the two bolts on the side that is open. This will allow you to secure the other side with the bolts on top. Once you have that secure then place the rubber feet supplied with the workbench on the bottom corners of the workbench.

Step 3: Assign IDs to Your Servos

Before we start building the robot in earnest, we first need to assign the IDs on all our servos. They come from the factory with an ID of 1. We will use the DynamixShiled to do this reassignment. The software for this robot can be found on my DynamixShield github repository. Clone or download this repo . You will need to copy the folders in the DynamixShield/libraries directory into your Arduino library folder. On Windows this will usually be located in Documents/Arduino/libraries. Once you have done this then load the DynamixShield/sketches/AssignDynamixelID sketch. You can set the current and target ID at the top of the sketch to specify what you want to set the ID to be. Then plug each Dynamixel Servo into the DynamixShield one at a time. Make sure you have a 12V power supply big enough to power the Dynamixel servos, that it is plugged into the DynamixShield, and that you have the power jumpers on for that servo connector. When you plug a Dynamixel servo into the board and it has power then the red LED on the back will flash briefly. You can then upload the new reassign sketch. If it worked, then the servo should be reassigned to the new value and start moving back and forth briefly, and then stop at its center position. Do this for each servo to number them with IDs 1 to 5. Be sure not to move the servo out of its center position during the future steps.

Step 4: Add the First Arm Axis

Next, we will add the first axis of the arm. It will rotate the arm around on the FP04 frame. Add eight N1 nuts into the nut holders on a Dynamixel Servo and then use four S1 bolts to connect the top of the servo to two F3 frame parts on either side as shown in the images. Then place the servo on the top portion of the FP04 frame and use four S2 bolts to connect the servo through the FP04 frame into the bottom of the F3 parts. This will secure the servo to the top of the FP04 frame.

Step 5: Connect Top Frame to Base

Now place two N1 nuts into the small divots for them on the FP04 frame. Then put the top frame onto the bottom one and use two S2 bolts to secure the top frame with the servo to the base. Then connect one of the Dynamixel cables to the servo and run it down through the frame and out the side.

Step 6: Add First Axis Servo Horn Frame

Make sure that your first servo is still centered, and then place another F3 Bioloid frame part onto the servo horn and secure it using four S1 bolts as shown.

Step 7: Add Second Servo Axis

The second axis of the arm will move the arm up and down. Place four N1 nuts into the bottom of the another Dynamixel servo as shown, and place it on the F3 frame. Use four S1 bolts to secure it to the F3 frame. Then connect one of the Dynamixel cables between the first and second servos.

Step 8: Build the Arm Connector Between Axis 2 and 3

Now we need to build the longer arm connector that goes between axis 2 and 3. We will use one F4, three F10, and one F3 Bioloid parts. Stack the F10 parts together and put them on top of the F4. Then put the F3 part on top of that. Use four S3 bolts and four N1 nuts to secure the whole thing together so it looks like the image above.Then use the BU, WA nut and washer with the S-B bolt to secure the arm component to the second servo. Use four S1 bolts to secure the arm to the servo horn. Be sure the servo horn is centered first.

Step 9: Add the Third Axis

Take the third Dynamixel servo and add four N1 nuts into the compartments on the bottom of the servo. Then place it in F3 frame for the arm part we just built and use four S1 bolts to connect it. Connect one of the Dynamixel cables between the third and fourth servos, being sure to run the wire through the hole in the F4 frame part.

Step 10: Create Second Arm Part Between Axis 3 and 4

We need to create the second arm component that goes between axis 3 and 4. Use an F1 and an F2 Bioloid frame part. Connect the F1 part perpendicular to the F2 component and secure it with four S2 bolts and four N1 nuts. Then use a BU and WA nut and washer with a S-B bolt to secure the F2 part to the third axis. Use four S1 nuts to secure the F2 to the servo horn. Be sure the servo horn is centered.

Step 11: Build the Gripper

Next, we need to build the gripper. For complete instructions on how to build the gripper arm see the Trossen site. I will go over a short version here.

1. Build TWO Gripper Hand Assemblies by tapping into the hole on bottom of a Gripper Hand with an Bushing Set through one hole on one Gripper Arm.

   *Remember not to tighten completely, just enough for the Bushing to sit in the arm and spin.

2. *Note the orientation of Gripper Arms to Hands in the second image.
3. Apply a Foam Pad to one Gripper Hand. For the other, thread the force sensor through the hole in the gripper arm and tape it to the center of the gripper hand. Then apply the foam pad on top of that. Note: Do NOT remove the black backing on the force sensor. It does not have tape built onto it. If you peel the black backing off your force sensor will be ruined.
4. Note the orientation of Servo Horn before attaching arms. Slide both Gripper Hand Assemblies onto Gripper Rail as shown in the fourth figure and mount the Gripper Arms to the Servo Horn of the fifth Dynamixel servo using the final (2) Bushing Sets. *Remember not to tighten completely, just enough for the Bushing to sit in the arm and spin. Also, make sure you get the Dynamixel servo with the ID of 5 and that its servo horn is centered.

Step 12: Connect Axis 4 to the Gripper

Take two F3 parts and connect them together using four S2 bolts and four N1 nuts as shown in the first figure. Then place four N1 nuts into the divots on the bottom of the fourth Dynamixel servo, and an additional four N1 nuts on the bottom of the fifth servo that is part of the gripper. Now Attach the two servos together using the F3 parts and eight S1 bolts. Connect one of the Dynamixel cables between the fourth and fifth servos.

Step 13: Connect Gripper Assembly to Third Axis

First use one of the servo extender wires that comes with the RobotGeek workbench and attach it to the female connector of the pressure sensor using a three-pin extender. Be sure that it is connected to the signal (white) and power (red) wires on the extender wire. Feed the force sensor cable through the hole on the F1 frame. Then connect the fourth servo to the rest of the arm using a BU and WA nut and washer, and a S-B bolt in the back of the F1 frame. Use four S1 bolts to secure the front of the of the F1. Be sure to pay attention to the orientation of the servo when you do this so the arm moves in the correct direction. Also, check again to make sure the servo horn is centered. Then connect one of the Dynamixel cables between the third and fourth servos, making sure to run the wire through the hole in the F2 part. We now have all the servo of our robot arm added.

Step 14: Build Force Sensor Cable

The next step is to build the force sensing cable. We will need to hack together the connector for the force sensor. The sensor needs to have a pull-down resistor for it to work correctly. To do that we will use a three-pin extender and solder a 220k resistor between the first and third pins. Clip off the excess wire of the resistor when you finish. This will connect the signal wire (white) to the ground wire (black) through the pull-down resistor. This will allow the Arduino to measure the change in resistance on the pressure sensor. Once you have that built then insert it into the extender wire that is connected to the force sensor and a new extender wire that you run down through the FP04 frame part. This will eventually be connected to the DynamixShield analog header row in the AD11 slot.

Step 15: Add the Joysticks.

Now take the two grove joysticks and secure them to the workbench on either side of the robot arm. Be sure to offset them a bit to give room for the Arduino. Use two S4 bolts, two SP-1 stand-offs, and two N1 nuts per joystick. Then connect your grove wires into the joystick so they are ready to hook to the DynamixShield.

Step 16: Attach the Arduino.

Next we will attach the Arduino microcontroller. There are two ways to do this. I wanted to be able to easily mount and dismount my Arduino and DynamixShield so I could swap it between different projects. To do this I bought some round magnets and drilled holes in the center of them as shown in figure one. I drilled a hole for a bolt, and then partially drilled a larger hole for the head to go in and be flush with the edge of the magnet. I was then able to attach this to the Arduino using four standoffs that came with the Workbench. Then I did something similar on the workbench. I used a magnet on top and one on bottom and bolted them on with a M3 x 10mm bolt. This allowed me to just plop the magnets attached to the Arduino down to the magnets on the workbench to quickly secure it temporarily. If you want to do this then you will also need to purchase the magents and larger M3 bolts.

If you would like it to be more secured than this then you simply need to use the stand-offs and M3x6mm bolts that come with the Workbench to secure the Arduino and DynamixShield to the Workbench.

Step 17: Connect the Wires.

Connect the Dynamix servo cable into one of the four slots in the DynamixShield. Then connect the right joystick into the Grove connector labeled AD0, and the left joystick into the Grove connector labeled AD2. Next, connect the pressure sensor cable into the last slot on the analog header for AD11. The Grove and Dynamixel connectors are polarized, so you do not have to worry about connecting them wrong. However, the pressure sensor cable is not polarized, so be sure to place the black wire on the top of the analog header. Except for the power, we are are now all hooked up! That was easy.

Step 18: Program the Arm.

You can find the sketch that controls the robot arm in the github repository under DynamixShield/sketches/RobotArm. I will briefly explain the main parts of the sketch.

1. First, you must include the DynamixSerial class. This class is what you will use to communicate with the Dynamixel servos.

#include

2. Next we have some variables to assign the pins for the joystick and pressure sensors, and to keep track of the servo movements.

#define SERVO_COUNT 5

#define AXIS_COUNT 4

// These constants won't change. They're used to give names

// to the pins used:

const int x1Pin = A0;

const int y1Pin = A1;

const int x2Pin = A2;

const int y2Pin = A3;

const int pressurePin = A11;

//Index of the AxisPos array that controls gripper open/close

const int gripperCloseIdx = 1;

const int gripperOpenIdx = 3;

const int gripperIdx = 4;

int servoPos[SERVO_COUNT];

int servoDir[SERVO_COUNT];

int servoAdd[SERVO_COUNT];

int axisID[AXIS_COUNT];

int axisPos[AXIS_COUNT];

int gripPressure = 0;

3. Then we declare the DynamixelSerial class. This class assumes that it is using Serial1 for communications to the Dynamixel servos. However, if you pass in a hardware serial port here you can override it. This is so the software can be used on custom projects that do not use the DynamixShield.

DynamixelSerial Dynamixel;

4. The next most important part is the setup. We startup our debug serial connection and then begin the DynamixelSerial class. It defaults to a 1 Mbaud communications rate and the pin that it uses to control the half-duplex of the Dynamixel protocol is set based on the board you are using. So on a Due it is pin 22, but on the zero it is pin 2. However, you also have the option of specifying these parameters in the call to the begin method to override what is used for custom projects.

#ifdef ENABLE_DEBUG

Serial.begin(57600);

while(!Serial);

Serial.println("Starting setup");

#endif

Dynamixel.begin (); // or Dynamixel.begin (1000000, 22);

5. Setup then has some code to reset all the servos to their default position to get it ready for movement. It also resets all the variables to their default state.

6. The main processing loop is pretty simple. It just calls processJoysticks() and then delays for 20 milliseconds.

void loop () {

processJoysticks();

delay (20);

}

7. The process joystick method has a loop that goes through each joystick axis and reads the analog value from the joystick.

void processJoysticks() {

String servoPosReport = "", servoAddReport = "", axisReport = "";

// read the analog in value:

for(int i=0; i

axisPos[i] = analogRead(axisID[i]);

8. It then scales the value from the joystick that is between 255 and 755 into a value between -5 and 5. If the value is greater than 1020 then it means we are pressing down on the joystick to close the gripper jaws, so we will let the process gripper method deal with that.

//If it is greater than 1020 then the button is being pressed.

if(axisPos[i] < 1020) {

servoAdd[i] = map(axisPos[i], 255, 755, -5, 5);

}

else {

servoAdd[i] = 0;

}

9. Then we add the movement value to the servos position.

if( (servoPos[i] + servoAdd[i]) >= 0 && (servoPos[i] + servoAdd[i] < 1024)) {

servoPos[i] += (servoDir[i] * servoAdd[i]);

}

10. Then if we had movement on this axis we set the new position of the servo.

if(servoAdd[i] != 0) {

Dynamixel.moveSpeed (i+1, servoPos[i], 150); delay(10);

}

11. Finally, it calls processGripper. That methods starts out by reading the analog signal for the pressure sensor.

void processGripper(String &servoPosReport, String &servoAddReport) {

gripPressure = analogRead(pressurePin);

12. Pushing down on the right joystick closes the girpper, while pushing down on the left joystick opens it. The method checks this by seeing if the axis position of the close or open gripper position is equal to 1023. This is the value read from the joystick when you are pressing down on it. If it is down then we set the servo add to be plus or minus 10. Otherwise, it adds zero.

if(axisPos[gripperCloseIdx] == 1023) {

if(gripPressure < 850) {

servoAdd[gripperIdx] = -10;

}

}

else if(axisPos[gripperOpenIdx] == 1023) {

servoAdd[gripperIdx] = 10;

}

else {

servoAdd[gripperIdx] = 0;

}

13. Then we set the servo position as long as it remains within 0 and 512. The gripper cannot move more than this.

if( (servoPos[gripperIdx] + servoAdd[gripperIdx]) >= 0
&& (servoPos[gripperIdx] + servoAdd[gripperIdx] <= 512)) {

servoPos[gripperIdx] += (servoDir[gripperIdx] * servoAdd[gripperIdx]);

}

14. Finally, if we added to the position then we need to set the new servo position of the gripper.

if(servoAdd[gripperIdx] != 0) {

Dynamixel.moveSpeed (gripperIdx+1, servoPos[gripperIdx], 150);

}

And that is it. There are some other things in the sketch I did not discuss here, but that is mainly for debugging purposes. You can turn on the debugging by uncommenting the //#define ENABLE_DEBUG 1 line at the top. This will print out what is happening as you run the sketch. Use the Arduino IDE to program you the sketch into your microcontroller and then lets run our new robot arm!

Step 19: Running the Robot.

We are now ready to run the robot. Make sure you have jacked in the 12V, 5A power supply to the DynamixShield, and that jumper J30 is in place to allow the Arduino to be powered from the DynamixShield. Also, be sure that jumpers for J17 are on as well. This will make sure that the Dynamixel servos get power. Here is the mapping that shows what each joystick axis does.

• Left horizontal axis: Rotates the robot around the base.
• Left vertical axis: Rotates the robot arm up and down.
• Left press down: Closes the gripper arm.
• Right horizontal axis: Rotates axis three to bend the elbow.
• Right vertical axis: Rotates axis four to bend the wrist.
• Right press down: Opens the gripper arm.

I hope you have enjoyed this instructable. If you like the DynamixShield and think you would find it useful then please support my Kickstarter campaign, (https://www.kickstarter.com/projects/neurorobotictech/arduino-shield-combines-smart-servos-and-plug-in-s) so I can produce this board commercially and make it available for your use.

Thanks,

David