Final Version

Fraestisch_SFTools.ino

 //static const int MOVE_UPWARD = 1;  // motor rotation clock wise
 
 int intermediateState = 0;
+bool isPowerOn = true;
 
 ExEzButton RedButton(RedBtn_Pin, false, 2000);
 ExEzButton GreenButton(GreenBtn_Pin, false, 2000);
 int displaycounter = 0; // Store current counter value
 volatile boolean TurnDetected;
 
-void printStatus(String actualStatus) {
-	if (!oldStatus.equals(actualStatus)) {
-		Serial.println(actualStatus);
-		oldStatus = actualStatus;
-	}
-}
+//void printStatus(String actualStatus) {
+//	if (!oldStatus.equals(actualStatus)) {
+//		Serial.println(actualStatus);
+//		oldStatus = actualStatus;
+//	}
+//}
 
 //**********************************
 //*** Limit switch interrupt routine
 //*****************************************************************************************************
 void Initialize() {
 	Router_Setup.readFromEEPROM();
-	Router_Setup.printValues();
+//	Router_Setup.printValues();
 
 	Display.showInitialization();
 	//attach an interrupt to the IO pin of the limit switch and specify the handler function
 		Stepper.startAsService(0);
 	}
 
-	if (Router_Setup.isToolChangOnPowerOn()) {
+	if (isPowerOn && Router_Setup.isToolChangOnPowerOn()) {
 		SetActualStatus(TOOL_CHANGE);
+		isPowerOn = false;
 	} else {
 		SetActualStatus(IDLE);
 	}
 			break;
 
 		case TOOL_CHANGE:
-			printStatus("TOOL_CHANGE");
+			//printStatus("TOOL_CHANGE");
 			BlueButton.loop();
 			RotaryControler.loop();
 			originStatus = TOOL_CHANGE;
 			break;
 
 		case CONFIGURATION:
-			printStatus("CONFIGURATION");
+			//printStatus("CONFIGURATION");
 			BlueButton.loop();
 			RotaryControler.loop();
 			Router_Setup.initialize();
 			break;
 
 		case NULLING:
-			printStatus("NULLING");
+			//printStatus("NULLING");
 			RedButton.loop();
 			if (RedButton.isPressed()) {
 				Router_Elevator.setZeroPosition();
 			RedButton.loop();
 			originStatus = NULLING_TLS;
 			if (intermediateState == 0) {
-				printStatus("NULLING_TLS,0");
+				//printStatus("NULLING_TLS,0");
 				//Move elevator to lowest point (lower limit switch triggers)
 				Router_Elevator.moveToLowerLimitSwitch();
 				SetActualStatus(MOVING_ELEVATOR);
 				intermediateState = 1;
 			} else if (intermediateState == 1) {
-				printStatus("NULLING_TLS,1");
+				//printStatus("NULLING_TLS,1");
 				if (RedButton.isPressed()) {
 					//Move elevator until it touch the TLS (WLS_PIN goes HIGH)
 					Router_Elevator.moveToUpperLimitSwitch();
 					intermediateState = 2;
 				}
 			} else if (intermediateState == 2) {
-				printStatus("NULLING_TLS,2");
+				//printStatus("NULLING_TLS,2");
 				//Move elevator back about toolLenghtSensorHeight (will be the 0-Position)
 				Router_Elevator.clearLimitSwitch();
 				Router_Elevator.moveRelativeInMillimeters(Router_Setup.getToolLenghtSensorHeight() * MOVE_DOWNWARD);
-				Serial.println(String(Router_Setup.getToolLenghtSensorHeight() * MOVE_DOWNWARD, 2));
+				//Serial.println(String(Router_Setup.getToolLenghtSensorHeight() * MOVE_DOWNWARD, 2));
 				SetActualStatus(MOVING_ELEVATOR);
 				intermediateState = 3;
 			} else { // nullingTLS_intermediateState is 3
-				printStatus("NULLING_TLS,3");
+				//printStatus("NULLING_TLS,3");
 				//Set the 0-Position as actual position
 				Router_Elevator.setZeroPosition();
 				SetActualStatus(IDLE);
 			break;
 
 		case IDLE:
-			printStatus("IDLE");
+			//printStatus("IDLE");
 			RedButton.loop();
 			GreenButton.loop();
 			BlueButton.loop();
 			RotaryControler.loop();
 			originStatus = IDLE;
+			if (RotaryControler.isSwitchLongPressed()) {
+				SetActualStatus(CONFIGURATION);
+			}
 			if (RotaryControler.isSwitchPressed()) {
 				Router_Elevator.toggleMode();
 			}
 			} else if (GreenButton.isPressed()) {
 				Router_Elevator.moveToTarget();
 				SetActualStatus(MOVING_ELEVATOR);
+			} else if (RedButton.isPressed()) {
+				Router_Elevator.moveToParkPosition();
+				SetActualStatus(MOVING_ELEVATOR);
 			}
 			break;
 
 		case DIVING:
-			printStatus("DIVING");
+			//printStatus("DIVING");
 			GreenButton.loop();
 			originStatus = DIVING;
 			Router_Elevator.setMaxDiveDistance();
 			if (GreenButton.isPressed()) {
-				Serial.println("GB pressed");
+				//Serial.println("GB pressed");
 				if (!Router_Elevator.maxDiveDistanceReached()) {
 					Router_Elevator.incrementDiveDistance();
 					Router_Elevator.moveToTarget();
 			break;
 
 		case MOVING_ELEVATOR:
-			printStatus("MOVING_ELEVATOR");
+			//printStatus("MOVING_ELEVATOR");
 			if (Router_Elevator.isLimitSwitchTriggerd()) {
 				delay(200);
 				SetActualStatus(RELEASE_SWITCH);
 			break;
 
 		case RELEASE_SWITCH:
-			printStatus("RELEASE_SWITCH");
+			//printStatus("RELEASE_SWITCH");
 			if (Router_Elevator.isLimitSwitchTriggerd()) {
 				Router_Elevator.tryReleaseLimitSwitch();
 			} else {

RouterElevator.cpp

 bool RouterElevator::isWLSTriggerd() {
 	if (WlsDetect.isConnected()) {
 		if (Wls.isPlugged()) {
-			Serial.println("The Tool is away from WLS");
+//			Serial.println("The Tool is away from WLS");
 			Stepper.clearLimitSwitchActive();
 			return false;
 		} else if (Wls.isUnplugged()) {
-			Serial.println("The Tool touched the WLS");
+//			Serial.println("The Tool touched the WLS");
 			Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
 			return true;
 		}
 
 void RouterElevator::setMaxDiveDistance() {
 	if (isDoDiveInitialization()) {
-		Serial.println("setMaxDiveDistance");
 		maxDiveDistance = targetDistance;
 		targetDistance = 0.0;
 		display.setDistanceValue(targetDistance);
 bool RouterElevator::incrementDiveDistance() {
 	float increment = Router_Setup.getLevelHeightDiving();
 	targetDistance = min(maxDiveDistance, targetDistance + increment);
-	Serial.println("targetDistance: " + String(targetDistance, 2));
 	display.setMaxDiveDistance(maxDiveDistance);
 	display.setDistanceValue(targetDistance);
 	display.setLevelHeightDiving(Router_Setup.getLevelHeightDiving());

RouterSetup.cpp

 	eeprom.end();
 }
 
-void RouterSetup::printValues() {
-	Serial.print(String(SPEED) + ": " + String(getSpeed(), 1) + ", ");
-	Serial.print(String(ACCELERATION) + ": " + String(getAcceleration(), 1) + ", ");
-	Serial.print(String(STEPSPERREV) + ": " + String(getStepsPerRev()) + ", ");
-	Serial.print(String(PITCH) + ": " + String(getPitch(), 1) + ", ");
-	Serial.print(String(TOOLLENGHTSNHT) + ": " + String(getToolLenghtSensorHeight(), 2) + ", ");
-	Serial.print(String(ENCSPEEDSLOW) + ": " + String(getEncoderSpeedSlow()) + ", ");
-	Serial.print(String(ENCSPEEDFAST) + ": " + String(getEncoderSpeedFast()) + ", ");
-	Serial.print(String(LVLHEIGHTDIVE) + ": " + String(getLevelHeightDiving(), 2) + ", ");
-	Serial.println(String(TOOLCHGONPWRON) + ": " + String(isToolChangOnPowerOn()));
-}
-
+//void RouterSetup::printValues() {
+//	Serial.print(String(SPEED) + ": " + String(getSpeed(), 1) + ", ");
+//	Serial.print(String(ACCELERATION) + ": " + String(getAcceleration(), 1) + ", ");
+//	Serial.print(String(STEPSPERREV) + ": " + String(getStepsPerRev()) + ", ");
+//	Serial.print(String(PITCH) + ": " + String(getPitch(), 1) + ", ");
+//	Serial.print(String(TOOLLENGHTSNHT) + ": " + String(getToolLenghtSensorHeight(), 2) + ", ");
+//	Serial.print(String(ENCSPEEDSLOW) + ": " + String(getEncoderSpeedSlow()) + ", ");
+//	Serial.print(String(ENCSPEEDFAST) + ": " + String(getEncoderSpeedFast()) + ", ");
+//	Serial.print(String(LVLHEIGHTDIVE) + ": " + String(getLevelHeightDiving(), 2) + ", ");
+//	Serial.println(String(TOOLCHGONPWRON) + ": " + String(isToolChangOnPowerOn()));
+//}
 
 void RouterSetup::initialize() {
 	if (doInitialization) {

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-03-01 10:11:02
+//This file has been generated on 2022-03-01 17:52:01
 
 #include "Arduino.h"
 #include <Arduino.h>
 #include "Status.h"
 #include "WLS.h"
 
-void printStatus(String actualStatus) ;
 void limitSwitchHandler() ;
 void Initialize() ;
 void setup() ;