//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);
}