#include <Servo.h>

Servo serwo1, serwo2, serwo3, serwo4, serwo5, serwo6;

void setup() {
    serwo1.attach(3);//powiazanie pinow z serwami
    serwo2.attach(5);
    serwo3.attach(6);
    serwo4.attach(9);
    serwo5.attach(10);
    serwo6.attach(11);
    
    //pozycja zerowa
    serwo1.write(90);
    serwo2.write(70);
    serwo3.write(70);
    serwo4.write(90);
    serwo5.write(70);
    serwo6.write(70);
    delay(500);
}

void ruch_v1(){
  //ruch roboczy po lini prostej
      serwo2.write(71);
      serwo3.write(65);
      serwo1.write(125);
      serwo5.write(71);
      serwo6.write(65);
      serwo4.write(55);
      delay(100);
      serwo2.write(72);
      serwo3.write(70);
      serwo1.write(120);
      serwo5.write(72);
      serwo6.write(70);
      serwo4.write(60);
      delay(100);
      serwo2.write(73);
      serwo3.write(79);
      serwo1.write(115);
      serwo5.write(73);
      serwo6.write(79);
      serwo4.write(65);
      delay(100);
      serwo2.write(75);
      serwo3.write(81);
      serwo1.write(110);
      serwo5.write(75);
      serwo6.write(81);
      serwo4.write(70);
      delay(100);
      serwo2.write(74);
      serwo3.write(85);
      serwo1.write(105);
      serwo5.write(74);
      serwo6.write(85);
      serwo4.write(75);
      delay(100);
      serwo2.write(72);
      serwo3.write(86);
      serwo1.write(100);
      serwo5.write(72);
      serwo6.write(86);
      serwo4.write(80);
      delay(100);
      serwo2.write(74);
      serwo3.write(89);
      serwo1.write(95);
      serwo5.write(74);
      serwo6.write(89);
      serwo4.write(85);
      delay(100);
      serwo2.write(71);
      serwo3.write(87);
      serwo1.write(90);
      serwo5.write(71);
      serwo6.write(87);
      serwo4.write(90);
      delay(100);
      serwo2.write(73);
      serwo3.write(88);
      serwo1.write(85);
      serwo5.write(73);
      serwo6.write(88);
      serwo4.write(95);
      delay(100);
      serwo2.write(73);
      serwo3.write(90);
      serwo1.write(80);
      serwo5.write(73);
      serwo6.write(90);
      serwo4.write(100);
      delay(100);
      serwo2.write(67);
      serwo3.write(91);
      serwo1.write(75);
      serwo5.write(67);
      serwo6.write(91);
      serwo4.write(105);
      delay(100);
      serwo2.write(69);
      serwo3.write(91);
      serwo1.write(70);
      serwo5.write(69);
      serwo6.write(91);
      serwo4.write(110);
      delay(100);
      serwo2.write(65);
      serwo3.write(91);
      serwo1.write(65);
      serwo5.write(65);
      serwo6.write(91);
      serwo4.write(115);
      delay(100);
      serwo2.write(68);
      serwo3.write(90);
      serwo1.write(60);
      serwo5.write(68);
      serwo6.write(90);
      serwo4.write(120);
      delay(100);
      serwo2.write(68);
      serwo3.write(92);
      serwo1.write(55);
      serwo5.write(68);
      serwo6.write(92);
      serwo4.write(125);
      delay(100);
      
    //podnoszenie
      for(int i=68, k=92; i<=110; i+=1, k+=2){
        serwo2.write(i);
        serwo5.write(i);
        if(k<=110){
        serwo3.write(k);
        serwo6.write(k);
        } 
        delay(10);
      }
      
    //przejazd serwo-korpus
      for(int i = 55; i<=125 ; i+=5){
      serwo1.write(i);
      serwo4.write(180-i);
      delay(50);
       }
      //opadniecie
    for(int i = 110; i>=65; i--){
       serwo3.write(i);
       serwo6.write(i);
       if(i>=71){
       serwo2.write(i);
       serwo5.write(i);
       }
      delay(10);
      }
}

void loop() { 
  ruch_v1();
}