#include <AccelStepper.h>                    // Includes AccelStepper library in this programe.
AccelStepper stepper(1, 3, 6);               // Pin configuration for stepper motor selection of X Section on CNC Shield. 

int speedValue,pauseValue,dirValue,dirmove;  // variables
int pauseRead,goforward,a;                   // variables
int redclr,greenclr,blueclr;                 // color variable
double holdTime,upTime,downTime,distance;     // variables

float maxSpeed,inpSpeed;                  // variables
float t1,t2;                              // Time variables

const int selection_Pin = A0;                // CNC Shield Abort(A0) pin to connect GND,3.3V,5V .
const int selection_Pin_Dir = A1;            // CNC Shield Hold(A2) pin to connect GND,Open .
const int pushbtn=12;                        // Pin number 12 selected for push button.
const int red = 11;                          // Pin number 11 selected for RED color in RGB LED.
const int green = 9;                         // Pin number 9 selected for GREEN color in RGB LED.
const int blue = 10;                         // Pin number 10 selected for BLUE color in RGB LED.
// Use CNC Shield Resume(A2) pin to connect GND,or Open to get two modes of functional types, for GND --> Push type mechanisum, Open --> Pull type mechanisum.
void getSpeedRange(int inp){
    if (inp <= 600) {                       // Speed = 1cm/sec
      redclr = 0;greenclr = 5;blueclr = 0;   // Green
      maxSpeed = 500;
    } else if (inp > 600 && inp <= 1470) {  // Speed = 2cm/sec
      redclr = 5;greenclr = 5;blueclr = 5;   // white
      maxSpeed = 980;
    }else if (inp > 1470 && inp <= 1900) {  // Speed = 3cm/sec
      redclr = 0;greenclr = 5;blueclr = 5;   // Aqua
      maxSpeed = 1470;
    }else if (inp > 1900 && inp <= 2500) {  // Speed = 4cm/sec
      redclr = 0;greenclr = 3;blueclr = 5;   // orange
      maxSpeed = 2000;
    } else if (inp > 2500 && inp <= 3080) {  // Speed = 5cm/sec
      redclr = 0;greenclr = 0;blueclr = 5;   // Blue
      maxSpeed = 2519;
    } else if (inp > 3080 && inp <= 3700) {  // Speed = 6cm/sec
      redclr = 5;greenclr = 0;blueclr = 5;   // Purple
      maxSpeed = 3080;
    } else if (inp > 3700 && inp <= 4450) {  // Speed = 7cm/sec
      redclr = 5;greenclr = 5;blueclr = 0;   // yellow
      maxSpeed = 3665;
    } else {                                 // Speed = 8cm/sec
      redclr = 5;greenclr = 0;blueclr = 0;   // Red
      maxSpeed = 4300;
    }
}
void direction(int dirV){                    // direction Function to set the Mode
  if(dirV >=10){                             // If selecter switch is at Left direction, "Pull Method selected"
       a=-2950;
       distance = 6.1;                       // distance between slide edge to blood spot is 6.1 cm 
       Serial.println("Pull Technique");     // Print on serial port.
  }
  else 
  {                                          // If selecter switch is at Right direction, "Push Method selected"
       distance = 5.71;                       // Push technique slide travel less then Pull technique to reach the spot 5.7 cm.
       a=-2755;
       Serial.println("Push Technique");     // Print on serial port.
  }
}
void setup() {                                          // Code run once
  Serial.begin(115200);                                 // Set Baudrate to 115200.
  }                                                       // setup ends

void loop() {  
    a=-2755;                                        // Running in loop                                                  // Infinite Loop ends                            // Find appropriate Mode selection according to voltage level selected, Push or Pull.
    stepper.moveTo(a);                                  // Stepper motor set to move at position defined in "a" variable.
    stepper.enableOutputs();                            // enable power.
    stepper.setAcceleration(60000);                     // ACCELERATION = Steps /(second)^2.
    stepper.setMaxSpeed(1990);                          // set speed.
    t1 = micros();                                      // t1 initialise milisecond value.
    while(stepper.currentPosition() != a)               // stepper motor run until not reached to a position defined in "a" variable.
          stepper.run();                                
    t2 = micros();                                      // t2 initialise milisecond value.
    upTime = distance/((t2-t1)*0.000001);                  // Differance between t2 and t1 gives time taken in the process in msec.
    Serial.print("Speed of going Forward: ");Serial.print((float)upTime,1);Serial.println(" cm/second"); // Print on serial port.
    t1 = micros();                                       // t1 initialise milisecond value.
    stepper.disableOutputs();                           // disable power.
    delay(5000);                                   // Pause for 1 sec.
    t2 =micros();                                        // t2 initialise milisecond value.
    holdTime=(t2-t1)*0.000001;                             // Differance between t2 and t1 gives time taken in the process in msec.
    Serial.print("Holding Time: ");Serial.print(holdTime);Serial.println(" seconds");  // Print on serial port.
    stepper.setAcceleration(60000);                     // ACCELERATION = Steps /(second)^2.
    stepper.setMaxSpeed(2000);                      // set speed.
    stepper.moveTo(0);                                  // Stepper motor set to move at position 0(init position).
    stepper.enableOutputs();                            // enable power.
    t1 = micros();                                      // t1 initialise milisecond value.
    while(stepper.currentPosition() != 0)               // stepper motor run until not reached to an initial position(0).
          stepper.run();
    t2 = micros();                                      // t2 initialise milisecond value.
    downTime=distance/((t2-t1)*0.000001);                  // Differance between t2 and t1 gives time taken in the process in msec.
    Serial.print("Speed of going Backward: ");Serial.print((float)downTime,1);Serial.println(" cm/second");  // Print on serial port.
    stepper.disableOutputs();                           // disable power.
    while(1);
}                                                       // Loop ends