The story of the Smartarduino Doit 6Dof Robotic Arm - Part 1

I've you've never wanted a robotic arm, this story is not for you. However, if you're like me, and you couldn't possibly think of a better way to invest a couple of hundred dollars, then read on.

A few days later, you will receive a cardboard box. No gloss, no smiling kids, just a brown box full of little plastic bags.

Take them out and look at them.

Ah, crap.

There is no way in Hell the contents of those bags could ever become a robot.

It came with DHL, a Christmas present from my dear Elisabeth

But, wait. There's a USB cable in there. They wouldn't put a USB cable in a box, unless ...

Hmm ... it had a USB cable, too ... I wonder ...

I grabbed the two biggest pieces and held them up together. One is a bracket of some sort and the other is ... a bracket of some sort. They didn't snap together or fit in any other way. Not for children, huh? This was not going to be easy. Besides,

I'm a computer programmer. A fluff-head, not a gear-head. 

I rummaged through the plastic bags and found a big bearing, then I looked at the photograph of the robotic arm at www.smartarduino.com. 

Yup. That bearing was going to sit between the two big brackets. Cool. This might be a robotic arm after all.

Scratching my head some more, I realized that it would be impossible to put this together without some sort of instruction manual. I found one, and since then, the vendor, www.smartarduino.com, has put one up on the site: Download it here

Milestone

The next day, I found that I had used up almost all the screws, there were no strange looking mechanical parts left in the box, and, most importantly, the thing looked and felt like a robotic arm.

Cool. Look how happy I am.

Now, for the software. I fasten the ESPDuino board and the PWM shield at the base of the robot, connect all the wires ... um ... some of those wires are too short. A company in Denmark, Let-Elektronik, sells extensions. Beautiful ones, too:

Me? I was too fired up to wait. I found some masking tape, a soldering iron, a butchered USB cable, and pressed on. The ESPDuino board and shield assembly don't really fit on the bracket, but the bracket has slits so it will fit in a skewed sort of way, using leftover screws from the brown cardboard box.

The robot is well engineered. This is stainless steel, not a plastic toy. Yet, one thing bothers me: The backlash between Servo 3 and the connecting rod to Joint 2. There are a number of ways to deal with this. The best way would be to find a tiny rubber band that would sit on the bearing. Another would be to find a clip, snap it onto the bearing and fasten it with masking tape. 

See the lower-left part of the photograph that is not out of focus. 

(Never mind the remote control. That's for the TV.)

Milestone 2

With trembling hands, I find a USB extension cable, plug it into my home computer, an aging, but adequate IBM x3440 e-Server sporting 16gigs of memory. Should I need it, I can add a second processor card and 16 gigs more. Holding my breath, I plug the other end into the blue USB cable.
Expecting a terrible explosion, howling sirens, and blinking blue flashes, I plug the blue USB cable into the ESPDuino controller board.

"VZZ"

I jump. Did it speak?

Nah. But the servos are powered up. The robot's joints won't budge. I keep it connected for a while, sniffing the air, touching the servos to feel the heat. 

Nothing. Powerful electromagnetic forces keep every angle fixed, and there's no heat dissipation whatsoever.

Great.

Now, what?

Milestone 3 - The software

As it turns out, I'm not even half finished. The hardware is in place. To a programmer, this means that the computer is plugged in and ready for use ... except that it needs an operating system, a development system, documentation and a touch of genius before it will do anything interesting.

I recognize the feeling. I felt it once before - in 1985 when I plugged my Sinclair Spectrum to the TV and typed 

PRINT "Hello"

This is 2017. Back in 1985, the unassuming little ESP-32 with its 1MB memory would have been the size of a freezer and cost more than a house.
The specs are impressive. Read them yourself.
It has built-in PWM capability for controlling the servos, but we won't be using that. We have a PWM 16 channel servo controller board mounted on top. There's more - much more - but if I'm ever going to finish this blog, I need to focus on what's actually used.

I should mention that I use Linux, not Windows. If you're a Windows or Mac user, you're probably not reading this, anyway.

The first thing I need is the Arduino IDE. There is probably an Arduino IDE in your package manager. If it's older than 1.8.4, get rid of it. Download from the link above and install it in your home dir.

Then, download The manuals for the ESPDuino and the servo controller board.
Follow the instructions.
No, I won't repeat them here.

Milestone 4 - The first robot Control program

By now, you should have installed the ESP-32 into the board manager.

If not, you know what to do. You want a menu that looks like the one below (or better).

You should also have installed the Adafruit PWM servo driver library.

If you've done all that, you're ready to program the robot.

The first thing to do is to include the AdaFruit library. 

From now on, an assumption is made: You have connected the base servo (Servo 0) to output 0, the servo for the first elevation arm (Servo 1) to output 1, the servo driving the connecting rod to output 2, and the wrist to 3, 4, and 5 - where 5 moves the end effector.

 /*************************************************** 

  This is an example for our Adafruit 16-channel PWM & Servo driver

  Servo test - this will drive 16 servos, one after the other

  Pick one up today in the adafruit shop!

  ------> http://www.adafruit.com/products/815

  These displays use I2C to communicate, 2 pins are required to  

  interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4

  Adafruit invests time and resources providing this open source code, 

  please support Adafruit and open-source hardware by purchasing 

  products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries.  

  BSD license, all text above must be included in any redistribution

 ****************************************************/

#include <stdio.h>

#include <Wire.h>

#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// you can also call it with a different address you want

//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);

// Depending on your servo make, the pulse width min and max may vary, you 

// want these to be as small/large as possible without hitting the hard stop

// for max range. You'll have to tweak them as necessary to match the servos you

// have!

#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)

#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)

Now, you have access to the servos through the object pwm.

If you don't believe me, compile this sketch and run it:

void setup() {

  Serial.begin(9600);

  pwm.begin();

  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

  while (!Serial) {

    ; // wait for serial port to connect. Needed for native USB port only

  }

  yield();

}

int a =90;

void loop(){

    if (a < SERVOMAX){

        a++;

    }

    if (a > SERVOMIN){

        pwm.setPWM(0, 0, a);

        delay(100);

    }

}

Oh, I forgot. There's supposed to be a 'FLASH' button on the board. 

Well, there isn't.

UPDATE: Don't worry about any of this. Plug in the USB cable, click the upload button and watch as the binary file gets transferred to the ESPDuino's memory.

In order to express the servo angles in degrees and not in pulse width, you can define a macro like the one below.

#define a2p(a) (int)(SERVOMIN + ((a * (SERVOMAX - SERVOMIN)) / 180))

Then, you can save the state of the robot as an array of angles.

float oldAngle[8]; /* Current state of the robot */

Before you compile, upload, and destroy something, let me 

stop you right there.

This is not the whole story.

Remember the connecting rod? Well, the next chapter is dedicated to that little bugger.

UPDATE: As it turns out, that thing was a feature, not a bug--but I had to learn kinematics and read a long and intricate scientific paper before I felt I understood something.

Have fun watching me Forrest Gump my way through it.

The Connecting Rod Catastrophe

Play this video.

Look at joints 1 and 2 as I alter the angle of servo 1. See how the angle of joint 2 increases as the servo increases angle 1.

Unless something is done to keep angle 2 constant as angle 1 changes, the connecting rod becomes too long, too short, or ends up in the dead center of joint 2.

In practice, the machine will lock up. The servos could overheat. If the servos were strong enough, the connecting rod would snap or bend.

There is a word for this. A mathematical word: Catastrophe. Watch this  most excellent video and be sure to understand what I mean when I later refer to some states: Stable Right, Stable Left, Catastrophe.

Where did I get the values 150 and 250? In fact, your values may be different. First, I built a control program in Java, with sliders to move the servos.

Then, I detached the connecting rod from the joint and moved servos back and forth. For every move, I checked to see if I could reattach the rod.
If not, I pressed the button marked 'Catastrophe'.
If I could attach the rod on the left side, crossing the joint, I pressed 'Stable L'.
If I could attach the joint normally, I pressed 'Stable R'.
Every time I pressed one of the three buttons, it would generate a line in the 'Data' window.

I copied the lines into a file, sorted it and entered it into a spreadsheet.

In the spreadsheet, I added columns, most notably one expressing the sum of angles 1 and 2.

Then I plotted it, and log & behold, I found the values 150 and 250.

What remains to be done, is go back to the Arduino sketch and 'translate' the value of a2 into a sensible PWM value, dependent on the state of angle 1, using the knowledge I have gained so far.

First, I'll define some macros to simplify things:

#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)
#define SERVORANGE (SERVOMAX-SERVOMIN)

// The findings from the spreadsheet #define A2MIN 150
#define A2MAX 250
#define A2RANGE (A2MAX-A2MIN)

The macro to convert from angle to pulse width is pretty straightforward:
#define a2p(a) (int)(SERVOMIN + ((a * SERVORANGE) / 180))

However, for Servo 2, it becomes more complicated. The joint angle a22a depends on both Servo 2 and Servo 1. It must always maintain the invariant 150 <  a1 + a2 < 250.
#define a22a(a,a1) ((A2MIN-a1) + ((a * A2RANGE) / 180)) <<< WRONG!!

Once we have the joint angle, we can get the pulse width.
#define a22p(a,a1) (a2p(a22a(a, a1)))

Yay! End of Part 1.
The Arduino sketch is available on Github.

Now, onwards to the difficult stuff.