So I have successfully completed the first part of my first quadruped leg programming experiment. I have managed to send the foot of a 2 dimensional 2 servo leg to an x,y coordinate.

to choose a new coordinate, the program must be modified (the x and y intergers) and reloaded, but it DOES use an Inverse Kinematics equation to assign the angles to the servos. Much of my difficulty came from trying to learn how the equations should look, some of it came from errors in program organization and io assignment.

Here is the code.

----------------------------------------

//servo leg experiment. 2 servos moving on a 2 dimensional x,y coordinate plane. hip servo at 0,0

//hip servo oriented such that 0 degrees is straight downward, and 180 is straight upward

//knee servo oriented so that 0 is directly toward femur, and that 180 is directly away from tibia (so 180 fully extends the leg)

//my model is backward, with the leg extending to my left, so I input negative values and it seems to want to work

//there are still problems with this code, the solutions it may pick may not be within the range of motion of the servos.

//all measurements are in milimeters.

//open serial monitor to see angle B then angle A, B is hip angle so printed first

#include <Servo.h>

#include <math.h>

Servo hip;

Servo knee;

int femur = 100;

int tibia = 90;

void setup() {

// put your setup code here, to run once:

hip.attach(9);

knee.attach(10);

Serial.begin(9600);

}

void loop() {

// put your main code here, to run repeatedly:

int x = -150; //change this value to assign a new desired x coordinate, and re-upload program

int y = 60; //change this value to assign a new desired y coordinate, and re-upload program.

//assuming servo hip is oriented, 0 straight down, 180 straight up, y is inverted, so -y is above the x axis

float L = sqrt((x*x)+(y*y));

float A = acos(((femur*femur)+(tibia*tibia)-(L*L))/(2*femur*tibia));//outputs number in radians as A

float B = acos(((femur*femur)+(L*L)-(tibia*tibia))/(2*femur*L))+acos(((L*L)+(y*y)-(x*x))/(2*L*y));//outputs number in radians as B

Serial.print(degrees(B));//converts radians to degrees

Serial.print(" ");

Serial.println(degrees(A));//converts radians to degrees

hip.write(degrees(B));//converts radians to degrees

knee.write(degrees(A));//converts radians to degrees

delay(50);

}