#include <Servo.h>


#define numServos 2
#define demoPoses 4
// Servo[0] -> dreta; servo[1] -> esquerra
Servo servos[2];
const int sVelocitat = 2;
const int servoPins[numServos] = {5,6};
int servoPos[numServos];
int servoPosInicial[numServos] ={100, 10};
int servoMIN[numServos] = {20, 50};
int servoMAX[numServos] = {160, 150};
int servoDEMO[demoPoses][numServos] = {


      {90, 90},
      {170, 10},
      {10, 170},
      {90, 90}};


// joy[14] -> vertical; joy[15] -> horitzontal
#define numJoys 2
const int joyPins[numJoys] = {14,15};
int joyPOS[numJoys];
const int joyBoto = 2;




void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(joyBoto, INPUT_PULLUP);
  // configuració dels servos
  for(int n=0; n<numServos; n++){
    servos[n].attach(servoPins[n]);
  }
  // posició inicial dels servos
  for(int n=0; n<numServos; n++){
    servos[n].write(80);
  }


}


void loop() {
  // put your main code here, to run repeatedly:
  if(!digitalRead(joyBoto)){
    // posició inicial dels servos
    for(int n=0; n<numServos; n++){
      servos[n].write(servoPosInicial[n]);
    }
    delay(1000);
    for(int m=0; m< demoPoses; m++){
      for(int n=0; n<numServos; n++){
        servos[n].write(servoDEMO[m][n]);
      }
      delay(1000);
    }
  }
  for(int n= 0; n<numJoys; n++){
    joyPOS[n] = analogRead(joyPins[n]);
    int graus = map(joyPOS[n], 0, 1023, 180, 0);
    if(joyPOS[n] > 600){
      //fer moure el servo
      servoPOS[n] += sVelocitat;
     
      servos[n].write(servosPOS[n]);
    }
    else if(joyPOS[n] < 400){
      //fer moure el servo
      servoPOS[n] -= sVelocitat;
    }
    servos[n.write(servoPOS[n])]
  }
}