#include <LiquidCrystal.h>
const byte rs = 0;             //pin for LCD
const byte en = 1;             //pin for LCD
const byte d4 = 3;             //pin for LCD
const byte d5 = 4;             //pin for LCD
const byte d6 = 5;             //pin for LCD
const byte d7 = 6;             //pin for LCD
const byte dirPin = 9;         //5v output pin for motor driver direction
const byte stepPin = 10;       //5v output pin for motor driver pulse
const byte switchPin = 2;      //input pullup pin for homing limit switch
const int strokeSteps = 2444;  //number of steps for full stroke, 1600 steps=72mm
int speedPot;                  //integer for potentiometer read value 0 to 1023, analog read for speed pot
int delPot;                    //integer for potentiometer read value 0 to 1023, anaolog read for stroke delay pot
int stepDel = 90;              //delay time between each motor step during operation, lower value=faster steps
unsigned long strokeDel;       //delay time between actuator strokes
const int homeDel = 1000;      //delay time between each motor step during homing sequence
const int homingSteps = 400;   //steps taken in direction away from limit switch after switch has been activated
const byte maxSpeed = 90;      //max speed expressed as the microseconds per pulse divided by 2
byte speedPerc;                //percent maximum speed of stepper motor, varies based on pot, used to define stepDel
int oldStepDel = 90;           //the value of stepDel as it is displayed on LCD
int oldStrokeDel = 100;        //the value of strokeDel as it is displayed on LCD
unsigned long endPush = 0;     //timer set at the of push stroke
unsigned long endPull = 1;     //timer set at end of pull stroke

LiquidCrystal lcd(rs, en, d4, d5, d6, d7);  //setup lcd object using variable defined pins


void setup() {
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(switchPin, INPUT_PULLUP);

  lcd.begin(16, 2);  //start lcd with 16 columns and 2 rows

  //indicate start of homing sequence on LCD
  lcd.setCursor(0, 0);
  lcd.print("Homing...");


  //set direction towards limit switch for homing
  digitalWrite(dirPin, LOW);

  //while limit switch is not pressed (HIGH state due to input_pullup), step motor towards the limit switch for homing
  while (digitalRead(switchPin)) {
    digitalWrite(stepPin, HIGH);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(homeDel);
  }

  //pause after limit switch is hit
  delay(500);

  lcd.setCursor(0, 0);
  lcd.print("Centering...");

  //set direction away from limit switch
  digitalWrite(dirPin, HIGH);

  //step motor away from limit switch, distance set by homingSteps
  for (int x = 0; x < homingSteps; x++) {
    digitalWrite(stepPin, HIGH);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(homeDel);
  }

  //pause before starting operation
  delay(500);

  attachInterrupt(digitalPinToInterrupt(switchPin), crash, FALLING);  //creates interrupt for limit switch, stops motion in case of collision with limit switch
}

void loop() {

  speedPot = analogRead(A0);                        //read value from potentiometer for speed
  delPot = analogRead(A1);                          //read value from potentiometer for stroke delay
  speedPerc = (5 * map(speedPot, 0, 1023, 2, 20));  //map pot value to a percentage between 30% and 100% rounded to nearest 5%
  stepDel = (100 * maxSpeed / (speedPerc));         //sets step delay time based on speedPerc, 100%=125 microsecond delay. 50%=250 microsecond delay
  strokeDel = (50 * map(delPot, 0, 1023, 1, 20));   //sets stroke delay between 50ms and 1000ms to nearest 50

  //if strokeDel or stepDel values have changed, refresh LCD
  if (strokeDel != oldStrokeDel || stepDel != oldStepDel) {
    //writes speed and stroke delay time to lcd
    lcd.clear();
    lcd.setCursor(0, 0);        //set lcd cursor start of first line
    lcd.print("Speed (%):  ");  //print "Speed %:" on lcd
    lcd.print(speedPerc);       //print speedPerc value after "Speed %:"
    lcd.setCursor(0, 1);        //set lcd cursor start of second line
    lcd.print("Delay (ms): ");
    lcd.print(strokeDel);      //print stroke delay time in ms
    oldStrokeDel = strokeDel;  //update old values
    oldStepDel = stepDel;      //update old values
  }

  //if there has been more time than strokeDel after then end of the pull, and the motor pulled more recently than it pushed, then push the platform away from limit switch
  //value for endPull **MUST** start off >endPush in order to trigger a push before pull
  if (millis() - endPull >= strokeDel && endPull > endPush) {

    //set direction away from limit switch for push
    digitalWrite(dirPin, HIGH);

    //accelerate motor for 2% of full stroke length
    for (int x = 0; x < (0.02 * strokeSteps); x++) {
      digitalWrite(stepPin, HIGH);
      digitalWrite(stepPin, LOW);
      delayMicroseconds((7 * ((0.01 * strokeSteps) - (x / 2))) + stepDel);
    }
    //perform 96% of full stroke at full speed
    for (int x = 0; x < (0.96 * strokeSteps); x++) {
      digitalWrite(stepPin, HIGH);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(stepDel);
    }
    //decelerate for 2% of stroke length
    for (int x = (0.2 * strokeSteps); x > 0; x--) {
      digitalWrite(stepPin, HIGH);
      digitalWrite(stepPin, LOW);
      delayMicroseconds((7 * ((0.01 * strokeSteps) - (x / 2))) + stepDel);
    }

    endPush = millis();  //set timer for end time of push stroke
  }

  //if there has been more time than strokeDel after then end of the push, and the motor pushed more recently than it pulled, then pull the platform away from limit switch
  if (millis() - endPush >= strokeDel && endPush > endPull) {

    //set direction towards limit switch for pull
    digitalWrite(dirPin, LOW);

    //accelerate motor for 2% of full stroke length
    for (int x = 1; x <= (0.02 * strokeSteps); x++) {
      digitalWrite(stepPin, HIGH);
      digitalWrite(stepPin, LOW);
      delayMicroseconds((7 * ((0.01 * strokeSteps) - (x / 2))) + stepDel);  //delay starts at roughly 164 + stepDel, as x increments towards 2% of strokeSteps then the delay approaches stepDel
    }
    //perform 96% of full stroke at full speed
    for (int x = 0; x < (0.96 * strokeSteps); x++) {
      digitalWrite(stepPin, HIGH);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(stepDel);
    }
    //decelerate for 2% of stroke length
    for (int x = (0.2 * strokeSteps); x >= 1; x--) {
      digitalWrite(stepPin, HIGH);
      digitalWrite(stepPin, LOW);
      delayMicroseconds((7 * ((0.01 * strokeSteps) - (x / 2))) + stepDel);  //delay starts at stepDel, as x decrements from 2% of strokeSteps towards 1 then the delay approaches roughly 164 + stepDel

      endPull = millis();  //set timer for end time of pull stroke
    }
  }


  void crash() {

    //while limit switch is pressed, freeze program here
    while (digitalRead(switchPin) == LOW) {
    }

    lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("System Crash");
      lcd.setCursor(0, 1);
      lcd.print("Please Restart");
      delay(500);

  }