Idle routine (targetvalue and elevator move to target)

Display.cpp

 /*****************
  ** Constructors.
  ****************/
-Display::Display() : mode(SLOW) {
+Display::Display() : mode(FAST) {
 	ssd1306 = Adafruit_SSD1306(SCREEN_WIDTH, SCREEN_HEIGHT);
 	u8g2_gfx.begin(ssd1306);
 	wlsConnected = false;
 	starttime = millis();
 	refreshScreen = true;
 	distanceValue = 0;
+	distanceUnit = "mm";
 }
 
 /******************
 			redrawFrame();
 			ssd1306.drawLine(37, 43, SCREEN_WIDTH - 1, 43, SSD1306_WHITE);
 			drawStatusText(STATUS_TXT_IDLE);
+			drawDistanceValue(distanceValue);
+			drawDistanceUnit(distanceUnit);
 			drawMode(this->mode);
 			break;
 		case NULLING:
 
 void Display::setMode(const ValueMode &mode) {
 	if (!this->mode.equals(mode)) {
-		Serial.println("Equal: " + this->mode.toString());
 		this->mode = mode;
 		setRefreshScreen();
 	}
 }
 
 void Display::drawDistanceValue(float distance) {
+	String txt = String(distance, 2);
+	char *s = &txt[0];
+	int16_t w = 0, h = 0;
+	u8g2_gfx.setFont(u8g2_font_logisoso22_tf);
+	w = u8g2_gfx.getUTF8Width(s);
+	h = u8g2_gfx.getFontAscent() - u8g2_gfx.getFontDescent();
+	u8g2_gfx.setCursor((SCREEN_WIDTH - w - 6), h);
+	u8g2_gfx.println(F(s));
 }
 
 void Display::drawDistanceUnit(String unit) {
+	String txt = unit;
+	char *s = &txt[0];
+	int16_t w = 0;
+	u8g2_gfx.setFont(u8g2_font_helvB12_tf);
+	w = u8g2_gfx.getUTF8Width(s);
+	u8g2_gfx.setCursor((SCREEN_WIDTH - w - 6), 41);
+	u8g2_gfx.println(F(s));
 }
 
 /********************************

Fraestisch_SFTools.ino

 	pinMode(GreenBtn_Pin, INPUT);
 	pinMode(RedBtn_Pin, INPUT);
 	pinMode(BlueBtn_Pin, INPUT);
-	pinMode(RotEnc_Switch_Pin, INPUT_PULLUP);
+	pinMode(RotEnc_Switch_Pin, INPUT);
 //	pinMode(RotEnc_Clk_Pin, INPUT_PULLUP);
 //	pinMode(RotEnc_Dta_Pin, INPUT_PULLUP);
 	pinMode(LIMIT_SWITCH, INPUT);
 				Router_Setup.onRotaryControlerLongSwitch();
 				SetActualStatus(INITIALIZATION);
 			} else if (BlueButton.isPressed()) {
+				Router_Setup.cancel();
 				SetActualStatus(IDLE);
 			}
 			break;
 
 		case IDLE:
 			printStatus("IDLE");
+			originStatus = IDLE;
 			if (RotaryControler.isSwitchPressed()) {
-				Serial.println("RotaryControler.isSwitchPressed");
 				Router_Elevator.toggleMode();
 			}
 			Router_Elevator.onRotaryControlerTurn(RotaryControler.getEncoderMove());
 				}
 			} else if (BlueButton.isLongPressed()) {
 				SetActualStatus(TOOL_CHANGE);
+			} else if (GreenButton.isLongPressed()) {
+
+			} else if (GreenButton.isPressed()) {
+				Router_Elevator.moveToTarget();
+				SetActualStatus(MOVING_ELEVATOR);
 			}
 			break;
 
 		case DIVING:
+			printStatus("DIVING");
+			originStatus = DIVING;
 			break;
 
 		case MOVING_ELEVATOR:

RotaryControler.cpp

 }
 
 RotaryEncoder::Direction RotaryControler::getEncoderMove() {
-	long newPos = Encoder.getPosition();
 	RotaryEncoder::Direction dir = Encoder.getDirection();
-	if (position != newPos) {
-		Serial.print("pos:");
-		Serial.print(newPos);
-		Serial.print(" dir:");
-		Serial.println((int) dir);
-		position = newPos;
-	}
 	return dir;
 }

RouterElevator.cpp

 
 RouterElevator::RouterElevator(ESP_FlexyStepper &_Stepper, Display &_display, RouterSetup &_Router_Setup,
 		WLS &_WlsDetect, WLS &_Wls, int _LimitSwitch, int _DOWNWARD_DIR)
-: Stepper(_Stepper), display(_display), Router_Setup(_Router_Setup), WlsDetect(_WlsDetect), Wls(_Wls), mode(SLOW) {
+: Stepper(_Stepper), display(_display), Router_Setup(_Router_Setup), WlsDetect(_WlsDetect), Wls(_Wls), mode(FAST) {
 	LimitSwitch = _LimitSwitch;
 	DOWNWARD_DIR = _DOWNWARD_DIR;
 	UPWARD_DIR = -DOWNWARD_DIR;
 	Stepper.setCurrentPositionAsHomeAndStop();
 	Stepper.setCurrentPositionInMillimeters(0);
 	Stepper.setTargetPositionRelativeInMillimeters(0);
+	targetDistance = 0.0;
+	setMode(FAST);
+	display.setDistanceValue(targetDistance);
 }
 
 void RouterElevator::moveRelativeInMillimeters(float distanceInMillimeters) {
 }
 
 void RouterElevator::moveToTarget(void) {
-	Stepper.moveToPositionInMillimeters(targetDistance);
+	Stepper.setTargetPositionInMillimeters(targetDistance);
 }
 
 void RouterElevator::moveToParkPosition(void) {
 	float parkOffset = -5; //Router_Setup.getParkPostionOffset() * -1;
-	Stepper.moveToPositionInMillimeters(parkOffset);
+	Stepper.setTargetPositionInMillimeters(parkOffset);
 }
 
 bool RouterElevator::isWLSTriggerd() {
 
 void RouterElevator::setMode(ValueMode mode) {
 	this->mode = mode;
-	Serial.println("RouterElevator::setMode: " + this->mode.toString());
 	display.setMode(this->mode);
 }
 
 }
 
 void RouterElevator::onRotaryControlerTurn(RotaryEncoder::Direction turn) {
-//	if (turn != RotaryEncoder::Direction::NOROTATION) {
-//		int sign = (turn == RotaryEncoder::Direction::CLOCKWISE) ? 1 : -1;
-//		//TODO: Update the targetDistance and show it on display
-//	}
+	if (turn != RotaryEncoder::Direction::NOROTATION) {
+		int sign = (turn == RotaryEncoder::Direction::CLOCKWISE) ? 1 : -1;
+		float increment = mode.isSlow() ? Router_Setup.getEncoderSpeedSlow() : Router_Setup.getEncoderSpeedFast();
+		targetDistance = max(0.0f, targetDistance + increment / 100 * sign);
+		display.setDistanceValue(targetDistance);
+		display.setDistanceUnit("mm");
+		display.setRefreshScreen();
+	}
 }

RouterSetup.cpp

 }
 
 void RouterSetup::onRotaryControlerLongSwitch() {
+	save();
+	doInitialization = true;
+}
+
+void RouterSetup::save() {
 	saveToEEPROM();
+}
+
+void RouterSetup::cancel() {
 	doInitialization = true;
 }
 
+
 void RouterSetup::onRotaryControlerTurn(RotaryEncoder::Direction turn) {
 
 	if (turn != RotaryEncoder::Direction::NOROTATION) {
 
 		switch (configStepIndex) {
 			case 0:
-				setSpeed(getSpeed() + 0.1 * sign);
+				setSpeed(max(0.0, getSpeed() + 0.1 * sign));
 				value = String(getSpeed(), 1);
 				break;
 			case 1:
-				setAcceleration(getAcceleration() + 0.1 * sign);
+				setAcceleration(max(0.0, getAcceleration() + 0.1 * sign));
 				value = String(getAcceleration(), 1);
 				break;
 			case 2:
 				}
 				break;
 			case 3:
-				setPitch(getPitch() + 0.1 * sign);
+				setPitch(max(0.0, getPitch() + 0.1 * sign));
 				value = String(getPitch(), 1);
 				break;
 			case 4:
-				setToolLenghtSensorHeight(getToolLenghtSensorHeight() + 0.01 * sign);
+				setToolLenghtSensorHeight(max(0.0, getToolLenghtSensorHeight() + 0.01 * sign));
 				value = String(getToolLenghtSensorHeight(), 2);
 				break;
 			case 5:
-				setEncoderSpeedSlow(getEncoderSpeedSlow() + 1 * sign);
+				setEncoderSpeedSlow(min(max(1, getEncoderSpeedSlow() + 1 * sign), 9));
 				value = String(getEncoderSpeedSlow());
 				break;
 			case 6:
-				setEncoderSpeedFast(getEncoderSpeedFast() + 1 * sign);
+				setEncoderSpeedFast(min(max(10, getEncoderSpeedFast() + 1 * sign), 100));
 				value = String(getEncoderSpeedFast());
 				break;
 			case 7:
-				setLevelHeightDiving(getLevelHeightDiving() + 0.1 * sign);
+				setLevelHeightDiving(min(max(0.1, getLevelHeightDiving() + 0.1 * sign), 10.0));
 				value = String(getLevelHeightDiving(), 1);
 				break;
 			case 8:
 	}
 }
 
+//***************************************************************
+//*********** Private methods ***********************************
+//***************************************************************
 String RouterSetup::getCfgOptForStepIndex(byte configStepIndex) {
 	String value = "";
 	String unit = getCfgOptUnitForStepIndex(configStepIndex);
 	}
 	return -1;
 }
+

RouterSetup.h

 		void onRotaryControlerSwitch();
 		void onRotaryControlerLongSwitch();
 		void onRotaryControlerTurn(RotaryEncoder::Direction turn);
+		void save();
+		void cancel();
 
 		void printValues();
 

ValueMode.cpp

 bool ValueMode::equals(ValueMode valueMode) {
 	return this->valueMode == valueMode.getValueMode();
 }
+
+bool ValueMode::isSlow() {
+	return valueMode == Mode::SLOW;
+}

ValueMode.h

 		Mode getValueMode() const;
 		void setValueMode(Mode valueMode);
 		bool equals(ValueMode valueMode);
+		bool isSlow();
 };
 
 #endif /* VALUEMODE_H_ */

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-23 22:58:18
+//This file has been generated on 2022-02-25 22:27:34
 
 #include "Arduino.h"
 #include <Arduino.h>