#include <AccelStepper.h>

#define TRIGGER 3
#define ECHO    2
#define SENSOR  4
#define LED_VM  5 
#define LED_VD  7   

int       distancia;
const int stepsPerRevolution = 200;

AccelStepper myStepper(stepsPerRevolution, 1, 8, 9);

void setup() {
  pinMode(ECHO, INPUT);
  pinMode(TRIGGER, OUTPUT);
  pinMode(SENSOR, INPUT_PULLUP);

  myStepper.MaxSpeed(100);
  myStepper.setAcceleration(50);   
  myStepper.setSpeed(200);

  Serial.begin(9600);
}

void loop() {
  /*Medição ultrassônica
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);
  
 // distancia = 0;
 distancia = pulseIn(ECHO, HIGH) / 58;
  Serial.print("Distância em CM: ");
  Serial.println(distancia);*/

  if (digitalRead(SENSOR) == LOW)
  {
    digitalWrite(LED_VD, HIGH);

    Serial.println("ENTRADA DE PEÇA");
    
    myStepper.moveTo(stepsPerRevolution);

    myStepper.run();
    }

  // Inverte o lado de rotação
  else if ((distancia <=2) || (distancia >= 200)) 
  {
    digitalWrite(LED_VM, HIGH);
    digitalWrite(LED_VD, LOW);

    myStepper.moveTo(-stepsPerRevolution);

    Serial.println("Distância máxima atingida! Invertendo sentido de rotação.");

    myStepper.run();
    } 
}