From b89fd8363c846caaa89ccbdee62b99344ca7b428 Mon Sep 17 00:00:00 2001
From: Holger Weber <hotzze@gmx.de>
Date: Thu, 26 Feb 2026 23:20:50 +0100
Subject: [PATCH] Use library for smooth pen servo move.

---
 include/EggDuino.h       |  8 +++---
 platformio.ini           |  2 ++
 src/Functions.cpp        | 10 +++----
 src/Helper_Functions.cpp | 57 ++++++++++++++++++++++++++++++++++++++--
 src/main.cpp             |  7 ++---
 5 files changed, 69 insertions(+), 15 deletions(-)

diff --git a/include/EggDuino.h b/include/EggDuino.h
index 8eb09a8..2655f44 100644
--- a/include/EggDuino.h
+++ b/include/EggDuino.h
@@ -2,13 +2,12 @@
 #define EGGDUINO_H
 
 #include <Arduino.h>
+#define SUPPRESS_HPP_WARNING
+#include <ServoEasing.h>
 #ifdef ESP32
-#include <ESP32Servo.h>
 #include <WiFi.h>
 #include <WebServer.h>
 #include <SPIFFS.h>
-#else
-#include <Servo.h>
 #endif
 
 #include <FastAccelStepper.h>
@@ -72,7 +71,7 @@ extern FastAccelStepperEngine g_stepEngine;
 extern FastAccelStepper *g_pStepperRotate;
 extern FastAccelStepper *g_pStepperPen;
 
-extern Servo penServo;
+extern ServoEasing penServo;
 extern SerialCommand SCmd;
 #ifdef ESP32
 extern SerialCommand g_BLECmd;
@@ -134,6 +133,7 @@ void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB);
 void storePenUpPosInEE();
 void storePenDownPosInEE();
 void updateStepCorrectionFactors();
+void movePenServoTo(int targetPos);
 
 bool initConfigStore();
 bool loadConfigFromFile();
diff --git a/platformio.ini b/platformio.ini
index 16dfa7b..543d5ac 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -16,6 +16,7 @@ monitor_speed = 115200
 upload_speed = 576000
 upload_port = /dev/ttyUSB*
 lib_deps = 
+	arminjo/ServoEasing
 	madhephaestus/ESP32Servo@^3.0.6
 	bblanchon/ArduinoJson@^6.21.5
 	gin66/FastAccelStepper@^0.33.13
@@ -31,6 +32,7 @@ monitor_port = /dev/cu.usb*
 upload_speed = 115200
 upload_port = /dev/cu.usb*
 lib_deps = 
+	arminjo/ServoEasing
 	madhephaestus/ESP32Servo@^3.0.6
 	bblanchon/ArduinoJson@^6.21.5
 	gin66/FastAccelStepper@^0.33.13
diff --git a/src/Functions.cpp b/src/Functions.cpp
index 828dcee..db236d2 100644
--- a/src/Functions.cpp
+++ b/src/Functions.cpp
@@ -119,11 +119,11 @@ void setPen()
 		switch (cmd)
 		{
 		case 0:
-			penServo.write(g_iPenUpPos);
+			movePenServoTo(g_iPenUpPos);
 			break;
 
 		case 1:
-			penServo.write(g_iPenDownPos);
+			movePenServoTo(g_iPenDownPos);
 			break;
 
 		default:
@@ -172,13 +172,11 @@ void doTogglePen()
    Log(__FUNCTION__);
 	if (g_iPenState == g_iPenUpPos)
 	{
-		penServo.write(g_iPenDownPos);
-		g_iPenState = g_iPenDownPos;
+		movePenServoTo(g_iPenDownPos);
 	}
 	else
 	{
-		penServo.write(g_iPenUpPos);
-		g_iPenState = g_iPenUpPos;
+		movePenServoTo(g_iPenUpPos);
 	}
 }
 
diff --git a/src/Helper_Functions.cpp b/src/Helper_Functions.cpp
index 2ae532b..7291992 100644
--- a/src/Helper_Functions.cpp
+++ b/src/Helper_Functions.cpp
@@ -1,5 +1,41 @@
 #include "EggDuino.h"
 
+namespace
+{
+int clampServoAngle(int angle)
+{
+	if (angle < 0)
+	{
+		return 0;
+	}
+	if (angle > 180)
+	{
+		return 180;
+	}
+	return angle;
+}
+
+uint_fast16_t servoSpeedFromRate(int rate)
+{
+	// EBB rate values are implementation-specific. We map them to ServoEasing degrees/second.
+	// Higher rate means faster movement.
+	if (rate <= 0)
+	{
+		return 70;
+	}
+	int speed = 20 + (rate / 2);
+	if (speed < 10)
+	{
+		speed = 10;
+	}
+	if (speed > 360)
+	{
+		speed = 360;
+	}
+	return (uint_fast16_t)speed;
+}
+}
+
 void updateStepCorrectionFactors()
 {
 	if (g_iRotMicrostep <= 0)
@@ -50,8 +86,25 @@ void initHardware()
 	}
 
 	motorsOff();
-	penServo.attach(g_iServoPin);
-	penServo.write(g_iPenState);
+	g_iPenState = clampServoAngle(g_iPenState);
+	penServo.attach(g_iServoPin, g_iPenState);
+	penServo.setEasingType(EASE_QUADRATIC_IN_OUT);
+}
+
+void movePenServoTo(int targetPos)
+{
+	targetPos = clampServoAngle(targetPos);
+	int currentPos = clampServoAngle(g_iPenState);
+	if (currentPos == targetPos)
+	{
+		return;
+	}
+
+	uint_fast16_t speed = 0;
+	speed = servoSpeedFromRate(g_iServoRateDown);
+	penServo.easeTo(targetPos, speed);
+
+	g_iPenState = targetPos;
 }
 
 void storePenUpPosInEE()
diff --git a/src/main.cpp b/src/main.cpp
index daaf00c..5593826 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "EggDuino.h"
+#include <ServoEasing.hpp>
 
 // EXTRAFEATURES - UNCOMMENT TO USE THEM -------------------------------------------------------------------
 
@@ -34,7 +35,7 @@ FastAccelStepper *g_pStepperRotate = NULL;
 FastAccelStepper *g_pStepperPen = NULL;
 
 // make Objects
-Servo penServo;
+ServoEasing penServo;
 SerialCommand SCmd;
 #ifdef ESP32
 SerialCommand g_BLECmd;
@@ -61,8 +62,8 @@ Button motorsToggle(motorsButton, toggleMotors);
 // Variables... be careful, by messing around here, everything has a reason and crossrelations...
 int g_iPenUpPos = 5;	   // can be overwritten from EBB-Command SC
 int g_iPenDownPos = 20;   // can be overwritten from EBB-Command SC
-int g_iServoRateUp = 0;   // from EBB-Protocol not implemented on machine-side
-int g_iServoRateDown = 0; // from EBB-Protocol not implemented on machine-side
+int g_iServoRateUp = 0;   // from EBB-Protocol, mapped to ServoEasing speed (up movement)
+int g_iServoRateDown = 0; // from EBB-Protocol, mapped to ServoEasing speed (down movement)
 long g_iRotStepError = 0;
 long g_iPenStepError = 0;
 int g_iPenState = g_iPenUpPos;