// Copyright 2013 Dany Qumsiyeh
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "AccelStepper.h"

// pin 11 - GND
// pin 12 - STEP
// pin 13 - DIR

int motorStepPin = 12;
int motorDirPin = 13;

float SCAN_SPEED = 380;
float MID_SPEED = 2000;
float FULL_SPEED = 4000;
float ACCELERATION = 4000;
float DECELERATION = 20000;

AccelStepper stepper(1, motorStepPin, motorDirPin); 

void setup(){
  // GND pin for motor driver
  pinMode(11, OUTPUT);
  digitalWrite(11, LOW);

  Serial.begin(9600);

  // wait 5s for user to start scanning
  delay(5000);
  Serial.println("");

  // tap motor while waiting for calibration
  stepper.setMaxSpeed(5000);
  stepper.setAcceleration(5000);
  for (int i=0; i<15; i++) {
    stepper.runToNewPosition(2);
    stepper.runToNewPosition(0);
    delay(1000);
  }

  // start scan
  Serial.println("");
  delay(500);
}

void loop(){
  // forward
  stepper.setMaxSpeed(SCAN_SPEED);
  stepper.setAcceleration(ACCELERATION);
  stepper.moveTo(12100);
  int stage = 0;
  while (stepper.distanceToGo()) {
    if (stage == 0 && stepper.currentPosition() > 3000) {
      // accelerate after scan sensors
      stepper.setMaxSpeed(MID_SPEED);
      stepper.setAcceleration(ACCELERATION);
      stage = 1;
    } else if (stage == 1 && stepper.currentPosition() > 5500) {
      stepper.setMaxSpeed(FULL_SPEED);
      stage = 2;
    } else if (stage == 2 && stepper.currentPosition() > 8000) {
      stepper.setAcceleration(DECELERATION);
      stage = 3;
    }
    stepper.run();
  }

  // backward
  stepper.setMaxSpeed(FULL_SPEED);
  stepper.setAcceleration(ACCELERATION);
  stepper.moveTo(0);
  stage = 0;
  while (stepper.distanceToGo()) {
    if (stage == 0 && stepper.currentPosition() < 3000) {
      stepper.setAcceleration(DECELERATION);
      stage = 1;
    } else if (stage == 1 && stepper.currentPosition() < 1000) {
      Serial.println("");
      stage = 2;
    }
    stepper.run();
  }

}