#include <Wire.h>

#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);

#define SERVOMIN  125 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  575 // this is the 'maximum' pulse length count (out of 4096)


int servoNumber = 0;
int estadogeneral = 0; //0: stop  1:start 2:cambio

#define boton 12

int setpointpiston=0;
int estadopiston=0;//izq:0  der:1

int estadoeje1=0;//1: en medio  2: listo
int estadoeje2=0;
int estadoeje3=0;

void setup() {
  pinMode(boton, INPUT_PULLUP);

  Serial.begin(9600);

  board1.begin();
  board1.setPWMFreq(60);
}

void loop() {
  estadogeneral = digitalRead(boton);

do {
  board1.setPWM(0, 0, angleToPulse(90) );
  board1.setPWM(1, 0, angleToPulse(0)  );
  board1.setPWM(2, 0, angleToPulse(90) );
  board1.setPWM(3, 0, angleToPulse(0)  );
  board1.setPWM(4, 0, angleToPulse(90) );
  board1.setPWM(5, 0, angleToPulse(0)  );
  board1.setPWM(6, 0, angleToPulse(0)  );
  board1.setPWM(7, 0, angleToPulse(180));
  board1.setPWM(8, 0, angleToPulse(0)  );
  
  delay(600);

  setpointpiston=180;

  do{
    estadoeje1=2;
    estadoeje2=2;
    estadoeje3=1;

    if (estadogeneral==2){
      while(estadoeje1=2 && estadoeje2=2 && estadoeje3=1 && estadopiston=1){
        board1.setPWM(0, 0, angleToPulse(90) );
        board1.setPWM(1, 0, angleToPulse(90)  );
        board1.setPWM(3, 0, angleToPulse(90) );
        board1.setPWM(4, 0, angleToPulse(90)  );
        board1.setPWM(5, 0, angleToPulse(90) );
        delay(300);

        setpointmotor1=valor1;
        setpointmotor3=valor3;

        while(estadomotor1==1){
          board1.setPWM(0, 0, angleToPulse(90) );
          delay(300);
          setpointmotor2=valor2;
          while(estadomotor2==1 && estadomotor3==1){
            estadogeneral=1;
          }
        }
      }
    }
    else{
      if(boton1==1){
        BT1=1;
      }
      else
    }

  } while(estadopiston=0);

} while (estadoGeneral == 0);
  
  delay(100);
 
}

/*
 * angleToPulse(int ang)
 * gets angle in degree and returns the pulse width
 * also prints the value on seial monitor
 */
int angleToPulse(int ang){
   int pulse = map(ang,0, 180, SERVOMIN,SERVOMAX);// map angle of 0 to 180 to Servo min and Servo max 
   return pulse;
}