So we're in the early stages of building this robot-drawer thing. We have a MAX patch that's doing some face-tracking and we are getting a single pair of X,Y coords out of it. We want to convert this to usable data for a servo-drawing apparatus. The challenge is that we need to get this into circular motion somehow. Here are some pictures attached.
The paper shows two arms, the bottom and middle circles are servos. We need to angle the "shoulder" servo, then angle the "elbow" servo.
We got this from a robotics website but can't make much sense of it:
//Use inverse kinematics to find the angles of the motors like the following equations:
float l1 = 80.4; // the length of the shoulder
float l2 = 88; // the length of the elbow
double cb2 = 2*l1*l2;
float c2 = (sq(x) + sq(y) - sq(l1) - sq(l2)) / cb2;
float s2 = sqrt(1-sq(c2));
float k1 = (l1 + l2 * c2);
float k2 = (l2 * s2);
float base_ang = rtod((atan2(y,x) - atan2(k2,k1)));
float elbow_ang = rtod(atan2(s2,c2)) + 90;
// x servo, y sero
myservo1.write(base_ang); myservo2.write(elbow_ang);
Our design is kind of similar to this: http://www.blogcdn.com/www.engadget.com/media/2012/03/doodle-clock.jpg in that it has a shoulder and elbow motor.
