//https://arduinogetstarted.com/tutorials/arduino-joystick

#define SEL1 2
#define SEL2 4
#define MIDDLE 11
#define LEFT 10
#define RIGHT 9
#define CLAW 6

#include <GyverButton.h>
GButton butt1(SEL1);
GButton butt2(SEL2);

#include <Servo.h>  
Servo middle, left, right, claw ;  // creates 4 "servo objects"

bool up = true;


void setup() 
{ 
  Serial.begin(9600);
  //for(auto s: sel) pinMode(s, INPUT_PULLUP);
  middle.attach(11);  // attaches the servo on pin 11 to the middle object
  left.attach(10);    // attaches the servo on pin 10 to the left object
  right.attach(9);    // attaches the servo on pin 9 to the right object
  claw.attach(6);     // attaches the servo on pin 6 to the claw object

 
} 
uint8_t setrs(uint8_t rs ){
if(rs >180) rs = rs%180;
return rs;
}

 
void loop() 
{ 
  butt1.tick();
  //if (butt1.isClick()) Serial.println("Click 1"); 
  butt2.tick();
  //if (butt2.isClick()) Serial.println("Click 2"); 

  int x1Value = analogRead(A0);
  int y1Value = analogRead(A1);
  int x2Value = analogRead(A2);
  int y2Value = analogRead(A3);



  
  static uint8_t u = 0;
  uint8_t m = 180;
  uint8_t l = 120;
  uint8_t r = 60;
  uint8_t c = 0;

  middle.write(setrs(m+u)); // sets the servo position according to the value(degrees)
  left.write(setrs(l+u));   // does the same
  right.write(setrs(r+u));  // and again
  claw.write(setrs(c+u));   // yes you've guessed it
  delay(1);       // doesn't constantly update the servos which can fry them
  if(u == 180) up = false ;
  if (u == 1) up = true;
  if (up) {u++;}else{u--;}
  //Serial.print("In cicle u=");
 // Serial.println(u);
}