// test a stepper motor with a Pololu A4988 driver board, onboard led will flash at each step
// this version uses delay() to manage timing
#define ECHO_PIN 3
#define TRIG_PIN 4
byte SW1 = A0;
byte directionPin = 8;
byte stepPin = 9;
int numberOfSteps = 200;
byte ledPin = 13;
int pulseWidthMicros = 20;  // microsecondo
int millisbetweenSteps = 10; // milliseconds - or try 100 for slower steps
int statostart;
float distance;
int countDown = 20;

void setup() {
  Serial.begin(9600);
  Serial.println("Starting StepperTest");
  digitalWrite(ledPin, LOW);
  pinMode(directionPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(SW1, INPUT_PULLUP);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

unsigned long tNow;
unsigned long elapsedTime;
void loop() {
  if (digitalRead(SW1) == LOW) testStepper();

  elapsedTime = millis() - tNow;
  if (elapsedTime >= 500) {
    tNow = millis();
    distance = readDistanceCM();
    Serial.print("Measured distance: ");
    Serial.println(readDistanceCM());

    bool isNearby = distance < 10;
    if(isNearby){
      countDown--;
      Serial.println(countDown);
      if(countDown == 0){
        testStepper();
        countDown = 20;
      }
    }
    else{
      countDown = 20;
    }

    digitalWrite(LED_BUILTIN, isNearby);
  }
}

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void testStepper()
{
  digitalWrite(directionPin, HIGH);
  for (int n = 0; n < numberOfSteps; n++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(pulseWidthMicros);
    digitalWrite(stepPin, LOW);
    delay(millisbetweenSteps);
    digitalWrite(ledPin, !digitalRead(ledPin));
  }
  delay(1000);
  digitalWrite(directionPin, LOW);
  for (int n = 0; n < numberOfSteps; n++) {
    digitalWrite(stepPin, HIGH);
    // delayMicroseconds(pulseWidthMicros); // probably not needed
    digitalWrite(stepPin, LOW);
    delay(millisbetweenSteps);
    digitalWrite(ledPin, !digitalRead(ledPin));
  }
}