Use library for smooth pen servo move.

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);
 	}
 }
 

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()

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;