diff --git a/Grbl_Esp32/nuts_bolts.cpp b/Grbl_Esp32/nuts_bolts.cpp
index 983820e3..482633be 100644
--- a/Grbl_Esp32/nuts_bolts.cpp
+++ b/Grbl_Esp32/nuts_bolts.cpp
@@ -164,17 +164,24 @@ float limit_value_by_axis_maximum(float *max_value, float *unit_vec)
   return(limit_value);
 }
 
-// int constrain(int val, int min, int max) {
-    // return (val < min)? min : (val > max)? max : val;    
-// }
-
-// long map(long x, long in_min, long in_max, long out_min, long out_max)
-// {
-  // return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-// }
-
-long mapConstrain(long x, long in_min, long in_max, long out_min, long out_max)
+float map_float(float x, float in_min, float in_max, float out_min, float out_max) // DrawBot_Badge
 {
-    x = constrain(x, out_min, out_max);
-    return map(x, in_min, in_max, out_min, out_max);
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+float constrain_float(float in, float min, float max) // DrawBot_Badge
+{
+	if (in < min)
+		return min;
+	
+	if (in > max)
+		return max;
+	
+	return in;
+}
+
+float mapConstrain(float x, float in_min, float in_max, float out_min, float out_max)
+{
+    x = constrain_float(x, in_min, in_max);
+    return map_float(x, in_min, in_max, out_min, out_max);
 }
diff --git a/Grbl_Esp32/nuts_bolts.h b/Grbl_Esp32/nuts_bolts.h
index 6671ca39..7a6facb3 100644
--- a/Grbl_Esp32/nuts_bolts.h
+++ b/Grbl_Esp32/nuts_bolts.h
@@ -80,8 +80,8 @@ float hypot_f(float x, float y);
 float convert_delta_vector_to_unit_vector(float *vector);
 float limit_value_by_axis_maximum(float *max_value, float *unit_vec);
 
-//int constrain(int val, int min, int max);
-//long map(long x, long in_min, long in_max, long out_min, long out_max);
-long mapConstrain(long x, long in_min, long in_max, long out_min, long out_max);
+float mapConstrain(float x, float in_min, float in_max, float out_min, float out_max);
+float map_float(float x, float in_min, float in_max, float out_min, float out_max);
+float constrain_float(float in, float min, float max);
 
 #endif
diff --git a/Grbl_Esp32/servo_pen.cpp b/Grbl_Esp32/servo_pen.cpp
index 81cad74f..e658d058 100644
--- a/Grbl_Esp32/servo_pen.cpp
+++ b/Grbl_Esp32/servo_pen.cpp
@@ -32,8 +32,8 @@ void servo_init()
 	//validate_servo_settings(true); // display any calibration errors
 
 	// Debug stuff
-	grbl_sendf(CLIENT_SERIAL, "[MSG:Servo max,min pulse times %.4f sec,%.4f sec]\r\n", SERVO_MAX_PULSE_SEC, SERVO_MIN_PULSE_SEC);
-	grbl_sendf(CLIENT_SERIAL, "[MSG:Servo max,min pulse counts %d,%d]\r\n", SERVO_MAX_PULSE, SERVO_MIN_PULSE);
+	//grbl_sendf(CLIENT_SERIAL, "[MSG:Servo max,min pulse times %.4f sec,%.4f sec]\r\n", SERVO_MAX_PULSE_SEC, SERVO_MIN_PULSE_SEC);
+	//grbl_sendf(CLIENT_SERIAL, "[MSG:Servo max,min pulse counts %d,%d]\r\n", SERVO_MAX_PULSE, SERVO_MIN_PULSE);
 	validate_servo_settings(true); // will print errors
 	// debug stuff
 
@@ -93,11 +93,14 @@ bool validate_servo_settings(bool verbose) // make sure the settings are reasona
 // this is the task
 void servoSyncTask(void *pvParameters)
 {
-	int32_t current_position[N_AXIS]; // copy of current location
-	float m_pos[N_AXIS];   // machine position in mm
+	//int32_t current_position[N_AXIS]; // copy of current location
+	//float m_pos[N_AXIS];   // machine position in mm
 	TickType_t xLastWakeTime;
 	const TickType_t xServoFrequency = SERVO_TIMER_INT_FREQ;  // in ticks (typically ms)
 	uint16_t servo_delay_counter = 0;
+	
+	float mpos_z, wpos_z;
+	float z_offset;
 
 	while(true) { // don't ever return from this or the task dies
 
@@ -106,16 +109,12 @@ void servoSyncTask(void *pvParameters)
 		if (!servo_pen_enable) {
 			servo_delay_counter++;
 			servo_pen_enable = (servo_delay_counter > SERVO_TURNON_DELAY);
-		} else {
-			if(!stepper_idle) {
-				memcpy(current_position,sys_position,sizeof(sys_position));  // get current position in step
-				system_convert_array_steps_to_mpos(m_pos,current_position); // convert to millimeters
-
-				calc_pen_servo(m_pos[Z_AXIS]); // calculate kinematics and move the servos
-			} else {
-				servo_disable();
-			}
+		} else {			
+				mpos_z = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS);  // get the machine Z in mm
+				z_offset = gc_state.coord_system[Z_AXIS]+gc_state.coord_offset[Z_AXIS]; // get the current z work offset
+				wpos_z = mpos_z - z_offset; // determine the current work Z			
 
+				calc_pen_servo(wpos_z); // calculate kinematics and move the servos
 		}
 	}
 }
@@ -133,7 +132,6 @@ void calc_pen_servo(float penZ)
 	if (validate_servo_settings(false)) { // if calibration settings are OK then apply them
 		// Apply a calibration to the minimum position
 		servo_pen_pulse_min = SERVO_MIN_PULSE * (settings.steps_per_mm[Z_AXIS] / 100.0);
-
 		// Apply a calibration to the maximum position
 		servo_pen_pulse_max = SERVO_MAX_PULSE * (settings.max_travel[Z_AXIS] / -100.0);
 	} else { // use the defaults
@@ -142,10 +140,12 @@ void calc_pen_servo(float penZ)
 	}
 
 	// determine the pulse length
-	servo_pen_pulse_len = mapConstrain(penZ, SERVO_PEN_RANGE_MIN_MM, SERVO_PEN_RANGE_MAX_MM, servo_pen_pulse_min, servo_pen_pulse_max );
-
-	// TODO only update it if it has changed
+	servo_pen_pulse_len = (uint32_t)mapConstrain(penZ, SERVO_PEN_RANGE_MIN_MM, SERVO_PEN_RANGE_MAX_MM, servo_pen_pulse_min, servo_pen_pulse_max );
 
+	// skip setting value if it is unchanged
+	if (ledcRead(SERVO_PEN_CHANNEL_NUM) == servo_pen_pulse_len)
+		return;
+  	
 	// update the PWM value
 	// ledcWrite appears to have issues with interrupts, so make this a critical section
 	portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED;
diff --git a/Grbl_Esp32/servo_pen.h b/Grbl_Esp32/servo_pen.h
index 7ab59165..bc996ec0 100644
--- a/Grbl_Esp32/servo_pen.h
+++ b/Grbl_Esp32/servo_pen.h
@@ -24,8 +24,6 @@
 	Everything occurs as a low priority task that syncs the servo with the
 	current machine position.
 
-	TODO Make it reversible (turn opposite way)
-
 */
 
 // ==== Begin: Things you are likely to change ====================