Oddly enough I solved this problem today. I’ve made a new file in the gcode parser called PenServo. Here’s an overview:
The function MoveServoToDegreesWithDelay will move the servo in 1 degree steps from where it is to where it needs to go. It pauses between each step for n milliseconds. The pause is programmable by setting ikServoDelayMsMax.
// PenServo.Ino Functions and variables for handling the servo which drives the pen up an down.
// 2016-11-05 : Started
#include <Servo.h>
Servo ZServo; // Create the servo object
static float ServoDegrees = 0.0 ; // This is a copy of what we hope the Servo is at
static float kOffPaperDegrees = 60.0 ;
void LiftPen ()
{
ZServo.write(kOffPaperDegrees);
}
void MoveServoToDegreesDirect (const float kDegs)
{
ZServo.write(kDegs);
ServoDegrees = kDegs ;
}
// These help set the speed of the servo
static int iServoMsDelayPerDeg = 10 ;
static const int ikServoDelayMsMax = 1000 ;
void MoveServoToDegreesWithDelay (const float kDegs)
{
Serial.print("move servo with delay =");
Serial.print(iServoMsDelayPerDeg) ;
// How big is the arc we have to move through?
const double kDeltaDegs = kDegs - ServoDegrees ;
// We start here, from where the servo is currently at
double IntermediateDegs = ServoDegrees ;
if (kDeltaDegs > 0) {
while (IntermediateDegs < kDegs) {
delay (iServoMsDelayPerDeg) ;
ZServo.write(IntermediateDegs);
IntermediateDegs += 1.0 ;
}
} else if (kDeltaDegs < 0) {
while (IntermediateDegs > kDegs) {
delay (iServoMsDelayPerDeg) ;
ZServo.write(IntermediateDegs);
IntermediateDegs -= 1.0 ;
}
} else {
// No movement required, do nothing
}
ServoDegrees = kDegs ;
}
void SetZServoDelayMs (const int ikMsDelay)
{
if ((ikMsDelay > 0) && (ikMsDelay < ikServoDelayMsMax)) {
iServoMsDelayPerDeg = ikMsDelay ;
}
}