// based on 8DOF-Q5-Bis-V3 - 01/08/2023 -
void(* resetFunc) (void) = 0; // soft reset function
#include <Servo.h> // Servo library
int Speed=2050; // delay between 2 orders
int LS_A, RS_A, pb=A0, st; // Left and Right Servo Angle, stop button, start time
const int Ax=100, Bx=-100, Y=15, lg_Itr=15, F_tr=0, B_tr=-50; // Ax Bx Y rectangle coordinates, iteration length , front and back translation
const int nb=8; // number of servos
Servo Srv[nb]; // Servos table
int Smin[nb]={ 550, 500, 540, 500, 500, 590, 510, 510}; // all servos values for 0°
int Smax[nb]={2650,2560,2550,2500,2500,2520,2550,2600}; // all servos values for 180°
int OnOff[nb]={1,1,1,1,1,1,1,1}; // on/off Servos table
int FRLS=0, FRRS=1, FLLS=2, FLRS=3, BRLS=4, BRRS=5, BLLS=6, BLRS=7;
//servos FRLS FRRS FLLS FLRS BRLS BRRS BLLS BLRS // you must modify these values according to your servos
int Err[nb]={ -2, -2, -2, -4, -2, 1, 0, -2 }; // 90° correction error
const int lgTab=((Ax-Bx)*2/lg_Itr)+2;
int FRL[lgTab], FRR[lgTab], FLL[lgTab], FLR[lgTab], BRL[lgTab], BRR[lgTab], BLL[lgTab], BLR[lgTab]; // Angles tables
void setup() {
delay(400); // for reset consideration
Serial.begin(9600);
Serial.print("\n\n\n\t REFRESH_INTERVAL="); Serial.print(REFRESH_INTERVAL);
//Serial.print("\n\t lgTab="); Serial.print(lgTab);
pinMode(pb,INPUT_PULLUP); // start/stop/reset button attachment
Init_Tabs_Angles();
int a=65; // Servos angle initialization
if(OnOff[FRLS]) { Srv[FRLS].attach( 2,Smin[FRLS],Smax[FRLS]); Srv[FRLS].writeMicroseconds(map( a+Err[FRLS],0,180,Smin[FRLS],Smax[FRLS])); } // FR Front Right leg - LS Left Servo
if(OnOff[FRRS]) { Srv[FRRS].attach( 3,Smin[FRRS],Smax[FRRS]); Srv[FRRS].writeMicroseconds(map(180-a+Err[FRRS],0,180,Smin[FRRS],Smax[FRRS])); } // FR Front Right leg - RS Right Servo
if(OnOff[FLLS]) { Srv[FLLS].attach( 4,Smin[FLLS],Smax[FLLS]); Srv[FLLS].writeMicroseconds(map( a+Err[FLLS],0,180,Smin[FLLS],Smax[FLLS])); } // FL Front Left leg - LS Left Servo
if(OnOff[FLRS]) { Srv[FLRS].attach( 5,Smin[FLRS],Smax[FLRS]); Srv[FLRS].writeMicroseconds(map(180-a+Err[FLRS],0,180,Smin[FLRS],Smax[FLRS])); } // FL Front Left leg - RS Right Servo
if(OnOff[BRLS]) { Srv[BRLS].attach(10,Smin[BRLS],Smax[BRLS]); Srv[BRLS].writeMicroseconds(map( a+Err[BRLS],0,180,Smin[BRLS],Smax[BRLS])); } // BR Back Right leg - LS Left Servo
if(OnOff[BRRS]) { Srv[BRRS].attach(11,Smin[BRRS],Smax[BRRS]); Srv[BRRS].writeMicroseconds(map(180-a+Err[BRRS],0,180,Smin[BRRS],Smax[BRRS])); } // BR Back Right leg - RS Right Servo
if(OnOff[BLLS]) { Srv[BLLS].attach(12,Smin[BLLS],Smax[BLLS]); Srv[BLLS].writeMicroseconds(map( a+Err[BLLS],0,180,Smin[BLLS],Smax[BLLS])); } // BL Back Left leg - LS Left Servo
if(OnOff[BLRS]) { Srv[BLRS].attach(13,Smin[BLRS],Smax[BLRS]); Srv[BLRS].writeMicroseconds(map(180-a+Err[BLRS],0,180,Smin[BLRS],Smax[BLRS])); } // BL Back Left leg - RS Right Servo
Serial.print("\n\t To start, click on the Start button"); while( digitalRead(pb) ); delay(400); Serial.print("\n\t Started");
st=millis();
}
void loop() {
if( millis()-st >= 5000) resetFunc();
Walk();
}
void Init_Tabs_Angles(){
int i;
i=0; //Serial.print("\n\t Init_Tabs_Angles - Front right paw"); // Front right paw
for( int p=Ax;p>=Bx;p=p-lg_Itr ){ IK(+p+F_tr,-Y,FRLS,FRRS); FRL[i]=LS_A; FRR[i]=RS_A;
//Serial.print("\n\t p="); Serial.print(p);Serial.print("\t i="); Serial.print(i);
//Serial.print("\t FRL[i]="); Serial.print(FRL[i]); Serial.print("\t FRR[i]="); Serial.print(FRR[i]);
i++;
}
for( int p=Bx;p<=Ax;p=p+lg_Itr ){ IK(+p+F_tr,+Y,FRLS,FRRS); FRL[i]=LS_A; FRR[i]=RS_A;
//Serial.print("\n\t p="); Serial.print(p);Serial.print("\t i="); Serial.print(i);
//Serial.print("\t FRL[i]="); Serial.print(FRL[i]); Serial.print("\t FRR[i]="); Serial.print(FRR[i]);
i++;
}
i=0; //Serial.print("\n\t Init_Tabs_Angles Front left paw"); // Front left paw
for( int p=Ax;p>=Bx;p=p-lg_Itr ){ IK(+p-F_tr,+Y,FLLS,FLRS); FLL[i]=LS_A; FLR[i]=RS_A;
//Serial.print("\n\t p="); Serial.print(p);Serial.print("\t i="); Serial.print(i);
//Serial.print("\t FLL[i]="); Serial.print(FLL[i]); Serial.print("\t FLR[i]="); Serial.print(FLR[i]);
i++;
}
for( int p=Bx;p<=Ax;p=p+lg_Itr ){ IK(+p-F_tr,-Y,FLLS,FLRS); FLL[i]=LS_A; FLR[i]=RS_A;
//Serial.print("\n\t p="); Serial.print(p);Serial.print("\t i="); Serial.print(i);
//Serial.print("\t FLL[i]="); Serial.print(FLL[i]); Serial.print("\t FLR[i]="); Serial.print(FLR[i]);
i++;
}
i=0; //Serial.print("\n\t Init_Tabs_Angles Back right paw"); // Back right paw
for( int p=Ax;p>=Bx;p=p-lg_Itr ){ IK(-p+B_tr,+Y,BLLS,BLRS); BRL[i]=LS_A; BRR[i]=RS_A;
//Serial.print("\n\t p="); Serial.print(p);Serial.print("\t i="); Serial.print(i);
//Serial.print("\t BRL[i]="); Serial.print(BRL[i]); Serial.print("\t BRR[i]="); Serial.print(BRR[i]);
i++;
}
for( int p=Bx;p<=Ax;p=p+lg_Itr ){ IK(-p+B_tr,-Y,BLLS,BLRS); BRL[i]=LS_A; BRR[i]=RS_A;
//Serial.print("\n\t p="); Serial.print(p);Serial.print("\t i="); Serial.print(i);
//Serial.print("\t BRL[i]="); Serial.print(BRL[i]); Serial.print("\t BRR[i]="); Serial.print(BRR[i]);
i++;
}
i=0; //Serial.print("\n\t Init_Tabs_Angles Back left paw"); // Back left paw
for( int p=Ax;p>=Bx;p=p-lg_Itr ){ IK(-p-B_tr,-Y,BRLS,BRRS); BLL[i]=LS_A; BLR[i]=RS_A;
//Serial.print("\n\t p="); Serial.print(p);Serial.print("\t i="); Serial.print(i);
//Serial.print("\t BLL[i]="); Serial.print(BLL[i]); Serial.print("\t BLR[i]="); Serial.print(BLR[i]);
i++;
}
for( int p=Bx;p<=Ax;p=p+lg_Itr ){ IK(-p-B_tr,+Y,BRLS,BRRS); BLL[i]=LS_A; BLR[i]=RS_A;
//Serial.print("\n\t p="); Serial.print(p);Serial.print("\t i="); Serial.print(i);
//Serial.print("\t BLL[i]="); Serial.print(BLL[i]); Serial.print("\t BLR[i]="); Serial.print(BLR[i]);
i++;
}
}
void Walk(){
for( int i=0;i<=lgTab-1;i++){
if (! digitalRead(pb)) resetFunc();
if (OnOff[FRLS]) Srv[FRLS].writeMicroseconds(FRL[i]); // Front right paw, left servo
if (OnOff[FRRS]) Srv[FRRS].writeMicroseconds(FRR[i]); // Front right paw, right servo
delayMicroseconds(Speed);
//Serial.print("\n\t Walk Front Right");Serial.print("\t i=");Serial.print(i);
//Serial.print("\t FRL="); Serial.print(FRL[i]);Serial.print("\t FRR="); Serial.print(FRR[i]);
if (OnOff[BLLS]) Srv[BLLS].writeMicroseconds(BLL[i]); // Back left paw, left servo
if (OnOff[BLRS]) Srv[BLRS].writeMicroseconds(BLR[i]); // Back left paw, right servo
delayMicroseconds(Speed);
//Serial.print("\n\t Walk Back Left");Serial.print("\t i=");Serial.print(i);
//Serial.print("\t BLL="); Serial.print(BLL[i]);Serial.print("\t BLR="); Serial.print(BLR[i]);
if (OnOff[FLLS]) Srv[FLLS].writeMicroseconds(FLL[i]); // Front left paw, left servo
if (OnOff[FLRS]) Srv[FLRS].writeMicroseconds(FLR[i]); // Front left paw, right servo
delayMicroseconds(Speed);
//Serial.print("\n\t Walk Front left");Serial.print("\t i=");Serial.print(i);
//Serial.print("\t FLL="); Serial.print(FLL[i]);Serial.print("\t FLR="); Serial.print(FLR[i]);
if (OnOff[BRLS]) Srv[BRLS].writeMicroseconds(BRL[i]); // Back right paw, left servo
if (OnOff[BRRS]) Srv[BRRS].writeMicroseconds(BRR[i]); // Back right paw, right servo
delayMicroseconds(Speed);
//Serial.print("\n\t Walk Back right paw");Serial.print("\t i=");Serial.print(i);
//Serial.print("\t BRL="); Serial.print(BRL[i]);Serial.print("\t BRR="); Serial.print(BRR[i]);
}
}
void IK(int Px, int Py, int LS, int RS){ // Inverse Kinematics function
if (! digitalRead(pb)) resetFunc();
float Ax=0, Ay=355, c=200; // position of the main paw axis and length of femur and tibia
float d=Ay-Py, e=Ax-Px; // d and e, sides of rectangle triangle
float h=sqrt((d*d)+(e*e)); // h, hypotenuse of rectangle triangle
float B=asin(d/h); if(e<0)B=(PI-B); // B is the top angle of the rectangle triangle
float A=acos(h/(2*c)); // A is the Diamond half top angle (cosin law)
LS_A=round(degrees(B-A))+Err[LS]; // LS_A is the left servo angle in degrees +Err
RS_A=round(degrees(B+A))+Err[RS]; // RS_A is the right servo angle in degrees +Err
//DEBUG
// Serial.print("\n\t Px="); Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t\tLeg servos : LS=");Serial.print(LS);Serial.print(" RS=");Serial.print(RS);
// Serial.print("\n\t Inverse Kinematics values : B°= ");Serial.print(round(degrees(B)));Serial.print(" A°= ");Serial.print(round(degrees(A)));
// Serial.print("\n\t Servo angles results : LS_A= ");Serial.print(LS_A);Serial.print("°\t\tRS_A= ");Serial.print(RS_A);Serial.print("°");
// - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL -
if (h>(c+c)){Serial.print("\n\t Target : Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t h=");Serial.print(h);Serial.print(" > ");Serial.print(c+c);Serial.print(" is too long !!!!!");return;}
if (LS_A<0) {Serial.print("\n\t Target : Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t LS=");Serial.print(LS);Serial.print("\t angle LS_A=");Serial.print(LS_A);Serial.print(" <0° is not reachable !!!!!");return;}
if (RS_A>180) {Serial.print("\n\t Target : Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t RS=");Serial.print(RS);Serial.print("\t angle RS_A=");Serial.print(RS_A);Serial.print(" >180° is not reachable !!!!!");return;}
// - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL - RESTRICTED AREA CONTROL -
LS_A=map(LS_A,0,180,Smin[LS],Smax[LS]);
RS_A=map(RS_A,0,180,Smin[RS],Smax[RS]);
}
