#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;
}