include <VarSpeedServo.

h>

VarSpeedServo Servobase;

VarSpeedServo Servogarra;

VarSpeedServo Servofrente;

VarSpeedServo Servodesce;

int Sensor_Pos1 = 7;

int Sensor_Pos2 = 8;

int start = 2;

int estado = 1;

int objeto = 0;

void setup()

Servobase.attach(3);

Servogarra.attach(11);

Servofrente.attach(6);

Servodesce.attach(5);

Servobase.write(90);

Servogarra.write(90);

Servodesce.write(0);

Servofrente.write(0);

pinMode(Sensor_Pos1, INPUT); //sensor

pinMode(Sensor_Pos2, INPUT); //sensor

pinMode(start,INPUT_PULLUP);
}

void loop()

if (estado == 1)

if (!digitalRead(start))

estado = 2;

else if (estado == 2)

if (!digitalRead(Sensor_Pos1))

estado = 3;

else if (estado == 3)

Servogarra.write(80, 50, true);

Servobase.write(53, 20, true);

//Servobase.write(48, 20, true);

Servodesce.write(0, 10, true);

Servofrente.write(30, 20,true);

Servodesce.write(55, 10, true);

Servogarra.write(80, 50, true); //fecha

estado = 4;

else if (estado == 4)

if (digitalRead(Sensor_Pos2))

estado = 5;

}
 else if (estado == 5)

Servofrente.write(30, 20,true);

Servogarra.write(120, 50, true);

Servodesce.write(1, 10, true);

Servobase.write(113, 20, true);

Servofrente.write(26, 20,true);

Servodesce.write(60, 10, true);

Servodesce.write(55, 10, true);

Servogarra.write(80, 40, true); //abre

Servofrente.write(0, 20,true);

Servodesce.write(0, 10, true);

estado = 6;

else if (estado == 6)

if (!digitalRead(Sensor_Pos2))

estado = 1;