#include "AccelStepper.h"
#define dir 4
#define step 5
#define EPin 6

const byte LeftL = 2;
const byte RightL = 3;
const int ChangeDir = 7;
int n = 0;
int Direction;
int Speed = 30;

bool Signo = 0;
bool NSigno = 0;

AccelStepper stepperC = AccelStepper(AccelStepper::DRIVER, step, dir);


void setup() {
  stepperC.setEnablePin(EPin);
  stepperC.setMinPulseWidth(3);
  Serial.begin(9600);
  pinMode(LeftL, INPUT);
  pinMode(RightL, INPUT);
  pinMode(ChangeDir, INPUT_PULLUP);
  //attachInterrupt(digitalPinToInterrupt(LeftL), LL, LOW);//Límite Izquierdo
  //attachInterrupt(digitalPinToInterrupt(RightL), RL, LOW);//Límite Drecho


  stepperC.setMaxSpeed(200);
}

void loop() {
  
  Serial.println(n);
  stepperC.enableOutputs();
  stepperC.setSpeed(Speed);
  // Step the motor with a constant speed as set by setSpeed():
  stepperC.runSpeed();
  stepperC.stop();
  Direction = digitalRead(ChangeDir);
  if(Direction==LOW){
    stepperC.stop();
    delayMicroseconds(10);
    Speed = -1*Speed;
    stepperC.setSpeed(Speed);
    delayMicroseconds(40);
    stepperC.runSpeed();
  }
  
  int D_cm=distancia(1); //lectura de distancia con numero de muestreo para el promedio
  Serial.println(D_cm);

  if(D_cm>9){
    NSigno = 1;
  }
  if(D_cm<9){
    NSigno = 0;
  }

  if (NSigno != Signo) {
    stepperC.stop();
    delayMicroseconds(10);
    Speed = -1*Speed;
    stepperC.setSpeed(Speed);
    delayMicroseconds(40);
    stepperC.runSpeed();
    Serial.println("Cambio de signo");
    Serial.println(NSigno); 
    Signo = NSigno;
  }
  n++;
}

float distancia(int n)
{
  long suma=0;
  for(int i=0;i<n;i++)
  {
    suma=suma+analogRead(A0);
    delay(53);
  }  
  float adc=suma/n;
  float distancia_cm = 17569.7 * pow(adc, -1.2062);
  return(distancia_cm);
}

//void LL() {
//  Serial.println("LL!!");
//}

//void RL() {
// Serial.println("RL!!");
//}