// Add library
#include <Keypad.h>
#include <LiquidCrystal.h>
#include <AccelStepper.h>
#include <EEPROM.h>
#include "HX711_ADC.h"
#include <SD.h>

// Define digital pins for motors
#define dirPin 19
#define stepPin 18
#define dirPin2 23
#define stepPin2 22
#define stepsPerRevolution 200
#define motorInterfaceType 1

// Define digital pins SD Card Module and Ultra Sonic Sensor
#define PIN_SPI_CS 53
#define ECHO_PIN_1 37
#define TRIG_PIN_1 35
#define ECHO_PIN_2 41
#define TRIG_PIN_2 39

// Define SD card 
File myFile;

// Define motors pins
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
AccelStepper stepper2 = AccelStepper(motorInterfaceType, stepPin2, dirPin2);

// Define Loadcell pins
HX711_ADC LoadCell_1(A0, A1);
HX711_ADC LoadCell_2(A2, A3);
unsigned long t = 0;      // Track time for valid loadcell data

// define LCD digital pins
LiquidCrystal lcd2(13, 12, 14, 15, 16, 17);
LiquidCrystal lcd(3, 2, 25, 27, 29, 31);

// define keypad digital pins
const byte ROWS = 4;       //four rows
const byte COLS = 4;       //four columns
char keys[ROWS][COLS] = {  //Defining keypad characters
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
byte rowPins[ROWS] = { 11, 10, 9, 8 };  //digital connect to the row pins of the keypad
byte colPins[COLS] = { 7, 6, 5, 4 };  //digital connect to the column pints of the keypad

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);  //mapping digital with keypad

// define common variables
String displacementValue = "";    // User defined max displacement
String speedValue = "";
double displacement;
double speedData;
int pressedA = 0;                 //Initialize key A state
int pressedB = 0;                 //Initialize key B state
int pressedA2 = 0;                 //Initialize key A state
int pressedB2 = 0;                 //Initialize key B state
int speedCheckA = 0;
int speedCheckB = 0;
int next = 0;                     //define motors operation
double steps = 0;                 //displacement translated as motor steps
int numCheck = 0;
double validMM = 100;             //Valid mm range
double validIN = 100;             //Valid in range
double validMMs = 100;             //Valid mm range
float validINs = 100;             //Valid in range
unsigned long int t1 = 0;         //time region
unsigned long int t2 = 0;
int i = 0;
int dir = 0;                      //motors direction
int mode2 = 0;                    //motors mode\
int num =4;

//holding variable
char key3;
char holdKey;
unsigned long t_hold;

// Initial function setup
void setup() {
  // Serial initial
  Serial.begin(9600);

  // Case SD not detected
  if (!SD.begin(PIN_SPI_CS)) {
    Serial.println(F("SD CARD FAILED, OR NOT PRESENT!"));
    while (1); // don't do anything more:
  }
  // SD detected
  Serial.println(F("SD CARD INITIALIZED."));
  SD.remove("arduino.txt"); // delete the file if existed

  // Create new file by opening file for writing
  myFile = SD.open("arduino.txt", FILE_WRITE);

  // Recording in new file
  if (myFile) {
    Serial.println("SD is recording");
  } else {
    Serial.print(F("SD Card: error on opening file arduino.txt"));
  }
  Serial.println("HX710B Demo with HX711 Library");
  Serial.println("Initializing the scale");

  // Loadcell initialize
  LoadCell_1.begin();
  LoadCell_2.begin();
  //LoadCell.setReverseOutput(); //uncomment to turn a negative output value to positive
  float calibrationValue_1; // calibration value 
  float calibrationValue_2;
  calibrationValue_1 = 0.42; 
  calibrationValue_2 = 0.42;

  unsigned long stabilizingtime = 2000; 
  boolean _tare = true; //perform tare
  byte loadcell_1_rdy = 0;
  byte loadcell_2_rdy = 0;
  while ((loadcell_1_rdy + loadcell_2_rdy) < 2) { //run startup, stabilization and tare, both modules simultaniously
    if (!loadcell_1_rdy) loadcell_1_rdy = LoadCell_1.startMultiple(stabilizingtime, _tare);
    if (!loadcell_2_rdy) loadcell_2_rdy = LoadCell_2.startMultiple(stabilizingtime, _tare);
  }
  if (LoadCell_1.getTareTimeoutFlag()) {
    Serial.println("Timeout, check MCU>HX711 no.1 wiring and pin designations");
  }
  if (LoadCell_2.getTareTimeoutFlag()) {
    Serial.println("Timeout, check MCU>HX711 no.2 wiring and pin designations");
  }
  LoadCell_1.setCalFactor(calibrationValue_1); // user set calibration value (float)
  LoadCell_2.setCalFactor(calibrationValue_2); // user set calibration value (float)
  Serial.println("Startup is complete");

  // Display start up commands via lcd
  lcd.begin(20, 4);
  lcd.setCursor(0, 0);
  lcd.print("  Press A to Start");
  lcd2.begin(20, 4);
  lcd2.setCursor(0, 0);
  lcd2.print("  Press A to Start");

  //digital pin for output
  pinMode(TRIG_PIN_1, OUTPUT);
  pinMode(ECHO_PIN_1, INPUT);
  pinMode(TRIG_PIN_2, OUTPUT);
  pinMode(ECHO_PIN_2, INPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  stepper.setMaxSpeed(1000);
  stepper2.setMaxSpeed(1000);
  digitalWrite(dirPin, HIGH);
  digitalWrite(dirPin2, HIGH);
}

// loop function for the system
void loop() {
  // 8mm per rev; 0.31496063 in per rev;
  // 200 steps per rev; 0.125 rev per mm; 1/0.31496063 rev per in;
  // 8 mm per 200 steps -> 0.04 mm per steps -> 25 steps per mm
  // 0.31496063 in per 200 steps -> 0.0015748 in per steps -> 635 steps per in
  // 1 mm per sec -> 25 steps per sec
  // 0.03937 in per sec -> 25 steps per sec

  // steps = 0.125*displacement*200
  // steps = 1/0.31496063*displacement*200

  // translate User defined max displacement to step
  int num =4;
  displacement = displacementValue.toDouble();
  if (pressedA == 1) {
    steps = 1 / 0.31496063 * displacement * 200;
  }
  if (pressedB == 1) {
    steps = 0.125 * displacement * 200;
  }
  // Translate User defined speed to motor speed
 double speed = speedValue.toDouble();
   if (speedCheckA == 1) {
    speedData = 25.4*speed * 32.67;
    speed = speedData;
  }
  else if (speedCheckB == 1){
    speedData = speed * 32.67;
    speed = speedData;
  } 

  myFile.println("-------------------------------------");

  // Enter mode 1 of the system
  if (next == 1) {
    // Clear LCD
    lcd.clear();
    lcd2.clear();
    // Speed of mode 2
    if (mode2 == 1) {
      speed = 0;                            // motor inital speed
    }
    lcd.setCursor(0, 0);
    if (mode2 == 0) {
      lcd.print("Mode1:Extend/Retract");
    }
    if (mode2 == 1) {
      lcd.print("Mode2:Manual Control");
    }
 
    // Data collection and LCD display loop
    while (next == 1)
    {
      // display data using imperial units
      if (pressedA == 1) {
        float distance_1 = readDistanceCM(TRIG_PIN_1, ECHO_PIN_1);
        float distance_2 = readDistanceCM(TRIG_PIN_2, ECHO_PIN_2);
        lcd.setCursor(0, 1);
        lcd.print("X = ");
        lcd.print(stepper.currentPosition() / (1 / 0.31496063) / 200, 4);
        lcd.print(" in");
        lcd.setCursor(0, 2);
        lcd.print("Y = ");
        lcd.print(stepper2.currentPosition() / (1 / 0.31496063) / 200, 4);
        lcd.print(" in");
        lcd2.setCursor(0, 2);
        lcd2.print("X = ");
        lcd2.print((-readDistanceCM(TRIG_PIN_1, ECHO_PIN_1)) * 0.39370079+4.3307,4);
        lcd2.setCursor(17, 2);
        lcd2.print(" in");
        lcd2.setCursor(0, 3);
        lcd2.print("Y = ");
        lcd2.print((-readDistanceCM(TRIG_PIN_2, ECHO_PIN_2)) * 0.39370079+4.3307,4);
        lcd2.setCursor(17, 3);
        lcd2.print(" in");
        // Uncomment below to display via serial monitor
        /*
          Serial.print("X = ");
          Serial.print(stepper.currentPosition() / (1 / 0.31496063) / 200, 4);
          Serial.print(" in  Y = ");
          Serial.print(stepper2.currentPosition() / (1 / 0.31496063) / 200, 4);
          Serial.print(" in  ");
          Serial.print("X = ");
          Serial.print((readDistanceCM(TRIG_PIN_1, ECHO_PIN_1))*0.39370079);
          Serial.print(" in  ");
          Serial.print("Y = ");
          Serial.print((readDistanceCM(TRIG_PIN_2, ECHO_PIN_2))*0.39370079);
          Serial.print(" in  ");
        */
        // Save data to sd card
        Loadcell();
        myFile.print("X = ");
        myFile.print(stepper.currentPosition() / (1 / 0.31496063) / 200, 4);
        myFile.print(" in  ");
        myFile.print("Y = ");
        myFile.print(stepper2.currentPosition() / (1 / 0.31496063) / 200, 4);
        myFile.print(" in");
        myFile.print("  X = ");
        myFile.print((-readDistanceCM(TRIG_PIN_1, ECHO_PIN_1)) * 0.39370079+4.3307,4);
        myFile.print(" in  ");
        myFile.print("Y = ");
        myFile.print((-readDistanceCM(TRIG_PIN_2, ECHO_PIN_2)) * 0.39370079+4.3307,4);
        myFile.print(" in  ");

        // Save load cell data to SD card at every time period
        float a = LoadCell_1.getData();
        float b = LoadCell_2.getData();
        String c = "g";
        if (pressedA == 1) {
          a = a * 0.00220462;
          b = b * 0.00220462;
          c = "lb";
        }
        // Reduce digits after decimal
        int num =4;
        if (a >= 10 | b >=10){
          num =3;
        }
        myFile.print("  X Load = ");
        myFile.print(a, num);
        myFile.print(" ");
        myFile.print(c);
        myFile.print("  Y Load = ");
        myFile.print(b, num);
        myFile.print(" ");
        myFile.print(c);
      }
      // display data using metric units
      if (pressedB == 1) {
        float distance_1 = readDistanceCM(TRIG_PIN_1, ECHO_PIN_1);
        float distance_2 = readDistanceCM(TRIG_PIN_2, ECHO_PIN_2);
        lcd.setCursor(0, 1);
        lcd.print("X = ");
        lcd.print(stepper.currentPosition() / 0.125 / 200, 4);
        lcd.print(" mm");
        lcd.setCursor(0, 2);
        lcd.print("Y = ");
        lcd.print(stepper2.currentPosition() / 0.125 / 200, 4);
        lcd.print(" mm");
        lcd2.setCursor(0, 2);
        lcd2.print("X = ");
        lcd2.print((-readDistanceCM(TRIG_PIN_1, ECHO_PIN_1)) * 10+110,4);
        lcd2.setCursor(17, 2);
        lcd2.print(" mm");
        lcd2.setCursor(0, 3);
        lcd2.print("Y = ");
        lcd2.print((-readDistanceCM(TRIG_PIN_2, ECHO_PIN_2)) * 10+110,4);
        lcd2.setCursor(17, 3);
        lcd2.print(" mm");
        // Uncomment below to display via serial monitor
        /*
                Serial.print("X = ");
                Serial.print(stepper.currentPosition() / 0.125 / 200, 4);
                Serial.print(" mm  Y = ");
                Serial.print(stepper2.currentPosition() / 0.125 / 200, 4);
                Serial.print(" mm  ");
                Serial.print("X = ");
                Serial.print((readDistanceCM(TRIG_PIN_1, ECHO_PIN_1))*10);
                Serial.print(" mm  ");
                Serial.print("Y = ");
                Serial.print((readDistanceCM(TRIG_PIN_2, ECHO_PIN_2))*10);
                Serial.print(" mm  ");
        */
        // Save data to SD card
        Loadcell();
        myFile.print("X = ");
        myFile.print(stepper.currentPosition() / 0.125 / 200, 4);
        myFile.print(" mm  ");
        myFile.print("Y = ");
        myFile.print(stepper2.currentPosition() / 0.125 / 200, 4);
        myFile.print(" mm");
        myFile.print("  X = ");
        myFile.print((-readDistanceCM(TRIG_PIN_1, ECHO_PIN_1)) * 10+110,4);
        myFile.print(" mm  ");
        myFile.print("Y = ");
        myFile.print((-readDistanceCM(TRIG_PIN_2, ECHO_PIN_2)) * 10+110,4);
        myFile.print(" mm  ");

        // Save load cell data to SD card at every time period
        float a = LoadCell_1.getData();
        float b = LoadCell_2.getData();
        String c = "g";
        if (pressedA == 1) {
          a = a * 0.00220462;
          b = b * 0.00220462;
          c = "lb";
        }
        int num =4;
        if (a >= 10 | b >=10){
          num =3;
        }
        myFile.print("  X Load = ");
        myFile.print(a, num);
        myFile.print(" ");
        myFile.print(c);
        myFile.print("  Y Load = ");
        myFile.print(b, num);
        myFile.print(" ");
        myFile.print(c);
      }
      // display time data
      t2 = millis();
      //Serial.print(t2 - t1);
      //Serial.println(" ms");
      lcd.setCursor(0, 3);
      lcd.print("t = ");
      lcd.print(t2 - t1);
      lcd.print(" ms");
      myFile.print("  t = ");
      myFile.print(t2 - t1);
      myFile.println(" ms  ");

      // enter mode 2 operation
      if (mode2 == 1) {
        if (key3) {
          holdKey = key3;
        }
        // Enter if key is pressed
        if (keypad.getState() == PRESSED) {
          if (holdKey == 'B' && stepper.currentPosition() < steps) {
            speed = speedData;
          }
          else if (holdKey == 'C' && stepper.currentPosition() > 0) {
            speed = -speedData;
          }
          else {
            speed = 0;
          }
        }
        // Enter if key is held
        else if (keypad.getState() == HOLD) {
          if ((millis() - t_hold) > 0 ) {
            if (holdKey == 'B' && stepper.currentPosition() < steps) {
              speed = speedData;
            }
            else if (holdKey == 'C' && stepper.currentPosition() > 0) {
              speed = -speedData;
            }
            else {
              speed = 0;
            }
            t_hold = millis();
          }
        }
        // Enter if idle
        else {
          speed = 0;
        }

        //motor running
        stepper.setSpeed(speed);
        stepper.runSpeed();

        stepper2.setSpeed(speed);
        stepper2.runSpeed();
      }

      // Enter mode 1 operation if operation 2 didnt run
      // Motor extension
      else if (stepper.currentPosition() <= steps && dir == 0) {
        stepper.setSpeed(speed);
        stepper.runSpeed();
        stepper2.setSpeed(speed);
        stepper2.runSpeed();
        // change direction
        if (stepper.currentPosition() > steps) {
          dir = 1;
        }

      }
      // Motor retraction
      else if (stepper.currentPosition() >= 0 && dir == 1) {
        stepper.setSpeed(-speed);
        stepper.runSpeed();
        stepper2.setSpeed(-speed);
        stepper2.runSpeed();
        // change direction
        if (stepper.currentPosition() < 0) {
          dir = 0;
        }
      }

      // stop motors
      key3 = keypad.getKey();
      if (key3 == 'A') {
        speed = 0;
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Motors Stopped");
        lcd.setCursor(0, 2);
        lcd.print("Press A to Continue");
        next = 0;
        validMM = 100;
        validIN = 100;
        validMMs = 100;
        validINs = 100;
        myFile.close();
        Serial.println("------Reading Data from SD------");
        myFile = SD.open("arduino.txt", FILE_READ);
        while (myFile.available()) {
          char ch = myFile.read(); // read characters one by one from Micro SD Card
          Serial.print(ch); // print the character to Serial Monitor
        }
        myFile.close();

      }
      i++;
    }
  }
int dot = 1;
  // User interface
  while (validIN > 1 && validMM > 25.4) {
    char key = keypad.getKey();               //key is input
    
    if (key != NO_KEY) {                      // When pressed

      // Request max displacement
      if (key == 'A' & next == 0) {                           //Continue after A is pressed
        myFile = SD.open("arduino.txt", FILE_WRITE);
        lcd.clear();
        lcd2.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Max");
        lcd.setCursor(0, 1);
        lcd.print("Displacement: in");
        lcd.setCursor(0, 2);
        lcd.print("0 - 1 in");
        dot = 1;
        displacementValue = "";
        numCheck = 0;
        pressedA = 1;
        pressedB = 0;
      }
      // Change to metrics
      if (key == 'B' & next == 0 & pressedA == 1) {            //Continue after B is pressed
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Max");
        lcd.setCursor(0, 1);
        lcd.print("Displacement: mm");
        lcd.setCursor(0, 2);
        lcd.print("0 - 25.4 mm");
        dot = 1;
        displacementValue = "";
        numCheck = 0;
        pressedA = 0;
        pressedB = 1;
      }

      // Enter numerical values
      if (key != 'A' && key != 'B' && key != 'C' && key != 'D' && key != '*' && key != '#' && (pressedA == 1 || pressedB == 1)) {
        displacementValue = displacementValue + key;  //Reset value
        lcd.clear();
        lcd2.clear();
        if (pressedA == 1) {
        lcd2.setCursor(0, 2);
        lcd2.print("0 - 1 in");
        }
        if (pressedB == 1) {
        lcd2.setCursor(0, 2);
        lcd2.print("0 - 25.4 mm");
        }
        lcd2.setCursor(0,0);
        lcd2.print("D to enter");

        lcd.setCursor(0, 0);
        lcd.print(displacementValue);        //display key input
        numCheck = 1;

      }
      // Allow decimal values
      if (key == '*' && dot == 1 && (pressedA == 1 || pressedB == 1)) {
        displacementValue = displacementValue + '.';  //Reset value
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(displacementValue);        //display key input
        dot = 0;
      }

      // Key evaluation
      if (key == 'D' && numCheck == 1) {  //if key D is pressed
      dot = 1;
        if (pressedA == 1) {
          validIN = displacementValue.toDouble();
        }
        if (pressedB == 1) {
          validMM = displacementValue.toDouble();
        }
        // Display input in imperial (Incorrect ver.)
        if (validIN > 1 && pressedA == 1) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Max Displacement");
          lcd.setCursor(0, 1);
          lcd.print("is too large");
          delay (2000);
          lcd.setCursor(0, 0);
          lcd.print("Enter Max");
          lcd.setCursor(0, 1);
          lcd.print("Displacement: in");
          displacementValue = "";
        }
        // Display input in metrics (Incorrect ver.)
        else if (validMM > 25.4 && pressedB == 1) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Max Displacement");
          lcd.setCursor(0, 1);
          lcd.print("is too large");
          delay (2000);
          lcd.setCursor(0, 0);
          lcd.print("Enter Max");
          lcd.setCursor(0, 1);
          lcd.print("Displacement: mm");
          displacementValue = "";
        }
        // Display inputs
        else {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Displacement: ");
          lcd.setCursor(0, 1);
          lcd.print(displacementValue);        //display key input
          if (pressedA == 1) {
            lcd.println(" in");
          }
          else {
            lcd.println(" mm");
          }

          delay(2000);
        }
      }
    }
  }
  // Request Speed
  lcd.clear();
  lcd2.clear();
  lcd.setCursor(0, 0);
  lcd.print("Enter Speed: in");
  lcd.setCursor(0, 1);
  lcd.print("(0.003937 -");
  lcd.setCursor(0, 2);
  lcd.print("0.03937 in/s)");
  speedValue = "";
  speedCheckA = 1;
  speedCheckB = 0;
  numCheck = 0;
  int pressedA2 = 1;
  int pressedB2 = 0;
  dot = 1;
  while ((validINs > 0.0393701 | validMMs > 1) | (validINs < 0.003937 | validMMs < 0.1)) {
    char key4 = keypad.getKey();               //key is input
    
    if (key4 != NO_KEY) {                      // When pressed
      if (key4 == 'A' & next == 0) {                           //Continue after A is pressed
        myFile = SD.open("arduino.txt", FILE_WRITE);
        lcd.clear();
        lcd2.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Speed: in");
        lcd.setCursor(0, 1);
        lcd.print("(0.003937 -");
        lcd.setCursor(0, 2);
        lcd.print("0.03937 in/s)");

        dot = 1;
        speedValue = "";
        speedCheckA = 1;
        speedCheckB = 0;
        numCheck = 0;
        pressedA2 = 1;
        pressedB2 = 0;
      }
      // Change to metrics
      if (key4 == 'B' & next == 0) {            //Continue after B is pressed
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Speed: mm");
        lcd.setCursor(0, 1);
        lcd.print("(0.1 - 1 mm/s)");

        dot = 1;
        speedValue = "";
        speedCheckA = 0;
        speedCheckB = 1;
        numCheck = 0;
        pressedA2 = 0;
        pressedB2 = 1;
      }

      // Enter numerical values
      if (key4 != 'A' && key4 != 'B' && key4 != 'C' && key4 != 'D' && key4 != '*' && key4 != '#' && (pressedA2 == 1 || pressedB2 == 1)) {
        speedValue = speedValue + key4;  //Reset value
        lcd.clear();
        lcd2.clear();
        if (pressedA2 == 1) {
        lcd2.setCursor(0, 2);
        lcd2.print("(0.003937 -");
        lcd2.setCursor(0, 3);
        lcd2.print("0.03937 in/s)");

        }
        if (pressedB2 == 1) {
        lcd2.setCursor(0, 2);
        lcd2.print("0.1 - 1 mm");

        }
        lcd2.setCursor(0,0);
        lcd2.print("D to enter");

        lcd.setCursor(0, 0);
        lcd.print(speedValue);        //display key input
        numCheck = 1;

      }
      // Allow decimal values
      if (key4 == '*' && dot == 1 && (pressedA2 == 1 || pressedB2 == 1)) {
        speedValue = speedValue + '.';  //Reset value
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(speedValue);        //display key input
        dot = 0;
      }

      // Key evaluation
      if (key4 == 'D' && numCheck == 1) {  //if key D is pressed
      dot = 1;
        if (pressedA2 == 1) {
          validINs = speedValue.toDouble();
          validMMs = validINs*25.4;
        }
        if (pressedB2 == 1) {
          validMMs = speedValue.toDouble();
          validINs = validMMs/25.4;

        }
        // Display input in imperial (Incorrect ver.)
        if ((validINs > 0.03937 || validINs < 0.003937) && pressedA2 == 1) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Invalid Speed");
          delay (2000);
          lcd.setCursor(0, 0);
          lcd.print("Enter Speed: in");
          lcd.setCursor(0, 1);
          lcd.print("(0.003937 -");
          lcd.setCursor(0, 2);
          lcd.print("0.03937 in/s)");
          speedValue = "";
        }
        // Display input in metrics (Incorrect ver.)
        else if ((validMMs > 1 || validMMs < 0.1) && pressedB2 == 1) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Invalid Speed");
          delay (2000);
          lcd.setCursor(0, 0);
          lcd.print("Enter Speed: mm");
          lcd.setCursor(0, 1);
          lcd.print("(0.1 - 1 mm/s)");
          speedValue = "";
        }
        // Display inputs
        else {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Displacement: ");
          lcd.setCursor(0, 1);
          lcd.print(displacementValue);        //display key input
          if (pressedA == 1) {
            lcd.println(" in");
          }
          else {
            lcd.println(" mm");
          }
          lcd.setCursor(0, 2);
          lcd.print("Speed: ");
          lcd.setCursor(0, 3);
          lcd.print(speedValue);        //display key input
          if (pressedA2 == 1) {
            lcd.println(" in/s");
          }
          else {
            lcd.println(" mm/s");
          }
          delay(2000);
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Mode1:Extend/Retract");
          lcd.setCursor(0, 1);
          lcd.print("Mode2:Manual Control");

          // Leading to mode 1 or mode 2
          char key2 = keypad.getKey();
          while (next == 0) {
            key2 = keypad.getKey();
            if (key2 == '1') {
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Mode1:Extend/Retract");
              next = 1;
              mode2 = 0;                  // enter mode 1
            }
            if (key2 == '2') {
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Mode2:Manual Control");
              next = 1;
              mode2 = 1;                  // enter mode 2
            }
          }
        }
      }
      
    }
  }
      delay(1000);
    t1 = millis();                  // record time at then end of loop
}
// Load cell function
void Loadcell() {
  static boolean newDataReady = 0;
  const int serialPrintInterval = 0; //increase value to slow down serial print activity

  // check for new data/start next conversion:
  if (LoadCell_1.update()) newDataReady = true;
  LoadCell_2.update();

  //get smoothed value from data set
  if ((newDataReady)) {
    if (millis() > t + serialPrintInterval) {
      float a = LoadCell_1.getData();
      float b = LoadCell_2.getData();
      String c = "g";
      lcd2.setCursor(0, 0);
      lcd2.print("X Load = ");
      if (pressedA == 1) {
        a = a * 0.00220462;
        b = b * 0.00220462;
        c = "lb";
      }
      int num =4;
      if (a >= 10 | b >=10){
        num =3;
      }
      lcd2.print(a, num);
      lcd2.setCursor(18, 0);
      lcd2.print(c);

      lcd2.setCursor(0, 1);
      lcd2.print("Y Load = ");
      lcd2.print(b, num);
      lcd2.setCursor(18, 1);
      lcd2.print(c);

      newDataReady = 0;
      t = millis();
    }
  }

  // receive command from serial terminal, send 't' to initiate tare operation:
  if (Serial.available() > 0) {
    char inByte = Serial.read();
    if (inByte == 't') {
      LoadCell_1.tareNoDelay();
      LoadCell_2.tareNoDelay();
    }
  }
  /*
    //check if last tare operation is complete
    if (LoadCell_1.getTareStatus() == true) {
      lcd2.setCursor(0,2);
      lcd2.print("Tare load cell 1 complete");
    }
    if (LoadCell_2.getTareStatus() == true) {
      lcd2.setCursor(0,3);
      lcd2.print("Tare load cell 2 complete");
    }
  */
}

// Ultra sonic sensor function
float readDistanceCM(int trig_pin, int echo_pin) {
  digitalWrite(trig_pin, LOW);
  //delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  //delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);
  int duration = pulseIn(echo_pin, HIGH);
  //if((duration * 0.034 / 2) <= 2){
  //  duration =2;
  //  return duration * 0.034 / 2;
  //  }
  //else{
  return duration * 0.034 / 2;
  //}
}