/*
 * Author David Florian
 * Simplified G-code interpreter 
 * 
 * If you decide not to implement optional features you must
 * comment out the correspondin
 * g blocks of code
 */
#define jfs

#ifdef otto
#include <TMCStepper.h>
#endif

//**** ARDUINO PINS *****

//***** X-Axis *****
#ifdef otto
 #define X_EN_PIN           22 // Enable
 #define X_STEP_PIN         23 // Step
 #define X_DIR_PIN          24 // Direction
 #define X_CS_PIN           25 // Chip select
 #define X_LIMIT            41 // Limit Switch
#else
  #define X_STEP_PIN                            54
  #define X_DIR_PIN                             55
  #define X_EN_PIN                              38
  #define X_CS_PIN                              53 // EXP2_07_PIN
  #define X_LIMIT                               3  // X_MIN_PIN
#endif

//***** Y-Axis *****
#ifdef otto
 #define Y_EN_PIN           26 // Enable
 #define Y_STEP_PIN         27 // Step
 #define Y_DIR_PIN          29 // Direction
 #define Y1_CS_PIN          28 // Chip select
 #define Y2_CS_PIN          30 // Chip select
 #define Y_LIMIT            42 // Limit Switch
#else
 #define Y_EN_PIN           56 // Enable
 #define Y_STEP_PIN         60 // Step
 #define Y_DIR_PIN          61 // Direction
 #define Y1_CS_PIN          49 // EXP2_04_PIN
 #define Y2_CS_PIN          49 // ???
 #define Y_LIMIT            14 // Y_MIN_PIN
#endif

//***** Z-Axis *****
#ifdef otto
 #define Z_EN_PIN           31 // Enable
 #define Z_STEP_PIN         32 // Step
 #define Z_DIR_PIN          33 // Direction
 #define Z_CS_PIN           34 // Chip select
 #define Z_LIMIT            45 // Limit Switch
#else
 #define Z_EN_PIN           62 // Enable
 #define Z_STEP_PIN         46 // Step
 #define Z_DIR_PIN          48 // Direction
 #define Z_CS_PIN           40 // Chip select
 #define Z_LIMIT            18 // Z_MIN_PIN
#endif

//***** P-Axis *****
#ifdef otto
 #define P_EN_PIN           62 // Enable
 #define P_STEP_PIN         36 // Step
 #define P_DIR_PIN          37 // Direction
 #define P_CS_PIN           38 // Chip select
#else
 #define P_EN_PIN           24 // E0_ENABLE_PIN
 #define P_STEP_PIN         26 // E0_STEP_PIN
 #define P_DIR_PIN          28 // E0_DIR_PIN
 #define P_CS_PIN           42 // E0_CS_PIN 
#endif


//***** Floating Head *****
#define FLOATING_HEAD      47 // Floating Head Switch

//***** Tip Sensors *****
#define X_LASER            44 // Yellow 
#define Y_LASER            40 // Black

//***** Optional LEDs ******
#define LED_SKIRT          5
#define LED_Pipette        4

//***** Optional Buttons *****
#define PLAY_PAUSE_BUTTON  50
#define PLAY_PAUSE_LED     2 // The LEDs in the button can be turned on and off
#define ALERT_BUTTON       46
#define ALERT_BUTTON_LED   3 // The LEDs in the button can be turned on and off

         

#ifdef otto
//**** STEPPER DRIVERS *****

//***** Configuring TMC2660 Stepper drivers *****
#define R_SENSE 0.1f // Match to your driver
                     // SilentStepStick series use 0.11
                     // UltiMachine Einsy and Archim2 boards use 0.2
                     // Panucatt BSD2660 uses 0.1
                     // Watterott TMC5160 uses 0.075

TMC2660Stepper driverX = TMC2660Stepper(X_CS_PIN, R_SENSE); 
TMC2660Stepper driverY1 = TMC2660Stepper(Y1_CS_PIN, R_SENSE);
TMC2660Stepper driverY2 = TMC2660Stepper(Y2_CS_PIN, R_SENSE); 
TMC2660Stepper driverZ = TMC2660Stepper(Z_CS_PIN, R_SENSE); 
TMC2660Stepper driverP = TMC2660Stepper(P_CS_PIN, R_SENSE); 
#endif

#ifdef jfs
#define motorInterfaceType 1
#endif

//***** Configuring Step/Dir Pins with AccelStepper *****

#include <AccelStepper.h>
#include <MultiStepper.h>

#ifdef otto
AccelStepper stepperX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper stepperZ(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN);
AccelStepper stepperP(AccelStepper::DRIVER, P_STEP_PIN, P_DIR_PIN);
#endif

#ifdef jfs
AccelStepper stepperX = AccelStepper(motorInterfaceType, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY = AccelStepper(motorInterfaceType, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper stepperZ = AccelStepper(motorInterfaceType, Z_STEP_PIN, Z_DIR_PIN);
AccelStepper stepperP = AccelStepper(motorInterfaceType, P_STEP_PIN, P_DIR_PIN);
#endif



#ifdef otto
//***** OPTIONAL LEDs CONFIGURATION *****
#include <Adafruit_NeoPixel.h> //These are WS2812 LEDs AKA Neopixels
int skirt_leds_num = 14; //Number of LEDs per strip
int pipette_leds_num = 8;

Adafruit_NeoPixel skirt_leds = Adafruit_NeoPixel(skirt_leds_num, LED_SKIRT, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel pipette_leds = Adafruit_NeoPixel(pipette_leds_num, LED_Pipette, NEO_GRB + NEO_KHZ800);
#endif

int changeDirection = 1;


// ***** Variables for Serial Communication with Computer *****

const byte numChars = 32;
char receivedChars[numChars];
char tempChars[numChars];        // temporary array for use when parsing

// variables to hold the parsed data
char mode[6];
int integerFromPC = 0;
float floatFromPC = 0.0;
boolean newData = false;


//***** Limit Switches *****
//variables to hold the limit switch states
boolean homeX;
boolean homeY;
boolean homeZ;
boolean homeP;

//***** STEP PER MM *****
//Its important that these values are correct
//so that the digital coordinate system = matches real world 
//Total step per revolution = full steps/rev * microsteps
//200 full steps * 16 microsteps = 3200 digital steps
int x_steps_mm = 80;
int y_steps_mm = 80;
int z_steps_mm = 80;
int p_steps_mm = 157.5; 


// variables to hold location data
double locX_in_mm;
double locY_in_mm;
double locZ_in_mm;
double locP_in_mm;

int locX_in_steps;
int locY_in_steps;
int locZ_in_steps;
int locP_in_steps;

#ifdef jfs
int locX_max = 27000;
int locY_max = -31200;
int locZ_max = 4400;
#endif

// variables to hold LED data

int ledR;
int ledG;
int ledB;

void setup() {
#ifdef otto
    SPI.begin();
#endif
    Serial.begin(9600);
    while(!Serial);

#ifdef otto   
    // Outputs for Chip Select (or SS) SPI Communication
    pinMode(X_CS_PIN, OUTPUT);
    pinMode(Y1_CS_PIN, OUTPUT);
    pinMode(Y2_CS_PIN, OUTPUT);
    pinMode(Z_CS_PIN, OUTPUT);
    pinMode(P_CS_PIN, OUTPUT);
    
    digitalWrite(X_CS_PIN, HIGH);
    digitalWrite(Y1_CS_PIN, HIGH);
    digitalWrite(Y2_CS_PIN, HIGH);
    digitalWrite(Z_CS_PIN, HIGH);
    digitalWrite(P_CS_PIN, HIGH);
#endif

    //***** Setting up Drivers *****
#ifdef otto
    // X-axis Driver
    driverX.begin();             // Initiate pins and registeries
    driverX.rms_current(1000);    // Set stepper current to 600mA. 
    driverX.microsteps(16);
    delay(100);

    // Y-axis Driver 1
    driverY1.begin();             // Initiate pins and registeries
    driverY1.rms_current(1000);    // Set stepper current to 600mA. 
    driverY1.microsteps(16);
    delay(100);

    // Y-axis Driver 2
    driverY2.begin();             // Initiate pins and registeries
    driverY2.rms_current(1000);    // Set stepper current to 600mA. 
    driverY2.microsteps(16);
    delay(100);

    // Z-axis Driver 
    driverZ.begin();             // Initiate pins and registeries
    driverZ.rms_current(400);    // Set stepper current to 600mA. 
    driverZ.microsteps(16);
    delay(100);

    // P-axis Driver 
    driverP.begin();             // Initiate pins and registeries
    driverP.rms_current(400);    // Set stepper current to 600mA. 
    driverP.microsteps(4);
    delay(100);
#endif

    //***** Setting up Switches, Sensors, and Buttons *****
    pinMode(X_LIMIT, INPUT); 
    pinMode(Y_LIMIT, INPUT); 
    pinMode(Z_LIMIT, INPUT); 
    pinMode(FLOATING_HEAD, INPUT); 
    pinMode(X_LASER, INPUT); 
    pinMode(Y_LASER, INPUT); 
    pinMode(PLAY_PAUSE_BUTTON, INPUT);
    pinMode(PLAY_PAUSE_LED, OUTPUT);
    digitalWrite(PLAY_PAUSE_LED, LOW);
    pinMode(ALERT_BUTTON, INPUT);
    pinMode(ALERT_BUTTON_LED, OUTPUT);
    digitalWrite(ALERT_BUTTON_LED, LOW);
    
    stepperX.setMaxSpeed(15000); // 100mm/s @ 160 steps/mm
    stepperX.setAcceleration(10000); // 2000mm/s^2
    stepperX.setEnablePin(X_EN_PIN);
    stepperX.setPinsInverted(false, false, true); // (directionInvert, stepInvert, enableInvert)
    stepperX.enableOutputs();

    stepperY.setMaxSpeed(12000); // 100mm/s @ 80 steps/mm
    stepperY.setAcceleration(8000); // 2000mm/s^2
    stepperY.setEnablePin(Y_EN_PIN);
    stepperY.setPinsInverted(false, false, true); // (directionInvert, stepInvert, enableInvert)
    stepperY.enableOutputs();

    stepperZ.setMaxSpeed(12000); // 100mm/s @ 80 steps/mm
    stepperZ.setAcceleration(7000); // 2000mm/s^2
    stepperZ.setEnablePin(Z_EN_PIN);
    stepperZ.setPinsInverted(false, false, true); // (directionInvert, stepInvert, enableInvert)
    stepperZ.enableOutputs();

    stepperP.setMaxSpeed(5000); // 100mm/s @ 80 steps/mm
    stepperP.setAcceleration(1000); // 2000mm/s^2
    stepperP.setEnablePin(P_EN_PIN);
    stepperP.setPinsInverted(true, false, true); // (directionInvert, stepInvert, enableInvert)
    stepperP.enableOutputs();

#ifdef jfs
pinMode(X_EN_PIN, OUTPUT); // Enable
digitalWrite(X_EN_PIN,LOW);
pinMode(Y_EN_PIN, OUTPUT); // Enable
digitalWrite(Y_EN_PIN,LOW);
pinMode(Z_EN_PIN, OUTPUT); // Enable
digitalWrite(Z_EN_PIN,LOW);
#endif 

#ifdef otto
    //***** Turn on LEDs *****
    skirt_leds.begin();
    for(int i=0;i<skirt_leds_num;i++){
    // skirt_leds.Color takes RGB values, from 0,0,0 up to 255,255,255
    skirt_leds.setPixelColor(i, skirt_leds.Color(50,50,50)); // red

    skirt_leds.show(); // This sends the updated pixel color to the hardware.
    }
    pipette_leds.begin();
    for(int i=0;i<pipette_leds_num;i++){
    // skirt_leds.Color takes RGB values, from 0,0,0 up to 255,255,255
    pipette_leds.setPixelColor(i, pipette_leds.Color(100,100,100)); // red

    pipette_leds.show(); // This sends the updated pixel color to the hardware.
    }
#endif
}

void loop() {
  recvWithStartEndMarkers();
    if (newData == true) {
        strcpy(tempChars, receivedChars);
            // this temporary copy is necessary to protect the original data
            //   because strtok() used in parseData() replaces the commas with \0
        Serial.println(tempChars);
        parseData();
        coordinateMove();
        newData = false;
    }
}

//============

void recvWithStartEndMarkers() {
  static boolean recvInProgress = false;
  static byte ndx = 0;
  char startMarker = '<';
  char endMarker = '>';
  char rc;

  while (Serial.available() > 0 && newData == false) {
      rc = Serial.read();
      if (recvInProgress == true) {
          if (rc != endMarker) {
              receivedChars[ndx] = rc;
              ndx++;
              if (ndx >= numChars) {
                  ndx = numChars - 1;
              }
          }
          else {
              receivedChars[ndx] = '\0'; // terminate the string
              recvInProgress = false;
              ndx = 0;
              newData = true;
          }
      }

      else if (rc == startMarker) {
          recvInProgress = true;
      }
  }
}

//============

void parseData() {      // split the data into its parts

    //***** Poll the current location of the stepper motors *****
    locX_in_mm = stepperX.currentPosition() / x_steps_mm;
    locY_in_mm = stepperY.currentPosition() / y_steps_mm;
    locZ_in_mm = stepperZ.currentPosition() / z_steps_mm;
    locP_in_mm = stepperP.currentPosition() / p_steps_mm;
    
    char * strtokIndx; // this is used by strtok() as an index
    strtokIndx = strtok(tempChars," ");
    strcpy(mode, strtokIndx);
    strtokIndx = strtok(NULL," ");
    while (strtokIndx != NULL){
      if (strtokIndx[0] == 'X'){
      memmove(strtokIndx, strtokIndx+1, strlen(strtokIndx));
        locX_in_mm = atof(strtokIndx); 
      }
      if (strtokIndx[0] == 'Y'){
      memmove(strtokIndx, strtokIndx+1, strlen(strtokIndx));
        locY_in_mm = -atof(strtokIndx); 
      }
      if (strtokIndx[0] == 'Z'){
      memmove(strtokIndx, strtokIndx+1, strlen(strtokIndx));
        locZ_in_mm = atof(strtokIndx); 
      }
      if (strtokIndx[0] == 'P'){
      memmove(strtokIndx, strtokIndx+1, strlen(strtokIndx));
        locP_in_mm = atof(strtokIndx); 
      }
      if (strtokIndx[0] == 'E'){
      memmove(strtokIndx, strtokIndx+1, strlen(strtokIndx));
        locP_in_mm = atof(strtokIndx); 
      }
      if (strtokIndx[0] == 'R'){
      memmove(strtokIndx, strtokIndx+1, strlen(strtokIndx));
        ledR = atoi(strtokIndx); 
      }
       if (strtokIndx[0] == 'U' || strtokIndx[0] == 'G'){
      memmove(strtokIndx, strtokIndx+1, strlen(strtokIndx));
        ledG = atoi(strtokIndx); 
      }
       if (strtokIndx[0] == 'B'){
      memmove(strtokIndx, strtokIndx+1, strlen(strtokIndx));
        ledB = atoi(strtokIndx); 
      }
      
    strtokIndx = strtok (NULL, " ");
    }
    
    //***** mm to steps conversions *****
    locX_in_steps = locX_in_mm * x_steps_mm;
#ifdef jfs
    if (locX_in_steps > locX_max) {
      Serial.println(" X off limits ");
      locX_in_steps = locX_max;
    }
#endif   
     locY_in_steps = locY_in_mm * y_steps_mm;
#ifdef jfs
     if (locY_in_steps < locY_max) {
      Serial.println(" Y off limits ");
      locY_in_steps = locY_max;
     }    
#endif 
    locZ_in_steps = locZ_in_mm * z_steps_mm;
#ifdef jfs
     if (locZ_in_steps > locZ_max) {
      Serial.println(" Z off limits ");
      locZ_in_steps = locZ_max;
     } 
#endif       
    locP_in_steps = locP_in_mm * p_steps_mm;

}

//============

void coordinateMove(){
  stepperX.setMaxSpeed(2000);
  stepperX.setAcceleration(1000);
  stepperY.setMaxSpeed(2000);
  stepperY.setAcceleration(1000);
  stepperZ.setMaxSpeed(2000);
  stepperZ.setAcceleration(1000);
  if (strcmp(mode, "G1") == 0){
    
    stepperX.moveTo(locX_in_steps);
    stepperY.moveTo(locY_in_steps);
    stepperZ.moveTo(locZ_in_steps);
    stepperP.moveTo(locP_in_steps);

    stepperX.setMaxSpeed(10000);
    stepperX.setAcceleration(3000);
    stepperY.setMaxSpeed(10000);
    stepperY.setAcceleration(4000);
    stepperZ.setMaxSpeed(10000);
    stepperZ.setAcceleration(6000);
    stepperP.setMaxSpeed(12000);
    stepperP.setAcceleration(6000);
    
    while(stepperX.targetPosition() != stepperX.currentPosition() or stepperY.targetPosition() != stepperY.currentPosition() or stepperZ.targetPosition() != stepperZ.currentPosition() or stepperP.targetPosition() != stepperP.currentPosition()){
        stepperX.run();
        stepperY.run();
        stepperZ.run();
        stepperP.run();
        delay(0.001);
        }
    Serial.println("<ok>");
  }

#ifdef otto
  //***** M150 - Change LED Color *****
  else if (strcmp(mode, "M150") == 0){
    for(int i=0;i<skirt_leds_num;i++){
      // skirt_leds.Color takes RGB values, from 0,0,0 up to 255,255,255
      skirt_leds.setPixelColor(i, skirt_leds.Color(ledR,ledG,ledB)); // red
      skirt_leds.show(); // This sends the updated pixel color to the hardware.
    }  
    for(int i=0;i<pipette_leds_num;i++){
      // skirt_leds.Color takes RGB values, from 0,0,0 up to 255,255,255
      pipette_leds.setPixelColor(i, pipette_leds.Color(ledR,ledG,ledB)); // red
      pipette_leds.show(); // This sends the updated pixel color to the hardware.
    }
    Serial.println("<ok>");
  }
#endif

  //***** M1001 - Laser/Mechanical probing of tip *****
  else if (strcmp(mode, "M1001") == 0){
    //Speeds and accelerations for tip probe move
    stepperX.setMaxSpeed(5000);
    stepperX.setAcceleration(2000);
    stepperY.setMaxSpeed(5000);
    stepperY.setAcceleration(2000);
    stepperZ.setMaxSpeed(5000);
    stepperZ.setAcceleration(500);
    
    boolean boolX = false;
    boolean tipcheckX = false;
    boolean tipcheckY = false;
    int coordX = stepperX.currentPosition();
    int coordY = stepperY.currentPosition();
    int coordXafter;
    int coordYafter;
    int laserXmove = coordX - 1600; //Take the current X position and move 1600 steps (20mm) in the -X direction
    int laserYmove = coordY + 8000; //Take the current Y position and move 8000 steps (20mm) in the +Y direction
  
    //Move in the -X direction until laser tripped
    stepperX.moveTo(laserXmove);
    Serial.println(stepperX.distanceToGo());
    while (boolX == false && stepperX.distanceToGo()!=0) {
    stepperX.run();
        if (digitalRead(X_LASER) != 0){
          delay(10);
          if (digitalRead(X_LASER) != 0){
            boolX= true;
            stepperX.setSpeed(0);
            coordXafter = stepperX.currentPosition();
            stepperX.move(400); //Move 5mm in the X+ direction. Now the pipette tip will be aligned with the tip that was used during the calibration on the motion controller screen.
            tipcheckX = true;
          }
        }
    }
    if (tipcheckX == false){
      stepperX.move(1600);
    }
    
    //Move back to the X starting point
    while(stepperX.targetPosition() != stepperX.currentPosition()){
      stepperX.run();
    }
  
    //Move up 10mm because the Y laser is higher
    stepperZ.move(4000);
    while(stepperZ.targetPosition() != stepperZ.currentPosition()){
        stepperZ.run();
    } 
  
     boolean boolY = false;
     stepperY.moveTo(laserYmove); 
      while (boolY == false && stepperY.distanceToGo()!=0) {
      stepperY.run();
          if (digitalRead(Y_LASER) != 0){
            delay(10);
            if (digitalRead(Y_LASER) != 0){
              boolY= true;
              stepperY.setSpeed(0);
              Serial.println(stepperY.distanceToGo());
              coordYafter = stepperY.currentPosition();
              stepperY.move(-2000); //Move 5mm in the Y- direction. Now the pipette tip will be aligned with the tip that was used during the calibration on the motion controller screen.
              tipcheckY = true;
            }
          }
      }

      if (tipcheckY == false){
      stepperY.move(-8000);
    }
      
      stepperZ.move(-4000);
      while(stepperY.targetPosition() != stepperY.currentPosition() or stepperZ.targetPosition() != stepperZ.currentPosition()){
        stepperY.run();
        stepperZ.run();
      }

      if (tipcheckY == false || tipcheckX == false){
        Serial.println("<no>");
      }
      else{
        Serial.println("<ok>");
      }
      
    }
  else if  (strcmp(mode, "J2") == 0){
    Serial.print("X Position ");
    Serial.println( stepperX.currentPosition());
    Serial.print("Y Position ");
    Serial.println( stepperY.currentPosition());
    Serial.print("Z Position ");
    Serial.println( stepperZ.currentPosition());    
  }
  else if (strcmp(mode, "JZ") == 0){

  //Homing Z at min
   stepperZ.setMaxSpeed(5000);
   stepperZ.setAcceleration(2000);
   int initial_homing = stepperZ.currentPosition();
   homeZ = false;
   while (homeZ == false){
      initial_homing--;  
      stepperZ.moveTo(initial_homing);
      stepperZ.run(); 
      if (digitalRead(Z_LIMIT) != 1) {        
        delay(1);
        if (digitalRead(Z_LIMIT) !=1){
          homeZ = true;
        }   
      }
    }
    stepperZ.setMaxSpeed(10000);
    stepperZ.setAcceleration(5000);
    stepperZ.setCurrentPosition(0);
   
    stepperP.setCurrentPosition(0);
    Serial.println("<Z Homed>");
}
    
  //***** G28 - Homing Cycle *****
  // For this code to work as written the limit switches must be located at
  // X - Mininum
  // Y - Minimun
  // Z - Minimun 
  else if (strcmp(mode, "G28") == 0){
   //Homing X at min
   stepperX.setMaxSpeed(5000);
   stepperX.setAcceleration(2000);
   int initial_homing = stepperX.currentPosition();
   homeX = false;
   while (homeX == false){
      initial_homing--;  
      stepperX.moveTo(initial_homing);
      stepperX.run(); 
      if (digitalRead(X_LIMIT) != 1) {        
        delay(1);
        if (digitalRead(X_LIMIT) !=1){
          homeX = true;
        }   
      }
    }
   stepperX.setMaxSpeed(10000);
   stepperX.setAcceleration(5000);
   stepperX.setCurrentPosition(0);
   //Homing Y at min
   stepperY.setMaxSpeed(5000);
   stepperY.setAcceleration(2000);
   initial_homing = stepperY.currentPosition();
   homeY = false;
   while (homeY == false){
      initial_homing++;  
      stepperY.moveTo(initial_homing);
      stepperY.run(); 
      if (digitalRead(Y_LIMIT) != 1) {        
        delay(1);
        if (digitalRead(Y_LIMIT) !=1){
          homeY = true;
        }   
      }
   }  
   stepperY.setMaxSpeed(10000);
   stepperY.setAcceleration(5000);
   stepperY.setCurrentPosition(0);
   //Homing Z at min
   stepperZ.setMaxSpeed(5000);
   stepperZ.setAcceleration(2000);
   initial_homing = stepperZ.currentPosition();
   homeZ = false;
   while (homeZ == false){
      initial_homing--;  
      stepperZ.moveTo(initial_homing);
      stepperZ.run(); 
      if (digitalRead(Z_LIMIT) != 1) {        
        delay(1);
        if (digitalRead(Z_LIMIT) !=1){
          homeZ = true;
        }   
      }
    }
    stepperZ.setMaxSpeed(10000);
    stepperZ.setAcceleration(5000);
    stepperZ.setCurrentPosition(0);
   
    stepperP.setCurrentPosition(0);
    Serial.println("<OK Homed>");
  }

    //########Grab Tip#########
  else if (strcmp(mode, "G38") == 0){
    //Move down until Floating head limit switch is triggered
    //The limit switch trig is debounced (50ms)

    int tipStatus = 0;
    int tipStepCount = stepperZ.currentPosition();
    int positionBeforeTip = stepperZ.currentPosition();
   
    stepperZ.setMaxSpeed(6000);
    stepperZ.setAcceleration(2000);
    
    while (tipStatus == 0){
      if (digitalRead(FLOATING_HEAD) != 0){
        
        stepperZ.setSpeed(0);
        stepperZ.moveTo(stepperZ.currentPosition());
       
        tipStatus = 1; 
      }
      else {
        tipStepCount--; 
        stepperZ.moveTo(tipStepCount);
      }
      stepperZ.run(); 
    }
    
    stepperZ.moveTo(positionBeforeTip);
    
    while(stepperZ.targetPosition() != stepperZ.currentPosition()){
      stepperZ.run();
      }
      Serial.println("<ok>");      
  }
}