This blog is closed!

The Maschinenraum project and this blog are now closed. If you're interested, you can follow me on my personal blog over at my website.

Wednesday, June 3, 2009

Articulated plotting robot for €10

As our first real Arduino project I came up with something that would give me quick results using cheap and available parts to be able to gather some experience in programming on the Atmel controller. I used two servos which can be driven directly off the Arduino board without additional circuitry to build a 2D robot arm being able to draw.

The arm structure is made of 3mm cardboard, using a longer screw and some wire to reinforce the upper arm - the "elbow" is floating freely, the only physical contacts to the base plane are the "shoulder" servo (which is screwed to the base) and the drawing tip. If the (heavy) drawing head is removed the tip of the forearm floats freely too.

I am using two "mini" hobby servos, available for approximately €5,- each. The drawing head is really just a wrench socket with a cut-off ball pen glued in.

Software-wise I implemented the whole stack of low-level servo control, servo position calculation, position interpolation and the transformation of Cartesian coordinates into polar coordinates to run on the Arduino. Currently, I am hard-coding the geometry to be drawn in the source code, but clearly a serial connection could be used to feed coordinates to the robot in real-time.

So here is how all this looks in action:

Unfortunately, as you can see all this sounds a bit nicer in theory than it looks in practice - although the theoretical accuracy of the servos should allow acceptable results, the mechanical error introduced by friction, the poor accuracy of the cheap servos and the amplification of small angular errors through the long arms results in an overall positional error of approximately +-0,5 cm (even more in some bad cases) which cannot be compensated in software. Even worse, as can be seen at the beginning of the video, in some cases the arm starts to oscillate quite violently in its attempts to correct a mechanically introduced error. This oscillation can often be only stopped by human intervention and may damage the small plastic gears inside the servos or other parts of the construction.

The result of my demo program - drawing an orthogonal spiral with a 1cm radius increment at each winding - is shown below. (Sorry for the bad pictures, I only have a webcam available for taking pictures right now)

For the future, besides the serial control mentioned above, a third servo would be nice which could lift the drawing head so the arm could move without drawing. Given the poor results so far I am not sure if I will do this, though.

Finally, the source code for the technically inclined:

// IMPORTANT: to facilitate integer math
// - angles are handled in "ticks" (= microseconds for servo control, approx. 1/10th degree)
// - coordinates are handled in tenths of millimeters

byte servoPin1 = 7;
byte servoPin2 = 8;

int angle1 = 900;
int angle2 = 900;

int targetAngle1;
int targetAngle2;

int SERVO_CALIB_1 = -60;
int SERVO_CALIB_2 = -60;

char dA1 = 0;
char dA2 = 0;

int i;

// conversion from rad to ticks (= microseconds for servo control)
// should be ~573, but 560 works better for me
float RAD_TO_TICKS = 560;

int coords[] = { 0, 0 };

// intial position
int posX = 1500;
int posY = 1500;

void setup() {
pinMode(servoPin1, OUTPUT);
pinMode(servoPin2, OUTPUT);

Serial.println("Pappkamerad says hello.");

// just draw the spiral once
drawOrthoSpiral(1500, 1500, 100, 10);

void loop() {
// nothing left to do

// future: read commands from serial and plot geometry

void drawOrthoSpiral(int center_x, int center_y, int stepwidth, byte steps) {

int pos = 0;

lineTo(center_x + stepwidth, center_y);

int i;

for (i=1; i<steps; i++) {
pos += stepwidth;
lineTo(center_x + pos, center_y - pos);
lineTo(center_x - pos , center_y - pos);
lineTo(center_x - pos, center_y + pos);
lineTo(center_x + pos + stepwidth, center_y + pos);


unsigned long LENGTH_A = 1850;
unsigned long LENGTH_B = 1680;

unsigned long A_SQUARED = LENGTH_A * LENGTH_A;
unsigned long B_SQUARED = LENGTH_B * LENGTH_B;


int* xyToPolar(long x, long y) {

//print("coords: ", x, y);

long r_squared = x*x + y*y;
float r = sqrt(r_squared);
int phi = acos(x * 1.0/r) * RAD_TO_TICKS;

//print("polar: ", r, phi);

// equal arm length case
int beta = asin(r / 3700.0) * RAD_TO_TICKS * 2; // double arm length = 370mm
int alpha = 1800 - phi - (beta / 2);

// different arm lengths case
// use cos rule to get angle from 3 known sides
int alpha = acos((A_SQUARED + r_squared - B_SQUARED)/(LENGTH_A * r * 2)) * RAD_TO_TICKS;
alpha += 900 - phi;

int beta = acos((float)(A_SQ_PLUS_B_SQ - r_squared) / A_TIMES_B_DOUBLE) * RAD_TO_TICKS;

//print("arm: ", alpha, beta);

// TODO: this uses a global array to store coords, ok if you know what you are doing
coords[0] = alpha;
coords[1] = beta;

return coords;

// draw line in cartesian coordinates
void lineTo(int targetX, int targetY) {

int steps = max(abs(posX-targetX), abs(posY-targetY)) << 2;

for (i=0; i<=steps; i++) {
xyToPolar(map(i,0,steps,posX,targetX), map(i,0,steps,posY,targetY));
servoPulse2(coords[0], coords[1]);

posX = targetX;
posY = targetY;


void print(char* str, long n1, long n2) {
Serial.print(", ");

char sign(long number) {
if (number == 0) return 0;
if (number < 0) return -1;
return 1;

int pulseWidth;

void servoPulse2(int ticks1, int ticks2) {

ticks1 += SERVO_CALIB_1;

// safety measure to protect robot from self-collision
ticks2 = max(100, ticks2);
ticks2 = 1800 - ticks2;

ticks2 += SERVO_CALIB_2;

//print("Servo Pulse: ", ticks1, ticks2);

int tickDiff = ticks2 - ticks1;
char tickSign = sign(tickDiff);

if (tickSign == 0) {
pulseWidth = ticks1 + 500;
digitalWrite(servoPin2, HIGH);
digitalWrite(servoPin1, HIGH);
digitalWrite(servoPin1, LOW);
digitalWrite(servoPin2, LOW);
else if (tickSign == 1) {
pulseWidth = ticks1 + 500;
digitalWrite(servoPin1, HIGH);
digitalWrite(servoPin2, HIGH);
digitalWrite(servoPin1, LOW);
digitalWrite(servoPin2, LOW);
else if (tickSign == -1) {
pulseWidth = ticks2 + 500;
tickDiff = -tickDiff;
digitalWrite(servoPin1, HIGH);
digitalWrite(servoPin2, HIGH);
digitalWrite(servoPin2, LOW);
digitalWrite(servoPin1, LOW);