Oracid

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

// from 8DOF-Q5-Bis-V1 - 14/07/2023 -
void(* resetFunc) (void) = 0;                               // soft reset function
#include <Servo.h>                                          // Servo library
int pb=A0, st;                                              // stop button and start time
int Speed=1650;                                             // delay between 2 orders
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
 
void  setup() {
  delay(400);                             // for reset consideration
  Serial.begin(9600);
  pinMode(pb,INPUT_PULLUP);               // start/stop/reset button attachment
 
  Srv[FRLS].attach( 2,Smin[FRLS],Smax[FRLS]); // FR Front Right leg - LS Left Servo
  Srv[FRRS].attach( 3,Smin[FRRS],Smax[FRRS]); // FR Front Right leg - RS Right Servo
  Srv[FLLS].attach( 4,Smin[FLLS],Smax[FLLS]); // FL Front Left  leg - LS Left Servo
  Srv[FLRS].attach( 5,Smin[FLRS],Smax[FLRS]); // FL Front Left  leg - RS Right Servo
  Srv[BRLS].attach(10,Smin[BRLS],Smax[BRLS]); // BR Back  Right leg - LS Left Servo
  Srv[BRRS].attach(11,Smin[BRRS],Smax[BRRS]); // BR Back  Right leg - RS Right Servo
  Srv[BLLS].attach(12,Smin[BLLS],Smax[BLLS]); // BL Back  Left  leg - LS Left Servo
  Srv[BLRS].attach(13,Smin[BLRS],Smax[BLRS]); // BL Back  Left  leg - RS Right Servo
 
  IK(0+F_tr,-Y,FRLS,FRRS); IK(0+B_tr,-Y,BRLS,BRRS); IK(0-F_tr,-Y,FLLS,FLRS); IK(0-B_tr,-Y,BLLS,BLRS);
 
  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 >= 5500)  resetFunc();
  Walk();
}
 
void Walk(){
  for( int i=Ax;i>=Bx;i=i-lg_Itr){ IK(+i+F_tr,-Y,FRLS,FRRS); IK(-i+B_tr,+Y,BRLS,BRRS); IK(+i-F_tr,+Y,FLLS,FLRS); IK(-i-B_tr,-Y,BLLS,BLRS); }
  for( int i=Bx;i<=Ax;i=i+lg_Itr){ IK(+i+F_tr,+Y,FRLS,FRRS); IK(-i+B_tr,-Y,BRLS,BRRS); IK(+i-F_tr,-Y,FLLS,FLRS); IK(-i-B_tr,+Y,BLLS,BLRS); }
}
 
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)
  int S1=round(degrees(B-A))+Err[LS];         // S1 is the left servo angle in degrees +Err
  int S2=round(degrees(B+A))+Err[RS];         // S2 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 : S1= ");Serial.print(S1);Serial.print("°\t\tS2= ");Serial.print(S2);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 (S1<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 S1=");Serial.print(S1);Serial.print(" <0° is not reachable !!!!!");return;}
  if (S2>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 S2=");Serial.print(S2);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 -
 
// Servos activation
  if (OnOff[LS]) Srv[LS].writeMicroseconds(map(S1,0,180,Smin[LS],Smax[LS]));   // set target Left servo if servo switch is On
  if (OnOff[RS]) Srv[RS].writeMicroseconds(map(S2,0,180,Smin[RS],Smax[RS]));   // set target Right servo if servo switch is On
  delayMicroseconds(Speed);                       // speed of action
}