diff --git a/Platform.cpp b/Platform.cpp
index 52494a7..38fb0b2 100644
--- a/Platform.cpp
+++ b/Platform.cpp
@@ -222,7 +222,8 @@ void Platform::Init()
 
 void Platform::InitZProbe()
 {
-  zProbeCount = 0;
+//  zProbeCount = 0;
+  zModOnThisTime = true;
   zProbeOnSum = 0;
   zProbeOffSum = 0;
 
@@ -231,7 +232,7 @@ void Platform::InitZProbe()
 	pinMode(zProbeModulationPin, OUTPUT);
 	digitalWrite(zProbeModulationPin, HIGH);	// enable the IR LED
   }
-  LastZProbeReading = GetRawZHeight();
+//  LastZProbeReading = GetRawZHeight();
 }
 
 
diff --git a/Platform.h b/Platform.h
index d9f5406..11a4419 100644
--- a/Platform.h
+++ b/Platform.h
@@ -503,11 +503,11 @@ class Platform
 
   float ZProbeStopHeight() const;		// The height above the bed at which the probe is triggered
   void SetZProbeStopHeight(float z);	// Set the height above the bed at which the probe is triggered
-  int ZProbe() const;					// Get an A->D value from the probe.  This may have been averaged etc
-  int ZProbeOnVal() const;				// Returns on value for a modulated probe; total for non-modulated
-  void SetZProbe(int iZ);				// Set the A->D value corresponding to SetZProbeStopHeight()
-  void SetZProbeType(int iZ);			// None (0), non-modulating (1) or modulating (2)
-  int GetZProbeType() const;			// Get the probe type: None (0), non-modulating (1) or modulating (2)
+  unsigned int ZProbe() const;			// Get an A->D value from the probe.  This may have been averaged etc
+  unsigned int ZProbeOnVal() const;		// Returns on value for a modulated probe; total for non-modulated
+  void SetZProbe(unsigned int iZ);		// Set the A->D value corresponding to SetZProbeStopHeight()
+  void SetZProbeType(int8_t iZ);		// None (0), non-modulating (1) or modulating (2)
+  int8_t GetZProbeType() const;			// Get the probe type: None (0), non-modulating (1) or modulating (2)
 
   // Heat and temperature
   
@@ -564,18 +564,19 @@ class Platform
 
   // Z probe
 
+  void InitZProbe();
+  void PollZHeight();
   int8_t zProbePin;
   int8_t zProbeModulationPin;
   int8_t zProbeType;
-  uint8_t zProbeCount;
-  long zProbeOnSum;		// sum of readings taken when IR led is on
-  long zProbeOffSum;	// sum of readings taken when IR led is on
-  uint16_t LastZProbeReading;
+//  uint8_t zProbeCount;
+  bool zModOnThisTime;
+  unsigned long zProbeOnSum;		// sum of readings taken when IR led is on
+  unsigned long zProbeOffSum;	// sum of readings taken when IR led is on
+//  uint16_t LastZProbeReading;
   int zProbeADValue;
   float zProbeStopHeight;
   bool zProbeEnable;
-  void InitZProbe();
-  void PollZHeight();
 
   // AXES
 
@@ -823,7 +824,7 @@ inline int Platform::GetRawZHeight() const
   return (zProbeType != 0) ? analogRead(zProbePin) : 0;
 }
 
-inline int Platform::ZProbe() const
+inline unsigned int Platform::ZProbe() const
 {
 	return (zProbeType == 1)
 			? (zProbeOnSum + zProbeOffSum)/NumZProbeReadingsAveraged		// non-modulated mode
@@ -832,7 +833,7 @@ inline int Platform::ZProbe() const
 			    : 0;														// z-probe disabled
 }
 
-inline int Platform::ZProbeOnVal() const
+inline unsigned int Platform::ZProbeOnVal() const
 {
 	return (zProbeType == 1)
 			? (zProbeOnSum + zProbeOffSum)/NumZProbeReadingsAveraged
@@ -841,6 +842,25 @@ inline int Platform::ZProbeOnVal() const
 				: 0;
 }
 
+inline void Platform::PollZHeight()
+{
+	uint16_t currentReading = GetRawZHeight();
+	// Compute a moving average
+	if (zModOnThisTime)
+		zProbeOnSum = zProbeOnSum + currentReading - zProbeOnSum/NumZProbeReadingsAveraged;
+	else
+		zProbeOffSum = zProbeOffSum + currentReading - zProbeOffSum/NumZProbeReadingsAveraged;
+//	LastZProbeReading = currentReading;
+//	zProbeCount = (zProbeCount + 1) % NumZProbeReadingsAveraged;
+	zModOnThisTime = !zModOnThisTime;
+	if (zProbeType == 2)
+	{
+		// Reverse the modulation, ready for next time
+		//digitalWrite(zProbeModulationPin, (zProbeCount & 1) ? HIGH : LOW);
+		digitalWrite(zProbeModulationPin, zModOnThisTime ? HIGH : LOW);
+	}
+}
+
 inline float Platform::ZProbeStopHeight() const
 {
 	return zProbeStopHeight;
@@ -851,41 +871,22 @@ inline void Platform::SetZProbeStopHeight(float z)
 	zProbeStopHeight = z;
 }
 
-inline void Platform::SetZProbe(int iZ)
+inline void Platform::SetZProbe(unsigned int iZ)
 {
 	zProbeADValue = iZ;
 }
 
-inline void Platform::SetZProbeType(int pt)
+inline void Platform::SetZProbeType(int8_t pt)
 {
 	zProbeType = (pt >= 0 && pt <= 2) ? pt : 0;
 	InitZProbe();
 }
 
-inline int Platform::GetZProbeType() const
+inline int8_t Platform::GetZProbeType() const
 {
 	return zProbeType;
 }
 
-inline void Platform::PollZHeight()
-{
-	uint16_t currentReading = GetRawZHeight();
-	if (zProbeType == 2)
-	{
-		// Reverse the modulation, ready for next time
-		digitalWrite(zProbeModulationPin, (zProbeCount & 1) ? HIGH : LOW);
-	}
-	if (zProbeCount & 1)
-	{
-		zProbeOffSum = zProbeOffSum - LastZProbeReading + currentReading;
-	}
-	else
-	{
-		zProbeOnSum = zProbeOnSum - LastZProbeReading + currentReading;
-	}
-	LastZProbeReading = currentReading;
-	zProbeCount = (zProbeCount + 1) % NumZProbeReadingsAveraged;
-}
 
 
 //********************************************************************************************************
diff --git a/RepRapFirmware.cpp b/RepRapFirmware.cpp
index 8e1c92b..174ad29 100644
--- a/RepRapFirmware.cpp
+++ b/RepRapFirmware.cpp
@@ -193,9 +193,8 @@ void RepRap::Init()
   platform->Message(HOST_MESSAGE, "\n");
   platform->Message(HOST_MESSAGE, NAME);
   platform->Message(HOST_MESSAGE, " is up and running.\n");
+  ResetLoopTimers();
   lastTime = platform->Time();
-  fastTime = FLT_MAX;
-  slowTime = 0;
 }
 
 void RepRap::Exit()
@@ -223,7 +222,7 @@ void RepRap::Spin()
   // Track the loop time
 
   float t = platform->Time();
-  float d = t = lastTime;
+  float d = t - lastTime;
   if(d < fastTime)
 	  fastTime = d;
   if(d > slowTime)
@@ -240,6 +239,7 @@ void RepRap::Diagnostics()
   webserver->Diagnostics();
   snprintf(scratchString, STRING_LENGTH, "Slowest loop time was %f secs; fastest was %f secs.\n", slowTime, fastTime);
   platform->Message(HOST_MESSAGE, scratchString);
+  ResetLoopTimers();
 }
 
 // Turn off the heaters, disable the motors, and
diff --git a/Reprap.h b/Reprap.h
index 6e283dd..1c07506 100644
--- a/Reprap.h
+++ b/Reprap.h
@@ -39,6 +39,7 @@ class RepRap
     Heat* GetHeat() const;				// Get the instance of the class that handles all heat and temperature
     GCodes* GetGCodes() const;			// Get the instance of the class that handles G Codes from all sources
     Webserver* GetWebserver() const;	// Get the instance of the class that handles Web traffic
+    void ResetLoopTimers();				// Reset the max and min for loop diagnostics
     
   private:
   
@@ -58,6 +59,7 @@ inline Heat* RepRap::GetHeat() const { return heat; }
 inline GCodes* RepRap::GetGCodes() const { return gCodes; }
 inline Webserver* RepRap::GetWebserver() const { return webserver; }
 inline bool RepRap::Debug() const { return debug; }
+inline void RepRap::ResetLoopTimers() { fastTime = FLT_MAX; slowTime = 0.0; }
 
 inline void RepRap::SetDebug(bool d)
 {