/*
  Run a Linear Actuator, 
  a nema17 bipolar motor and 
  two servo motors in sequence.

  Steps are
  1: Linear actuator goes out to a distance of 2.5 inches and stops.
  2: The two servos which are side by side would trigger to 20 degrees and -20 degrees simultaneously and stop.
  3: Linear actuator would come back about 2 inches and stop.
  4: The nema17 motor would go 4 complete turns clockwise and stop.
  5: Linear actuator goes 4 inch retraction and stops.
  6: The nema17 would then go 4 turns counterclockwise.
  7: Then the two servo motors would open back up -20 degrees and +20 degrees
  8: Repeat the whole process when triggered.
*/

#include <Stepper.h>
#include <Servo.h>

// serial communication baudrate
#define BAUDRATE                  115200

//Arduino pin where everithing are connected to
#define STEPPER_DIR_PIN           2     //stepper dir pin
#define STEPPER_STEP_PIN          3     //stepper step pin
#define ACTUATOR_FWD_PIN          4     //actuator forward relay
#define ACTUATOR_BWD_PIN          5     //actuator backward relay
#define ACTUATOR_SW_START_POS_PIN 6     //start position switch pin
#define ACTUATOR_SW_MID_POS_PIN   7     //middle position switch pin
#define ACTUATOR_SW_END_POS_PIN   8     //end position switch pin
#define SERVO_1_PIN               10    //servo 1 pin 
#define SERVO_2_PIN               11    //servo 2 pin

//parameters
#define STEPPER_STEPS_PER_REV     4*4*200   //steps per revolution
#define STEPPER_SPEED             60    //speed of the motor in rpm


//global variables
uint8_t step = 0;            //number logic step

//Stepper object
Stepper stepper(STEPPER_STEPS_PER_REV,  STEPPER_DIR_PIN, STEPPER_STEP_PIN);

//servos setup
Servo myservo1;  // create servo object to control a servo
Servo myservo2;  // create servo object to control a servo

//Called once after power on
void setup() {

  Serial.begin(BAUDRATE);

  //pins mode setup
  pinMode(ACTUATOR_FWD_PIN, OUTPUT);
  pinMode(ACTUATOR_BWD_PIN, OUTPUT);
  pinMode(ACTUATOR_SW_START_POS_PIN, INPUT_PULLUP);
  pinMode(ACTUATOR_SW_MID_POS_PIN, INPUT_PULLUP);
  pinMode(ACTUATOR_SW_END_POS_PIN, INPUT_PULLUP);

  //stepper setup
  stepper.setSpeed(STEPPER_SPEED);

  //servo setup
  myservo1.attach(SERVO_1_PIN);  // attaches the servo to the servo object
  myservo2.attach(SERVO_2_PIN);  // attaches the servo to the servo object
  myservo1.write(90);
  myservo2.write(90);
}

//main loop
void loop() {
  
  // Continuously read the status of the switch
  int switchStartState = digitalRead(ACTUATOR_SW_START_POS_PIN);
  int switchMiddleState = digitalRead(ACTUATOR_SW_MID_POS_PIN);
  int switchEndState = digitalRead(ACTUATOR_SW_END_POS_PIN);


  // steps switch
  switch (step) {
    case 0: //start
        //wait for start
        step = 10;
        Serial.println("Step 1");
      break;
    case 10: //Linear actuator goes out to a distance of 2.5 inches and stops.
      digitalWrite(ACTUATOR_FWD_PIN, HIGH);
      if(switchMiddleState == LOW){
        digitalWrite(ACTUATOR_FWD_PIN, LOW);
        step = 20;
        Serial.println("Step 2");
      }
      break;
    case 20: //The two servos which are side by side would trigger to 20 degrees and -20 degrees simultaneously and stop.
        myservo1.write(90+20);
        myservo2.write(90-20);
        delay(500);
        step = 30;
        Serial.println("Step 3");
      break;
    case 30: //Linear actuator would come back about 2 inches and stop.
      digitalWrite(ACTUATOR_BWD_PIN, HIGH);
      if(switchStartState == LOW){
        digitalWrite(ACTUATOR_BWD_PIN, LOW);
        step = 40;
        Serial.println("Step 4");
      }
      break;
    case 40: //The nema17 motor would go 4 complete turns clockwise and stop.
      stepper.step(STEPPER_STEPS_PER_REV);
      delay(500);
      step = 50;
      Serial.println("Step 5");
      break;
    case 50: //Linear actuator goes 4 inch retraction and stops.
      digitalWrite(ACTUATOR_FWD_PIN, HIGH);
      if(switchEndState == LOW){
        digitalWrite(ACTUATOR_FWD_PIN, LOW);
        step = 51;
      }
      break;
    case 51: //Linear actuator goes 4 inch retraction and stops.
      digitalWrite(ACTUATOR_BWD_PIN, HIGH);
      if(switchStartState == LOW){
        digitalWrite(ACTUATOR_BWD_PIN, LOW);
        step = 60;
        Serial.println("Step 6");
      }
      break;
    case 60: //The nema17 would then go 4 turns counterclockwise.
      stepper.step(-STEPPER_STEPS_PER_REV);
      delay(500);
      step = 70;
      Serial.println("Step 7");
      break;
    case 70: //Then the two servo motors would open back up -20 degrees and +20 degrees
      myservo1.write(90);
      myservo2.write(90);
      delay(500);
      step = 80;
      Serial.println("Step 8");
      break;
    case 80: //Repeat the whole process when triggered.
      step = 0;
    break;
  }
}