#include <Servo.h>

Servo Garra; 

int bt_sobe      = A0;
int bt_desce     = A1;
int bt_frente    = A2; 
int bt_tras      = A3;
int bt_esquerda  = A4;
int bt_direita   = A5;
int bt_garra     = 2;
int zero_M1      = 6;
int zero_M2      = 5;
int zero_M3      = 4;




int stp1 = 13;
int dir1 = 12;
int stp2 = 11;
int dir2 = 10;
int stp3 = 9; 
int dir3 = 8;

int encoder_M1 = 0;
int encoder_M2 = 0;
int encoder_M3 = 0;


void setup() 
{

Garra.attach(7);

pinMode(bt_sobe,     INPUT_PULLUP);
pinMode(bt_desce,    INPUT_PULLUP);
pinMode(bt_frente,   INPUT_PULLUP);
pinMode(bt_tras,     INPUT_PULLUP);
pinMode(bt_esquerda, INPUT_PULLUP);
pinMode(bt_direita,  INPUT_PULLUP);
pinMode(bt_garra,    INPUT_PULLUP);
//pinMode(bt_zero,     INPUT_PULLUP);
pinMode(zero_M1,     INPUT_PULLUP);
pinMode(zero_M2,     INPUT_PULLUP);
pinMode(zero_M3,     INPUT_PULLUP);



pinMode(stp1, OUTPUT);
pinMode(dir1, OUTPUT);
pinMode(stp2, OUTPUT);
pinMode(dir2, OUTPUT);
pinMode(stp3, OUTPUT);
pinMode(dir3, OUTPUT);

Garra.write(0);

//ZERO M1
while(digitalRead(zero_M1) == HIGH)
  {
    digitalWrite(dir1, LOW);
    digitalWrite(stp1, HIGH);
    delay(50);
    digitalWrite(stp1, LOW);
    delay(50);
  }

//ZERO M2
while(digitalRead(zero_M2) == HIGH)
  {
    digitalWrite(dir2, LOW);
    digitalWrite(stp2, HIGH);
    delay(50);
    digitalWrite(stp2, LOW);
    delay(50);
  }

//ZERO M3
while(digitalRead(zero_M3) == HIGH)
  {
    digitalWrite(dir3, LOW);
    digitalWrite(stp3, HIGH);
    delay(50);
    digitalWrite(stp3, LOW);
    delay(50);
  }

//RETORNA M1 PARA O CENTRO 
for( int i = 0; i<= 100; i++)
{
  digitalWrite(dir1, HIGH);
  digitalWrite(stp1, HIGH);
  delay(10);
  digitalWrite(stp1, LOW);
  delay(10);
}

//RETORNA M2 PARA O CENTRO 
for( int i = 0; i<= 120; i++)
{
  digitalWrite(dir2, HIGH);
  digitalWrite(stp2, HIGH);
  delay(10);
  digitalWrite(stp2, LOW);
  delay(10);
}

//RETORNA M3 PARA O CENTRO 
for( int i = 0; i<= 270; i++)
{
  digitalWrite(dir3, HIGH);
  digitalWrite(stp3, HIGH);
  delay(10);
  digitalWrite(stp3, LOW);
  delay(10);
}


}

void loop() 
{
//motor 1 - SOBE/DESCE
if(digitalRead(bt_sobe) == LOW && digitalRead(bt_desce) == HIGH && encoder_M1 <100)
  {
    digitalWrite(dir1, HIGH);
    digitalWrite(stp1, HIGH);
    encoder_M1 = encoder_M1 + 1;
    delay(10);
    digitalWrite(stp1, LOW);
    delay(10);
  }

if(digitalRead(bt_sobe) == HIGH && digitalRead(bt_desce) == LOW && encoder_M1 > -100)
  {
    digitalWrite(dir1, LOW);
    digitalWrite(stp1, HIGH);
    encoder_M1 = encoder_M1 - 1;
    delay(10);
    digitalWrite(stp1, LOW);
    delay(10);
  }

  //motor 2 - FRENTE/TRAS
if(digitalRead(bt_frente) == LOW && digitalRead(bt_tras) == HIGH && encoder_M2 <120)
  {
    digitalWrite(dir2, HIGH);
    digitalWrite(stp2, HIGH);
    encoder_M2 = encoder_M2 + 1;
    delay(10);
    digitalWrite(stp2, LOW);
    delay(10);
  }

if(digitalRead(bt_frente) == HIGH && digitalRead(bt_tras) == LOW && encoder_M2 > -120)
  {
    digitalWrite(dir2, LOW);
    digitalWrite(stp2, HIGH);
    encoder_M2 = encoder_M2 - 1;
    delay(10);
    digitalWrite(stp2, LOW);
    delay(10);
  }

  //motor 3 - ESQUEDA/DIREITA
if(digitalRead(bt_esquerda) == LOW && digitalRead(bt_direita) == HIGH && encoder_M3 <270)
  {
    digitalWrite(dir3, HIGH);
    digitalWrite(stp3, HIGH);
    encoder_M3 = encoder_M3 + 1; 
    delay(10);
    digitalWrite(stp3, LOW);
    delay(10);
  }

if(digitalRead(bt_esquerda) == HIGH && digitalRead(bt_direita) == LOW && encoder_M3 > -270)
  {
    digitalWrite(dir3, LOW);
    digitalWrite(stp3, HIGH);
    encoder_M3 = encoder_M3 - 1; 
    delay(10);
    digitalWrite(stp3, LOW);
    delay(10);
  }

if(digitalRead(bt_garra) == LOW)
  {
    Garra.write(180);
  }
else
  {
    Garra.write(0);
  }
 
}