Arduino animatroniczne oczy – serwa powiek nie mrugają, zacina się mruganie (kod)

#include <Servo.h>
//#include
////////////////// tworzenie obiektuw serw
Servo x;
Servo y;
Servo pg;  //gurna prawa powieka
Servo pd;  //dolna prawa powieka
Servo lg;  //gurna lewa powieka
Servo ld;  // dolna lewa powieka
////////////////// tworzenie obiektuw serw
int wyzwalacz = 1;
//////////////czekanie
unsigned long poprzedniCzas2 = 0;
unsigned long poprzedniCzas = 0;
const long interwal2 = 5000;
const long interwal = 5000; // czas czekania w ms. czekanie do mrugania
////////////////
void setup() {
  // put your setup code here, to run once:
  //pszypis pinuw
  x.attach(3);
  y.attach(5);
  pg.attach(6);
  pd.attach(9);
  lg.attach(10);
  ld.attach(11);
  x.write(100);
  /////////////
}  //koniec void setup
//// zakresy ruchu               x    y   pg  pd  lg  ld
//int startAngle[SERVO_COUNT] = {45, 50, 100, 40, 90, 90};
//int endAngle[SERVO_COUNT]   = {145, 105, 180, 100, 140, 160};
void loop() {
  pozycja(45, 50);
  delay(500);
  pozycja(145, 50);
  delay(500);
  pozycja(45, 50);
  delay(500);
  pozycja(145, 50);
  delay(500);
  pozycja(95, 50);
  delay(100);
  pozycja(95, 110);
  mrugnij();
  delay(2000);
}
int pozycja(int pozycjax, int pozycjay) {
  if (wyzwalacz == 1) {
    if (pozycjay < 76) {  //////////////if do gurnych powiek
      pg.write(160);
      lg.write(50);
    }  //koniec if
    else {
      pg.write(100);
      lg.write(130);
    }  //koniec else
    if (pozycjay > 84) { ///////////////id do dolnych powiek
      pd.write(40);
      ld.write(140);
    } else {
      pd.write(100);
      ld.write(90);
    }///koniec else
  }//koniec wyzwalacza
  x.write(pozycjax);
  y.write(pozycjay);
}  //koniec funkcj
int mrugnij() {
  wyzwalacz = 0;
  unsigned long aktualnyCzas = millis();
  if (aktualnyCzas - poprzedniCzas >= interwal) {   //czekanie do mrugania
    poprzedniCzas = aktualnyCzas;
    pg.write(180);
    lg.write(90);
    pd.write(160);
    ld.write(180);
  }
  if (aktualnyCzas -  poprzedniCzas2 >= interwal2) {
    poprzedniCzas2 = aktualnyCzas;
    //zakresy ruchu               x    y   pg  pd  lg  ld
    //int startAngle[SERVO_COUNT] = {45, 50, 100, 40, 90, 90};
    //int endAngle[SERVO_COUNT]   = {145, 105, 180, 100, 140, 160};
    pg.write(100);
    lg.write(140);
    pd.write(40);
    ld.write(90);
  }
  wyzwalacz = 1;
}

#include <Servo.h>
//#include
////////////////// tworzenie obiektuw serw
Servo x;
Servo y;
Servo pg;  //gurna prawa powieka
Servo pd;  //dolna prawa powieka
Servo lg;  //gurna lewa powieka
Servo ld;  // dolna lewa powieka
////////////////// tworzenie obiektuw serw
int wyzwalacz = 1;
//////////////czekanie
int motor = 2;       //////motor do wyrzucania cuksuw
////////////
int ps1 =  13;  //lewy
int ps2 =  12;  //środkowy      przyciski
int ps3 =  8;   //prawy
//////////
unsigned long poprzedniCzas2 = 0;
unsigned long poprzedniCzas = 0;
const long interwal2 = 5000;
const long interwal = 5000; // czas czekania w ms. czekanie do mrugania
////////////////
void setup() {
  // put your setup code here, to run once:
  //pszypis pinuw
  x.attach(3);
  y.attach(5);
  pg.attach(6);
  pd.attach(9);
  lg.attach(10);
  ld.attach(11);
  x.write(100);
  /////////////
  pinMode(motor,OUTPUT);
///////////
pinMode(ps1, INPUT_PULLUP);
pinMode(ps2, INPUT_PULLUP);
pinMode(ps3, INPUT_PULLUP);
////////////

}  //koniec void setup
//// zakresy ruchu               x    y   pg  pd  lg  ld
//int startAngle[SERVO_COUNT] = {45, 50, 100, 40, 90, 90};
//int endAngle[SERVO_COUNT]   = {145, 105, 180, 100, 140, 160};
void loop() {
  mrugnij();
  wsyp();
  pozycja(45, 15);
  delay(500);
  pozycja(145, 15);
  delay(500);
  pozycja(45, 15);
  delay(500);
  pozycja(145, 15);
  delay(500);
  pozycja(45, 15);
  delay(100);

  pozycja(95, 100);
  
  delay(2000);
}
int pozycja(int pozycjax, int pozycjay) {
  if (wyzwalacz == 1) {
    if (pozycjay < 76) {  //////////////if do gurnych powiek
      pg.write(160);
      lg.write(50);
    }  //koniec if
    else {
      pg.write(100);
      lg.write(130);
    }  //koniec else
    if (pozycjay > 84) { ///////////////id do dolnych powiek
      pd.write(40);
      ld.write(140);
    } else {
      pd.write(100);
      ld.write(90);
    }///koniec else
  }//koniec wyzwalacza
  x.write(pozycjax);
  y.write(pozycjay);
}  //koniec funkcj
int mrugnij() {
  wyzwalacz = 0;
  unsigned long aktualnyCzas = millis();
  if (aktualnyCzas - poprzedniCzas >= interwal) {   //czekanie do mrugania
    poprzedniCzas = aktualnyCzas;
    pg.write(180);
    lg.write(90);
    pd.write(160);
    ld.write(180);
  }
  if (aktualnyCzas -  poprzedniCzas2 >= interwal2) {
    poprzedniCzas2 = aktualnyCzas;
    //zakresy ruchu               x    y   pg  pd  lg  ld
    //int startAngle[SERVO_COUNT] = {45, 50, 100, 40, 90, 90};
    //int endAngle[SERVO_COUNT]   = {145, 105, 180, 100, 140, 160};
    pg.write(100);
    lg.write(140);
    pd.write(40);
    ld.write(90);
  }
  wyzwalacz = 1;
}
void wsyp() {

  if (digitalRead(ps1) == LOW) {

    digitalWrite(motor, HIGH);

  }
  else {

    digitalWrite(motor, LOW);

  }

}