you can see some of the error modes i was encountering in the ^ shaped "straight line" where theta1 was clipping out because I was feeding it negative values based on my inverse kinematics calculated angles
i blame servo jitter for how shaky the lines are :)
this took forever and a heaping bowlful of confusion to get to where i am. actually i'm still confused. but basically i spent all day sunday working on this, emailed out to MITERS, and finally a hallmate, pranjal, helped me out (https://github.com/pranjalv123/servoarm) today and fixed where I was stuck at in an hour or two.
essentially he rewrote the code in python (I was actually starting to do this) to graph and understand what was going on. For instance, the bottom-most image is the working envelope of the robot arm;
Isn't that fascinating? It's like a yin yang. If you play around with the servo arm this sort of working envelope makes sense.
So turns out my code was decently fine, the negative values just meant I was giving it bad inputs that it physically couldn't reach given the arm link lengths I'd given it.
We initially tried to wrap the negative values around by doing mod180 'ing it,
theta1 = ((int)theta1+180)%180
but this gives the sort of trajectory shown in the upper image
(I think... it may also be that elbowup needs to be false to generate that sort of trajectory)
setting elbowup to be true fixed a lot of issues to (which makes sense physically as more x,y coordinates can be reached if the elbow is up rather than down, it's easy to see if you play around with it ... see the inverse kinematics chapter on http://www.eng.utah.edu/~cs5310/chapters.html if this elbowup/down stuff is coming out of nowhere -- basically there are two combinations of theta 1 and theta 2 that will work for any given x,y coordinate and you just pick whether you want the elbow up or down solution)
and finally the working enveloped helped me pick correct x,y values to feed it. Initially I was just basing my x,y values off of the instructables www.instructables.com/id/Robotic-Arm-with-Servo-Motors/?ALLSTEPS but scaled down 1/3, which was just a guess of mine based on my servo joint limitations versus her motor joint limitations
it is also surprisingly close to the dimensions in mm I give it, eg.
will give me a line about 60 mm long (I put the link lengths in as mm), which is exciting.
I'm still not sure what's going on with why the straight line up and down is at a 45degree angle, but that's probably a constant offset problem. Fixable either in code or if I set the initial conditions on the angle the links are mounted on the servos better.