with Router elevator object

Display.cpp

 	int16_t w = 0;
 	u8g2_gfx.setFont(u8g2_font_helvB12_tf);
 	w = u8g2_gfx.getUTF8Width(s);
-	Serial.println("w: " + String(w));
 	u8g2_gfx.setForegroundColor(SSD1306_WHITE);
 	u8g2_gfx.setCursor((SCREEN_WIDTH - w) / 2, 30);
 	u8g2_gfx.println(F(s));
 
 	char *g;
 	w = u8g2_gfx.getUTF8Width(". . . . .");
-	Serial.println("w: " + String(w));
 	u8g2_gfx.setCursor((SCREEN_WIDTH - w) / 2, 50);
 	String gauge = "";
 	for (int i = 0; i < 6; i++) {

Fraestisch_SFTools.ino

 /*
- Steuerung für Frästisch_SFTools
+ Steuerung f�r Fr�stisch_SFTools
 
- Diese Datei enthält den Code für die Implementierung der Steuerung eines Frästisches mit 2 Nema17 Schrittmotoren.
- Sie unterstützt:
- - Schnelles/langsames Verstellen der Fräserhöhe
+ Diese Datei enth�lt den Code f�r die Implementierung der Steuerung eines Fr�stisches mit 2 Nema17 Schrittmotoren.
+ Sie unterst�tzt:
+ - Schnelles/langsames Verstellen der Fr�serh�he
  - Werkzeugwechsel
  - Automatisches Nullen mit WLS
  - Eintauchen mit vordefinierter Tiefe
- - Für obige Funktionen notwendige Konfiguration
+ - F�r obige Funktionen notwendige Konfiguration
 
  Erstellt:	05.01.2021
  Autor: 	Flo Smilari
 #include <ESP_FlexyStepper.h>
 #include <HardwareSerial.h>
 #include <pins_arduino.h>
-#include <RotaryEncoder.h>
 #include <Wire.h>
 #include <WString.h>
 
 #include "Display.h"
 #include "ExEzButton.h"
 #include "RotaryControler.h"
+#include "RouterElevator.h"
 #include "RouterSetup.h"
 #include "Status.h"
 #include "WLS.h"
 static const int RotEnc_Clk_Pin = 32;
 static const int RotEnc_Dta_Pin = 33;
 
-static const float MOVE_DOWNWARD = -1; // motor rotation counter clock wise
-static const float MOVE_UPWARD = 1;    // motor rotation clock wise
+static const int MOVE_DOWNWARD = -1; // motor rotation counter clock wise
+//static const int MOVE_UPWARD = 1;    // motor rotation clock wise
 
-byte limitSwitchState = 1;
-int previousDirection = 0;
+//byte limitSwitchState = 1;
+//int previousDirection = 0;
 int nullingTLS_intermediateState = 0;
 
 ExEzButton RedButton(RedBtn_Pin, false, 2000);
 ExEzButton GreenButton(GreenBtn_Pin, false, 2000);
 ExEzButton BlueButton(BlueBtn_Pin, false, 2000);
-//ExEzButton RotarySwitch(RotEnc_Switch_Pin, true, 2000);
 
 WLS WlsDetect(WLS_DETECT_Pin, true);
 WLS Wls(WLS_Pin);
 Display Display;
 ESP_FlexyStepper Stepper;
 RouterSetup Router_Setup;
+RouterElevator Router_Elevator(Stepper, Display, WlsDetect, Wls, LIMIT_SWITCH, MOVE_DOWNWARD);
 
 Status actualStatus;
 Status originStatus;
 //*** Limit switch interrupt routine
 //**********************************
 void limitSwitchHandler() {
-	limitSwitchState = digitalRead(LIMIT_SWITCH);
+//	limitSwitchState = digitalRead(LIMIT_SWITCH);
+	Router_Elevator.limitSwitchHandler();
 }
 
 //*****************************************************************************************************
 	Display.showInitialization();
 	//attach an interrupt to the IO pin of the limit switch and specify the handler function
 	attachInterrupt(digitalPinToInterrupt(LIMIT_SWITCH), limitSwitchHandler, CHANGE);
+
 	Stepper.connectToPins(STEP, DIR);
 	// set the speed and acceleration rates for the stepper motor
 	Stepper.setSpeedInStepsPerSecond(Router_Setup.getSpeed() * Router_Setup.getStepsPerRev());
 	Stepper.setStepsPerMillimeter(Router_Setup.getStepsPerRev() / Router_Setup.getPitch());
 	Stepper.setAccelerationInStepsPerSecondPerSecond(Router_Setup.getAcceleration() * Router_Setup.getStepsPerRev());
 	Stepper.setDecelerationInStepsPerSecondPerSecond(Router_Setup.getAcceleration() * Router_Setup.getStepsPerRev());
-	Stepper.startAsService(0);
+	if (!Stepper.isStartedAsService()) {
+		Stepper.startAsService(0);
+	}
+
 	if (Router_Setup.isToolChangOnPowerOn()) {
 		actualStatus = TOOL_CHANGE;
 	} else {
 		case NULLING:
 			printStatus("NULLING");
 			if (RedButton.isPressed()) {
-				Stepper.setCurrentPositionInMillimeters(0);
-				Stepper.setTargetPositionRelativeInMillimeters(0);
+//				Stepper.setCurrentPositionInMillimeters(0);
+//				Stepper.setTargetPositionRelativeInMillimeters(0);
+				Router_Elevator.setZeroPosition();
 				actualStatus = IDLE;
 			}
 			break;
 			if (nullingTLS_intermediateState == 0) {
 				printStatus("NULLING_TLS,0");
 				//Move elevator to lowest point (lower limit switch triggers)
-				Stepper.setTargetPositionRelativeInMillimeters(300 * MOVE_DOWNWARD);
+//				Stepper.setTargetPositionRelativeInMillimeters(300 * MOVE_DOWNWARD);
+				Router_Elevator.moveToLowerLimitSwitch();
 				actualStatus = MOVING_ELEVATOR;
 				nullingTLS_intermediateState = 1;
 			} else if (nullingTLS_intermediateState == 1) {
 				printStatus("NULLING_TLS,1");
 				if (RedButton.isPressed()) {
 					//Move elevator until it touch the TLS (WLS_PIN goes HIGH)
-					Stepper.setTargetPositionRelativeInMillimeters(300 * MOVE_UPWARD);
+//					Stepper.setTargetPositionRelativeInMillimeters(300 * MOVE_UPWARD);
+					Router_Elevator.moveToUpperLimitSwitch();
 					actualStatus = MOVING_ELEVATOR;
 					nullingTLS_intermediateState = 2;
 				}
 			} else if (nullingTLS_intermediateState == 2) {
 				printStatus("NULLING_TLS,2");
 				//Move elevator back about toolLenghtSensorHeight (will be the 0-Position)
-				Stepper.clearLimitSwitchActive();
-				Stepper.setTargetPositionRelativeInMillimeters(Router_Setup.getToolLenghtSensorHeight() * MOVE_DOWNWARD);
+				Router_Elevator.clearLimitSwitch();
+				Router_Elevator.moveRelativeInMillimeters(Router_Setup.getToolLenghtSensorHeight() * MOVE_DOWNWARD);
 				Serial.println(String(Router_Setup.getToolLenghtSensorHeight() * MOVE_DOWNWARD, 2));
 				actualStatus = MOVING_ELEVATOR;
 				nullingTLS_intermediateState = 3;
 			} else { // nullingTLS_intermediateState is 3
 				printStatus("NULLING_TLS,3");
 				//Set the 0-Position as actual position
-				Stepper.setCurrentPositionInMillimeters(0);
-				Stepper.setTargetPositionRelativeInMillimeters(0);
+//				Stepper.setCurrentPositionInMillimeters(0);
+//				Stepper.setTargetPositionRelativeInMillimeters(0);
+				Router_Elevator.setZeroPosition();
 				actualStatus = IDLE;
 				nullingTLS_intermediateState = 0;
 			}
 
 		case MOVING_ELEVATOR:
 			printStatus("MOVING_ELEVATOR");
-			if (limitSwitchState == LOW) {
+//			if (limitSwitchState == LOW) {
+			if (Router_Elevator.isLimitSwitchTriggerd()) {
 				// this will cause to stop any motion that is currently going on and block further movement in the same direction as long as the switch is active
-				Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
+//				Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
 				delay(200);
 				actualStatus = RELEASE_SWITCH;
 			} else {  // limitSwitchState is HIGH
-				if (Stepper.getDistanceToTargetSigned() == 0) {
+//				if (Stepper.getDistanceToTargetSigned() == 0) {
+				if (Router_Elevator.isTargetPositionReached()) {
+					Serial.println("isTargetPositionReached");
+					actualStatus = originStatus;
+					delay(200);
+				} else if (Router_Elevator.isWLSTriggerd()) {
+//				} else if (WlsDetect.isConnected()) {
+//					if (Wls.isPlugged()) {
+//						Serial.println("The Tool is away from WLS");
+//						Stepper.clearLimitSwitchActive();
+//					} else if (Wls.isUnplugged()) {
+//						Serial.println("The Tool touched the WLS");
+//						Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
 					actualStatus = originStatus;
 					delay(200);
-				} else if (WlsDetect.isConnected()) {
-					if (Wls.isPlugged()) {
-						Serial.println("The Tool is away from WLS");
-						Stepper.clearLimitSwitchActive();
-					} else if (Wls.isUnplugged()) {
-						Serial.println("The Tool touched the WLS");
-						Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
-						actualStatus = originStatus;
-						delay(200);
-					}
+//					}
 				}
-				Stepper.clearLimitSwitchActive();
-				previousDirection = Stepper.getDirectionOfMotion();
+//				Stepper.clearLimitSwitchActive();
+//				previousDirection = Stepper.getDirectionOfMotion();
+				Router_Elevator.clearLimitSwitch();
+				Router_Elevator.checkDirection();
 			}
 			break;
 
 		case RELEASE_SWITCH:
-			if (limitSwitchState == LOW) {
-				Stepper.moveRelativeInMillimeters(0.05 * previousDirection * -1); // move in opposite direction (away from switch)
+			printStatus("RELEASE_SWITCH");
+			//			if (limitSwitchState == LOW) {
+			if (Router_Elevator.isLimitSwitchTriggerd()) {
+//				Stepper.moveRelativeInMillimeters(0.05 * previousDirection * -1); // move in opposite direction (away from switch)
+				Router_Elevator.tryReleaseLimitSwitch();
 			} else {
 				actualStatus = originStatus;
 			}

RouterElevator.cpp

+/*
+ * RouterElevator.cpp
+ *
+ *  Created on: 12.02.2022
+ *      Author: FSmilari
+ */
+
+#include "RouterElevator.h"
+
+RouterElevator::RouterElevator(ESP_FlexyStepper &_Stepper, Display &_display, WLS &_WlsDetect, WLS &_Wls, int _LimitSwitch, int _DOWNWARD_DIR) :
+		WlsDetect(_WlsDetect), Wls(_Wls) {
+	Stepper = _Stepper;
+	display = _display;
+//	WlsDetect = _WlsDetect;
+//	Wls = _Wls;
+	LimitSwitch = _LimitSwitch;
+	DOWNWARD_DIR = _DOWNWARD_DIR;
+	UPWARD_DIR = -DOWNWARD_DIR;
+}
+
+void RouterElevator::setZeroPosition() {
+	Stepper.setCurrentPositionInMillimeters(0);
+	Stepper.setTargetPositionRelativeInMillimeters(0);
+}
+
+void RouterElevator::moveRelativeInMillimeters(float distanceInMillimeters) {
+	Stepper.setTargetPositionInMillimeters(distanceInMillimeters);
+}
+
+void RouterElevator::moveToLowerLimitSwitch(void) {
+	Stepper.setTargetPositionInMillimeters(300 * DOWNWARD_DIR);
+}
+
+void RouterElevator::moveToUpperLimitSwitch(void) {
+	Stepper.setTargetPositionInMillimeters(300 * UPWARD_DIR);
+}
+
+void RouterElevator::clearLimitSwitch(void) {
+	Stepper.clearLimitSwitchActive();
+}
+
+void RouterElevator::tryReleaseLimitSwitch(void) {
+	Stepper.moveRelativeInMillimeters(0.05 * previousDirection * -1); // move in opposite direction (away from switch)
+}
+
+bool RouterElevator::isLimitSwitchTriggerd() {
+	return limitSwitchState == LOW;
+}
+
+bool RouterElevator::isTargetPositionReached() {
+	return Stepper.getDistanceToTargetSigned() == 0;
+}
+
+bool RouterElevator::isWLSTriggerd() {
+	if (WlsDetect.isConnected()) {
+		if (Wls.isPlugged()) {
+			Serial.println("The Tool is away from WLS");
+			Stepper.clearLimitSwitchActive();
+			return false;
+		} else if (Wls.isUnplugged()) {
+			Serial.println("The Tool touched the WLS");
+			Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
+			return true;
+		}
+	}
+	return false;
+}
+
+void RouterElevator::limitSwitchHandler() {
+	limitSwitchState = digitalRead(LimitSwitch);
+	Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
+}
+
+void RouterElevator::checkDirection() {
+	previousDirection = Stepper.getDirectionOfMotion();
+}

RouterElevator.h

+/*
+ * RouterElevator.h
+ *
+ *  Created on: 12.02.2022
+ *      Author: FSmilari
+ */
+
+#ifndef ROUTERELEVATOR_H_
+#define ROUTERELEVATOR_H_
+
+#include <Arduino.h>
+#include <ESP_FlexyStepper.h>
+
+#include "Display.h"
+#include "WLS.h"
+
+class RouterElevator {
+
+	private:
+		ESP_FlexyStepper Stepper;
+		Display display;
+		int previousDirection = 0;
+		byte limitSwitchState = 1;
+		int LimitSwitch;
+		WLS &WlsDetect, &Wls;
+		int DOWNWARD_DIR;
+		int UPWARD_DIR;
+
+	public:
+		RouterElevator(ESP_FlexyStepper &_Stepper, Display &_display, WLS &_WlsDetect, WLS &_Wls, int _LimitSwitch, int _DOWNWARD_DIR);
+		void setZeroPosition(void);
+		void moveRelativeInMillimeters(float distanceInMillimeters);
+		void moveToLowerLimitSwitch(void);
+		void moveToUpperLimitSwitch(void);
+		void clearLimitSwitch(void);
+		void tryReleaseLimitSwitch(void);
+		bool isLimitSwitchTriggerd(void);
+		bool isWLSTriggerd(void);
+		bool isTargetPositionReached(void);
+		void limitSwitchHandler(void);
+		void checkDirection(void);
+};
+
+
+#endif /* ROUTERELEVATOR_H_ */

WLS.cpp

 		return getStateRaw() == LOW;
 	}
 }
+

WLS.h

 		bool inverted;
 
 	public:
+		WLS();
 		WLS(int pin);
 		WLS(int pin, bool _inverted);
 		bool isPlugged(void);

sloeber.ino.cpp

 //This is a automatic generated file
 //Please do not modify this file
 //If you touch this file your change will be overwritten during the next build
-//This file has been generated on 2022-02-05 01:05:00
+//This file has been generated on 2022-02-13 02:49:42
 
 #include "Arduino.h"
 #include <Arduino.h>
 #include <ESP_FlexyStepper.h>
 #include <HardwareSerial.h>
 #include <pins_arduino.h>
-#include <RotaryEncoder.h>
 #include <Wire.h>
 #include <WString.h>
 #include "Display.h"
 #include "ExEzButton.h"
 #include "RotaryControler.h"
+#include "RouterElevator.h"
 #include "RouterSetup.h"
 #include "Status.h"
 #include "WLS.h"