#include <Servo.h>

Servo servohorfrR;  // Servomotors for front right
Servo servovertfrR;
Servo servohorfrL;  // Servomotors for front left
Servo servovertfrL;
Servo servohorbcR;  // Servomotors for back right
Servo servovertbcR;
Servo servohorbcL;  // Servomotors for back left
Servo servovertbcL;


int pos = 0;    // variable to store the servo position
int pos2 = 0;
int count = 0 ;

void setup() {
  servohorfrR.attach(9);  // attaches the servo on pin 9 to the servo object
  servovertfrR.attach(10);
  servohorfrL.attach(1);
  servovertfrL.attach(2);
  servohorbcR.attach(3);
  servovertbcR.attach(4);
  servohorbcL.attach(5);
  servovertbcL.attach(6);


  servohorfrR.write(pos);  // attaches the servo on pin 9 to the servo object
  servovertfrR.write(pos2);
  servohorfrL.write(pos);
  servovertfrL.write(pos2);
  servohorbcR.write(pos);
  servovertbcR.write(pos2);
  servohorbcL.write(pos);
  servovertbcL.write(pos2);
}

void loop() {
  
  //Movement of the leg front right, has to be split up but the timing and switching between movements will have to be decided with the actual robot
   for (pos = 0; pos <= 180; pos += 4) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servohorfrR.write(pos);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 1 ms for the servo to reach the position
     }
   for (pos2=0; pos2 <= 45; pos2 +=1) {
      servovertfrR.write(pos2);
      delay(10);
      }

  



// Movement of the leg front left
    for (pos = 0; pos <= 180; pos += 4) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servohorfrL.write(pos);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 1 ms for the servo to reach the position
     }
     for (pos2=0; pos2 <= 45; pos2 +=1) {
      servovertfrL.write(pos2);
      delay(10);
      }

 





// Movement of the leg back right
      for (pos = 0; pos <= 180; pos += 4) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servohorbcR.write(pos);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 1 ms for the servo to reach the position
        }
    for (pos2=0; pos2 <= 45; pos2 +=1) {
      servovertbcR.write(pos2);
      delay(10);
     }

 



  // Movement of the leg back left
     for (pos = 0; pos <= 180; pos += 4) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servohorbcL.write(pos);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 1 ms for the servo to reach the position
     }
    for (pos2=0; pos2 <= 45; pos2 +=1) {
      servovertbcL.write(pos2);
      delay(10);
     }

  


// Moving front leg right back
    for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    servohorfrR.write(pos);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 10 ms for the servo to reach the position
    }
    for (pos2=45; pos2 >= 0; pos2 -=1) {
      servovertfrR.write(pos2);
      delay(10);
     }



// Moving front leg left back
    for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    servohorfrL.write(pos);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 10 ms for the servo to reach the position
    }
      for (pos2=45; pos2 >= 0; pos2 -=1) {
      servovertfrL.write(pos2);
      delay(10);
     }




// Moving back leg right back
   for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    servohorbcR.write(pos);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 10 ms for the servo to reach the position
     }
   for (pos2=45; pos2 >= 0; pos2 -=1) {
      servovertbcR.write(pos2);
      delay(10);
   }



// Moving back leg left back
    for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    servohorbcL.write(pos);              // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 10 ms for the servo to reach the position
    }
   for (pos2=45; pos2 >= 0; pos2 -=1) {
      servovertbcL.write(pos2);
      delay(10);
    }
}