Switch and Socket Endurance Tester

/*Project:Switch/Socket Endurance Tester
  Author: Cajetan Chinonso Emmanuel*/

//#include <LEM.h>
#include <Wire.h>                                                                   // Include wire library
#include <LiquidCrystal_I2C.h>                                                      // Include New Liquid crystal for I2C bus
#include <Servo.h>                                                                  // Include Servo Library
#include <SD.h>                                                                     // Load SD library
#include <SPI.h>                                                                    // Load SPI library

#define LINEAR_ACTUATORPIN1 5                                                       // Define Servo Pin
#define LINEAR_ACTUATORPIN2 6

LiquidCrystal_I2C lcd(0x27, 20, 4);                                                 // set the LCD address to 0x27 for a 16 chars and 2 line display

Servo LINEAR_ACTUATOR1;                                                             // Set object as servo
Servo LINEAR_ACTUATOR2;

//int linearValue = 1000;                                                             // Value for linear actuator on start

int returnButton = 2;
int startPin = 3;                                                                   // Start pin
int resetBoard = 9;
int resetPin = 7;

int count_A;                                                                          // Counter 1 Value
int count_B;

const int setPoint = 10000;
int countPin1 = A1;
int countPin2 = A2;
int startVal = 0;                                                                   // Value to store start

unsigned long currentMillis = 0;
unsigned long currentMillis_B = 0;
unsigned long previousMillis;
unsigned long previousMillis_B;

const long on_SetTime = 20000;

byte currentButtonState_A;
byte previousButtonState_A;

byte currentButtonState_B;
byte previousButtonState_B;

boolean counting;
boolean counting_B;

//Lem sensor(HO_50_S, A0);

const int chipSelect = 4;
File testData;


void setup() {
  Serial.begin(115200);                                                             // Begin Serial Monitoring
  //  sensor.calibrate();

  digitalWrite(9, HIGH);

  pinMode(9, OUTPUT);
  SD.begin(chipSelect);

  lcd.init();                                                                       // LCD with 16 chars, 2 line display
  lcd.clear();                                                                      // Clear screen
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Scanning...");

  LINEAR_ACTUATOR1.attach(LINEAR_ACTUATORPIN1, 1000, 2000);                            // min and max position for servo
  LINEAR_ACTUATOR1.writeMicroseconds(1550);
  LINEAR_ACTUATOR2.attach(LINEAR_ACTUATORPIN2, 1000, 2000);
  LINEAR_ACTUATOR2.writeMicroseconds(1550);


  pinMode(startPin, INPUT_PULLUP);                                                   // Set start pin as input
  pinMode(resetPin, OUTPUT);
  pinMode(A1, INPUT_PULLUP);
  pinMode(A2, INPUT_PULLUP);
  pinMode(2, INPUT_PULLUP);
  pinMode(7, INPUT_PULLUP);

  if (!SD.begin(chipSelect)) {
    Serial.println("Card failed, or not present");
    lcd.setCursor(0, 0);
    lcd.print("No SD detected");
    lcd.setCursor(0, 1);
    lcd.print("Insert SD card");
    delay(750);
    digitalWrite(resetBoard, LOW);
    // don't do anything more:
    while (1);
  }
  //  Serial.println("card initialized.");
  delay(200);
  lcd.clear();
}

void stopRun() {
  //  linearValue = 1075;
  LINEAR_ACTUATOR1.writeMicroseconds(1000);
  LINEAR_ACTUATOR2.writeMicroseconds(1000);
}

void testFail() {
  if (digitalRead(7) == HIGH) {
    digitalWrite(resetBoard, LOW);
  }
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Failed at " + String(count_A));
  lcd.setCursor(0, 1);
  lcd.print("Failed at " + String(count_B));
  delay(4000);
}

void loop() {
  //  float I = sensor.getCurrentAC();
  startVal = digitalRead(startPin);

  lcd.setCursor(3, 0);
  lcd.print("INITIALISE");

  if (digitalRead(2) == HIGH) {
    stopRun();
  }

  if (digitalRead(7) == HIGH) {
    digitalWrite(resetBoard, LOW);
  }

  if (startVal == LOW && digitalRead(2) == LOW) {

    lcd.clear();
    //    linearValue = 1000;
    delay(1000);

    while (startVal == LOW && digitalRead(2) == LOW) {

      lcd.setCursor(0, 0);
      lcd.print("Pos 1 =");
      lcd.setCursor(0, 1);
      lcd.print("Pos 2 =");

      unsigned long currentMillis = millis();
      unsigned long currentMillis_B = millis();

      previousButtonState_A = currentButtonState_A;
      previousButtonState_B = currentButtonState_B;

      currentButtonState_A = digitalRead(A1);
      currentButtonState_B = digitalRead(A2);

      if (count_B <= 2 and currentMillis_B - previousMillis_B >= 20000) {
        if (counting_B = true) {
          lcd.setCursor(0, 1);
          lcd.print("NOT APPLICABLE");
          currentMillis_B = previousMillis_B;
          counting_B = false;
        }
      }

      if (currentButtonState_A == LOW) {
        Serial.println("OFF");
        //        linearValue = 1430;     //retract
        //        delay(200);
        //        currentMillis = onSwitch;
        delay(200);
        LINEAR_ACTUATOR1.writeMicroseconds(1550);
        delay(200);
        LINEAR_ACTUATOR2.writeMicroseconds(1800);
        //        delay(200);
      }

      else {
        if (previousButtonState_A == HIGH) {
          Serial.println("ON");
          //          currentMillis = offSwitch;
          delay(200);
          LINEAR_ACTUATOR1.writeMicroseconds(1800);
          delay(200);
          LINEAR_ACTUATOR2.writeMicroseconds(1550);
          //          delay(200);
        }
      }

      if (currentButtonState_A == LOW and previousButtonState_A == HIGH) {

        count_A++;
        //        float I = sensor.getCurrentAC();

        lcd.setCursor(8, 0);
        lcd.print(count_A);
        lcd.setCursor(0, 1);
        //        lcd.print("Amp: " + String(I) + "A");
        //        Serial.println(count);
        testData = SD.open("TSTdata.txt", FILE_WRITE);
        if (testData) {
          testData.println("Number of Cycles: " + String(count_A));
          testData.println("");
          testData.close();

          /*Serial.println("Number of Cycles: " + String(count) + "; Current: " + String(I * 11.538461538) + "A");
            Serial.println("");*/
        }

        previousMillis = currentMillis;

      }

      else if (currentButtonState_B == LOW and previousButtonState_B == HIGH) {

        count_B++;
        //        float I = sensor.getCurrentAC();

        lcd.setCursor(8, 1);
        lcd.print(count_B);
        lcd.setCursor(0, 1);
        //        lcd.print("Amp: " + String(I) + "A");
        //        Serial.println(count);
        testData = SD.open("TSTdata.txt", FILE_WRITE);
        if (testData) {
          testData.println("Number of Cycles: " + String(count_B));
          testData.println("");
          testData.close();

          /*Serial.println("Number of Cycles: " + String(count) + "; Current: " + String(I * 11.538461538) + "A");
            Serial.println("");*/
        }

        previousMillis = currentMillis;

      }


      else if (count_A and count_B == setPoint) {
        while (counting = true) {
          testData = SD.open("TSTdata.txt", FILE_WRITE);
          //          if (testData) {
          //            testData.println("Cycles completed; Current: " + String(I) + "A");
          //            testData.println("");
          //            testData.close();
          //
          //            /*Serial.println("Cycles completed; Current: " + String(I * 11.538461538) + "A");
          //              Serial.println("");*/
          //          }


          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Start New");
          lcd.setCursor(0, 1);
          lcd.print("Test Successful");
          delay(4000);
          counting = false;

          stopRun();
          if (digitalRead(2) == HIGH) break;
        }
      }


      else if ((currentButtonState_A == HIGH or previousButtonState_A == LOW) && (currentMillis - previousMillis >= on_SetTime)) {
        while (counting = true) {

          testData = SD.open("TSTdata.txt", FILE_WRITE);
          previousMillis = currentMillis;
          if (testData) {
            testData.println("Failed at " + String(count_A));
            //            testData.println("Current at " + String(count_A) + ": ");
            testData.println("");
            testData.close();

            /* Serial.println("Failed at " + String(count));
              Serial.println("Current at " + String(count) + ": " + String(I * 11.538461538) + "A");
              Serial.println("");*/
          }
          testFail();
          delay(100);
          stopRun();
          counting = false;
          if (digitalRead(2) == HIGH) break;
        }
      }

      if (digitalRead(2) == HIGH) break;
    }


    if (digitalRead(2) == HIGH) {
      lcd.clear();
      lcd.setCursor(0, 0);
      stopRun();
    }
  }
}