//Inverse Kinematics for Use with Robotic Biped by Quinn Mikelson

// servos max and min 750 ---- 2100

//L1 = 58mm

//L2 = 45mm

const float pi = 3.14159; //PI Defined

float x, y; //X and Y Variables used by the IK subroutines

float theta1, theta2, theta3; //Unknown angles calculated for servos 3,4,5,19,20,21

float l1 = 75; //length of the connections between each leg

float l2 = 57; // " "

float servo3pulse, servo4pulse, servo5pulse, servo19pulse, servo20pulse, servo21pulse; //The microsecond pulses (750 - 2100) used to move servos

float servo1, servo2, servo3, servo4, servo5, servo6, servo17, servo18, servo19, servo20, servo21, servo22, a1500pulse; // Used in the StartPos routine

float fracdone; // X - Minimum / Maximum = Fraction(0 --- 1) of step completion

int FirstStep = 0; // May be used later, useful for synchronous movement

void setup() {

Servos(); // Defines servo starting posistions

Serial.begin(9600);

StartPos(); // moves all servos to starting posistions

delay(2000);

Serial.println("READY");

}

void loop() {

//int i;

float xCenter = -30; // Defined numbers critical for leg movement, currently I still have to use Excel to calculate these

float yCenter = -202.14;

float CirRadius = 119.14;

int leg;

int xmin, xmax, xstep;

xmin = -80; // User Inputted Min / Max Step Values

xmax = 20;

xstep = 10;

for(leg = 1; leg <= 2; leg++){ // switches the value for leg, Leg = 1, then Leg = 2, Then Leg = 1, etc....

for(x = xmin; x <= xmax; x = x + xstep){ // runs through all values from min to max based on inverse kinematic equations

y = yCenter + sqrt(sq(CirRadius)-sq(x-xCenter)); // calculates the Y variable, also used in IK

fracdone = (x - xmin) / (xmax-xmin); // the fracdone equation, used in Shift

CheckHypotenuse(); // I dont know why i did this, an oversight perhaps? I just dont really want to modify things yet (it all sorta works)

if(CheckHypotenuse()==0){ // if the angle is too far to reach, stop movement

return;

}

IK(); // calculates X and Y into Thetas

normalize(); // Corrects for servo error, and puts servos in correct quadrant

if(leg == 1){ // if First Leg, do only this...

servo3pulse = D2Pminus(a1500pulse, theta1);

servo4pulse = D2Pplus(a1500pulse, theta2);

servo5pulse = D2Pminus(a1500pulse, theta3);

shift(leg);

buildmove(3, servo3pulse); //Servo 3 = servo3pulse = theta1

buildmove(4, servo4pulse); //Servo 4 = servo4pulse = theta2

buildmove(5, servo5pulse);

finishmove(500);

delay(500);

}

else{ //If Second Leg, do only this...

servo19pulse = D2Pplus(a1500pulse, theta1);

servo20pulse = D2Pminus(a1500pulse, theta2);

servo21pulse = D2Pplus(a1500pulse, theta3);

shift(leg);

buildmove(19, servo19pulse); //Servo 3 = servo3pulse = theta1

buildmove(20, servo20pulse); //Servo 4 = servo4pulse = theta2

buildmove(21, servo21pulse);

finishmove(500);

delay(500);

}

} //Next x

FirstStep = 1; //Tells the program the initial step has taken place

} //Next leg

}

void IK(){

// Based on Robot_Arm_Designer.xls

float c2, s2, k1, k2;

c2=(sq(x)+sq(y)-sq(l1)-sq(l2))/(2*l1*l2);

s2=sqrt(1-sq(c2));

k1=l1+l2*c2;

k2=l2*s2;

theta1=atan2(y,x)-atan2(k2,k1);

theta2=atan2(s2,c2);

if (theta1<0){

theta1 = 2 * pi + theta1;

}

if (theta2<0) {

theta2 = 2 * pi + theta2;

}

theta2 =(theta2 * (180.0/pi));

theta1 = (theta1 * (180.0/pi));

theta3 = (360 - theta1 - theta2);

}

int D2Pminus(int hardware, float deg){

return(hardware - deg * 10);

}

int D2Pplus(int hardware, float deg){

return(hardware + deg * 10);

}

void buildmove(int servo, int position1) {

Serial.print(" #");

Serial.print(servo);

Serial.print(" P");

Serial.print(position1);

}

void finishmove(int time) {

Serial.print(" T");

Serial.println(time);

}

void check(){

if (servo3pulse <= 750) servo3pulse = 750;

else if (servo3pulse >= 2100) servo3pulse = 2100;

else if (servo4pulse <= 750) servo4pulse = 750;

else if (servo4pulse >= 2100) servo4pulse = 2100;

else if (servo19pulse <= 750) servo19pulse = 750;

else if (servo19pulse >= 2100) servo19pulse = 2100;

else if (servo20pulse <= 750) servo20pulse = 750;

else if (servo20pulse >= 2100) servo20pulse = 2100;

}

int CheckHypotenuse(){ //Returns 1 if Target is Within Reach, 0 if Target Out of Reach

float hypotenuse;

hypotenuse = sqrt(sq(x) + sq(y));

if ((hypotenuse - l1 -l2) > 0.001) {

Serial.print("hypotenuse =");

Serial.println(hypotenuse);

Serial.print("This is greater than");

Serial.println(l1 + l2);

return(0);

}

else

return(1);

}

void normalize(){

theta1 = theta1 - 270;

theta2 = theta2 - 90;

theta3 = theta3 - 75;

}

void shift(int leg){

int rollfootpulse;

if(fracdone == 0){

int angle = 15;

if(leg == 1){

rollfootpulse = D2Pplus(servo6, angle);

buildmove(6, rollfootpulse);

rollfootpulse = D2Pplus(servo2, angle);

buildmove(2, rollfootpulse);

rollfootpulse = D2Pplus(servo22, angle);

buildmove(22, rollfootpulse);

rollfootpulse = D2Pplus(servo18, angle);

buildmove(18, rollfootpulse);

finishmove(800);

return;

}

else if(leg == 2){

rollfootpulse = D2Pminus(servo6, angle + 2);

buildmove(6, rollfootpulse);

rollfootpulse = D2Pminus(servo2, angle + 2);

buildmove(2, -rollfootpulse);

rollfootpulse = D2Pminus(servo22, angle + 2);

buildmove(22, rollfootpulse);

rollfootpulse = D2Pminus(servo18, angle + 2);

buildmove(18, -rollfootpulse);

finishmove(800);

return;

}

}

if(fracdone == .8){

int angle = 0;

if(leg == 1){

rollfootpulse = D2Pplus(servo6, angle);

buildmove(6, rollfootpulse);

rollfootpulse = D2Pplus(servo2, angle);

buildmove(2, -rollfootpulse);

rollfootpulse = D2Pplus(servo22, angle);

buildmove(22, rollfootpulse);

rollfootpulse = D2Pplus(servo18, angle);

buildmove(18, -rollfootpulse);

finishmove(800);

return;

}

else if(leg == 2){

rollfootpulse = D2Pminus(servo6, angle);

buildmove(6, rollfootpulse);

rollfootpulse = D2Pminus(servo2, angle);

buildmove(2, -rollfootpulse);

rollfootpulse = D2Pminus(servo22, angle);

buildmove(22, rollfootpulse);

rollfootpulse = D2Pminus(servo18, angle);

buildmove(18, -rollfootpulse);

finishmove(800);

return;

}

}

}

void Servos(){

servo1 = 1500;

servo2 = 1540;

servo3 = 1900;

servo4 = 1300;

servo5 = 1700;

servo6 = 1550;

servo17 = 1500;

servo18 = 1650;

servo19 = 1100;

servo20 = 1700;

servo21 = 1300;

servo22 = 1550;

a1500pulse = 1500;

return;

}

void StartPos(){

buildmove(1, servo1);

buildmove(2, servo2);

buildmove(3, servo3);

buildmove(4, servo4);

buildmove(5, servo5);

buildmove(6, servo6);

buildmove(17, servo17);

buildmove(18, servo18);

buildmove(19, servo19);

buildmove(20, servo20);

buildmove(21, servo21);

buildmove(22, servo22);

finishmove(500);

}