I wanted to make some kind of a robot using my Arduino and some RC servo’s. I was spending a lot of time at All Hands Active, an underground hacker space in Ann Arbor. It’s a great workspace downtown with a super active group of really smart and creative “kids”.
I found some tongue depressors (wide Popsicle sticks) and metal tacks and decided to make a linkage.
I figured I could use two servo’s to control the position of a pen over a surface and make a simplistic plotting robot. The servo’s are accurate enough with the linkage to draw simple shapes, but not really good enough to write. It’s kind of a cool way to make a 2 axis manipulator though, you only needs a few penies worth of wood and glue, an Arduino, patience and a couple RC servos.
The tacks are the “smooth button kind”, the smooth surface touches the table, and the tacks create rotary joins. It works really well, pushing the tacks through the “popsicle sticks” creates a tight rotary joint. I added a second layer of tongue depressors to make it more stiff and keep the pointy bits of the tacks from being exposed.
I needed to attach the servo’s and after trying a lot of glues, it seemed like wood on wood parts were by attached the best using wood glue (weird.. i know), attaching the plastic servo bit to the wood worked best using epoxy. I glued on some extra wooden tongue depressor bits to give some lateral support to the “floating” servo because it’s impossible to exactly line up it’s axis of rotation with the pin joint so it would experience lateral loading as it turns.
I added a * RIDICULOUSLY* over complicated pen holder using yet more tongue depressors. I used a step drill to ream out the hole, a normal drill bit is useless at drilling thin stuff like a tongue depressor. I made it fit a Sharpie marker.
The trick in this robot is converting two axis of rotary motion into X and Y co-ordinates. I wanted to be able to say “robot: go to 0.5x and 1.3y”, I needed software that converted that into “30 degrees servo A, 72 degrees servo B”. The problem is that while it’s easy to calculate X and Y from angles and lengths using geometry, it’s harder to go the other way and figure out what angles you need to get a particular X and Y. There is a fairly straight forward solution if all the lengths of the linkage are equal, but you wouldn’t be a HUGE DORK if you relied on that crap, and you also wouldn’t be able to extend the method to any linkage (even 3D ones!!) or account for the offset of the pen tip from the joint.
Calculating X and Y from angles uses a technique from engineering called “inverse kinematics”. The assumption is that for relatively small motions, the direction of a small angular change can be assumed to be constant for a given motion.
The picture below show’s the math:
The cool thing is that exact technique can be used for different types of linkages, even those in 3D or having more than 2 actuators (although the method of matrix inversion changes a little).
The hardware implementation is super easy with the Arduino… the RC servo has 3 wires, Yellow: PWM signal, Black: Ground, Red: 5 volts. You can power the two servos from the Arduino board (because the power needed to move the pen is low), and the Arduino’s built in servo library takes care of converting the PWM signal into simple angle commands in the code.
If you’re doing this project, you have to be careful to glue the servo’s down with them rotated to a position that will give you the range of angles you need. After I built it, i added calibration variables to the code that account for the offset between the linkage angle, and the servo command angle.
The linkage needs to be fixed to the desk. I added a biiiiig stack of tongue depressors to the back of the base servo. I use a little plastic clamp, works pretty good.
Source Code _ Posicle Stick Robot is fairly straight forward… wait, no i just read it again, its quite not-straight forward!
The top part is full of all the constants that define the robot. Here are some big ones:
MYBASE_PIN; servo attached to base, pwm pin number
MYFLOAT_PIN; floating servo, pwm pin number
BASE_INIANG=0; the initial angle of the base servo wrt the table
FLOATINIANG=90; the initial angle of the floating servo wrt the table
CloseEnough=0.010; the robot will step until it gets this close
StepSize; the distance the robot will move per step
rbc=1.0; the distance from node b (floating servo) to node c (joint near pen)
rb=0.9739; the distance from node a (base servo) to node b
rcd=0.114; the distance from node c to the pen (node d)
x_1, y_1; the initial x and y positions of the pen
n_jacIters; # of iterations run to get the command angles
BASE_LOWLIM; the lowest angle the base servo will go without binding
BASE_HIGHLIM; the highest angle the base servo will go without binding
BASE_0DEGOFFSET=70; 0 degrees in table space is a servo command of 70
BASE_BACKLASH; the angle required to overcome backlash when reversing direction
(FLOAT_xxx; similar to the base variables)
VERBOSE; display intermediate data for debugging (0=no display, 1=display)
setup(); this initializes some variables
loop(); the main loop
getCoord(); subroutine that reads in commands from user
charToFloat(…); subroutine that converts user X.XXX commands to float
Cmnd_A_and_B(…); subroutine that commands servos, handles limits, and backlash
initialize_A_and_B(); initializes the servo’s during start up
refresh_A_and_B(); refreshes the servo’s position when not moving.
PrintCoordAndAngle(..) ; debugging print out
PrintCoord(..); debugging print out
Type1ToStart(); wait for user to press 1 to begin program