Support for modulated IR sensor

Add code to support sensors that allow the IR output to be modulated, to
reduce the sensitivity to ambient IR. Use M558 P2 command to enable
sensor modulation. Also changed the sensor reading averaging code to
give more consistent z-height seeking.

GCodes.cpp

@@ -174,7 +174,7 @@ void GCodes::Spin()
 
 	  if(platform->GetLine()->Status() & byteAvailable)
 	  {
-		  // Read several bytes instead of just one. This is mainly to speed up file uploading.
+		  // Read several bytes instead of just one. This approximately doubles the speed of file uploading.
 		  int8_t i = 0;
 		  do
 		  {
@@ -815,8 +815,14 @@ bool GCodes::SetPrintZProbe(GCodeBuffer* gb, char* reply)
 		{
 			platform->SetZProbe(gb->GetIValue());
 		}
-	} else
+	} else if (platform->GetZProbeType() == 2)
+	{
+		snprintf(reply, STRING_LENGTH, "%d (%d)", platform->ZProbe(), platform->ZProbeOnVal());
+	}
+	else
+	{
 		snprintf(reply, STRING_LENGTH, "%d", platform->ZProbe());
+	}
 	return true;
 }
 
@@ -1621,7 +1627,13 @@ bool GCodes::ActOnGcode(GCodeBuffer *gb)
 
     case 558: // Set Z probe type
     	if(gb->Seen('P'))
+    	{
     		platform->SetZProbeType(gb->GetIValue());
+    	}
+    	else
+    	{
+    		snprintf(reply, STRING_LENGTH, "%d", platform->GetZProbeType());
+    	}
     	break;
 
     case 559: // Upload config.g

Platform.cpp

@@ -97,12 +97,15 @@ void Platform::Init()
   potWipes = POT_WIPES;
   senseResistor = SENSE_RESISTOR;
   maxStepperDigipotVoltage = MAX_STEPPER_DIGIPOT_VOLTAGE;
-  zProbePin = -1; // Default is to use the switch
+
-  zProbeCount = 0;
+  // Z PROBE
-  zProbeSum = 0;
+
-  zProbeValue = 0;
+  zProbePin = Z_PROBE_PIN;
+  zProbeModulationPin = Z_PROBE_MOD_PIN;
+  zProbeType = 0;	// Default is to use the switch
   zProbeADValue = Z_PROBE_AD_VALUE;
   zProbeStopHeight = Z_PROBE_STOP_HEIGHT;
+  InitZProbe();
 
   // AXES
 
@@ -188,9 +191,6 @@ void Platform::Init()
     thermistorInfRs[i] = ( thermistorInfRs[i]*exp(-thermistorBetas[i]/(25.0 - ABS_ZERO)) );
   }
 
-  if(zProbePin >= 0)
-	  pinMode(zProbePin, INPUT);
-  
   if(coolingFanPin >= 0)
   {
 	  pinMode(coolingFanPin, OUTPUT);
@@ -207,13 +207,28 @@ void Platform::Init()
   active = true;
 }
 
+void Platform::InitZProbe()
+{
+  zProbeCount = 0;
+  zProbeOnSum = 0;
+  zProbeOffSum = 0;
+  for (uint8_t i = 0; i < NumZProbeReadingsAveraged; ++i)
+  {
+	  zProbeReadings[i] = 0;
+  }
+
+  if (zProbeType != 0)
+  {
+	pinMode(zProbeModulationPin, OUTPUT);
+	digitalWrite(zProbeModulationPin, HIGH);	// enable the IR LED
+  }
+}
+
 void Platform::StartNetwork()
 {
 	network->Init();
 }
 
-
-
 void Platform::Spin()
 {
   if(!active)
@@ -345,7 +360,7 @@ void Platform::SetHeater(int8_t heater, const float& power)
 
 EndStopHit Platform::Stopped(int8_t drive)
 {
-	if(zProbePin >= 0)
+	if(zProbeType > 0)
 	{  // Z probe is used for both X and Z.
 		if(drive != Y_AXIS)
 		{
@@ -841,6 +856,26 @@ void Line::Init()
 	//while (!SerialUSB.available());
 }
 
+void Line::Spin()
+{
+	// Read the serial data in blocks to avoid excessive flow control
+	if (numChars <= lineBufsize/2)
+	{
+		int16_t target = SerialUSB.available() + (int16_t)numChars;
+		if (target > lineBufsize)
+		{
+			target = lineBufsize;
+		}
+		while ((int16_t)numChars < target)
+		{
+			int incomingByte = SerialUSB.read();
+			if (incomingByte < 0) break;
+			buffer[(getIndex + numChars) % lineBufsize] = (char)incomingByte;
+			++numChars;
+		}
+	}
+}
+
 //***************************************************************************************************
 
 // Network/Ethernet class
@@ -1232,27 +1267,6 @@ void NetRing::ReleaseHs()
 	hs = 0;
 }
 
-void Line::Spin()
-{
-	// Read the serial data in blocks to avoid excessive flow control
-	if (numChars <= lineBufsize/2)
-	{
-		int16_t target = SerialUSB.available() + (int16_t)numChars;
-		if (target > lineBufsize)
-		{
-			target = lineBufsize;
-		}
-		while ((int16_t)numChars < target)
-		{
-			int incomingByte = SerialUSB.read();
-			if (incomingByte < 0) break;
-			buffer[(getIndex + numChars) % lineBufsize] = (char)incomingByte;
-			++numChars;
-		}
-	}
-}
-
-
 
 
 

Platform.h

@@ -91,9 +91,10 @@ Licence: GPL
 #define POT_WIPES {1, 3, 2, 0} // Indices for motor current digipots (if any)
 #define SENSE_RESISTOR 0.1   // Stepper motor current sense resistor
 #define MAX_STEPPER_DIGIPOT_VOLTAGE ( 3.3*2.5/(2.7+2.5) ) // Stepper motor current reference voltage
-#define Z_PROBE_AD_VALUE 400
+#define Z_PROBE_AD_VALUE (400)
-#define Z_PROBE_STOP_HEIGHT 0.7 // mm
+#define Z_PROBE_STOP_HEIGHT (0.7) // mm
-#define Z_PROBE_PIN 0 // Analogue pin number
+#define Z_PROBE_PIN (0) 		// Analogue pin number
+#define Z_PROBE_MOD_PIN (61)	// Digital pin number to turn the IR LED on (high) or off (low)
 #define MAX_FEEDRATES {50.0, 50.0, 3.0, 16.0}    // mm/sec
 #define ACCELERATIONS {800.0, 800.0, 10.0, 250.0}    // mm/sec^2
 #define DRIVE_STEPS_PER_UNIT {87.4890, 87.4890, 4000.0, 420.0}
@@ -184,6 +185,7 @@ Licence: GPL
 #define BAUD_RATE 115200 // Communication speed of the USB if needed.
 
 const uint16_t lineBufsize = 256;				// use a power of 2 for good performance
+const uint16_t NumZProbeReadingsAveraged = 8;	// must be an even number, preferably a power of 2 for performance, and no greater than 64
 
 /****************************************************************************************************/
 
@@ -326,6 +328,8 @@ protected:
 	void Spin();
 
 private:
+	// Although the sam3x usb interface code already has a 512-byte buffer, adding this extra 256-byte buffer
+	// increases the speed of uploading to the SD card by 10%
 	char buffer[lineBufsize];
 	uint16_t getIndex;
 	uint16_t numChars;
@@ -481,9 +485,11 @@ class Platform
   
   float ZProbeStopHeight();
   void SetZProbeStopHeight(float z);
-  long ZProbe();
+  int ZProbe() const;
+  int ZProbeOnVal() const;
   void SetZProbe(int iZ);
   void SetZProbeType(int iZ);
+  int GetZProbeType() const;
 
   // Heat and temperature
   
@@ -539,15 +545,19 @@ class Platform
   float maxStepperDigipotVoltage;
 //  float zProbeGradient;
 //  float zProbeConstant;
-  long zProbeValue;
   int8_t zProbePin;
-  int8_t zProbeCount;
+  int8_t zProbeModulationPin;
-  long zProbeSum;
+  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 zProbeReadings[NumZProbeReadingsAveraged];
   int zProbeADValue;
   float zProbeStopHeight;
 
 // AXES
 
+  void InitZProbe();
   void PollZHeight();
 
   float axisLengths[AXES];
@@ -804,14 +814,25 @@ inline void Platform::SetMaxFeedrate(int8_t drive, float value)
 
 inline int Platform::GetRawZHeight()
 {
-  if(zProbePin >= 0)
+  return (zProbeType != 0) ? analogRead(zProbePin) : 0;
-    return analogRead(zProbePin);
-  return 0;
 }
 
-inline long Platform::ZProbe()
+inline int Platform::ZProbe() const
 {
-	return zProbeValue;
+	return (zProbeType == 1)
+			? (zProbeOnSum + zProbeOffSum)/NumZProbeReadingsAveraged		// non-modulated mode
+			: (zProbeType == 2)
+			  ? (zProbeOnSum - zProbeOffSum)/(NumZProbeReadingsAveraged/2)	// modulated mode
+			    : 0;														// z-probe disabled
+}
+
+inline int Platform::ZProbeOnVal() const
+{
+	return (zProbeType == 1)
+			? (zProbeOnSum + zProbeOffSum)/NumZProbeReadingsAveraged
+			: (zProbeType == 2)
+			  ? zProbeOnSum/(NumZProbeReadingsAveraged/2)
+				: 0;
 }
 
 inline float Platform::ZProbeStopHeight()
@@ -831,22 +852,33 @@ inline void Platform::SetZProbe(int iZ)
 
 inline void Platform::SetZProbeType(int pt)
 {
-	if(pt != 0)
+	zProbeType = (pt >= 0 && pt <= 2) ? pt : 0;
-		zProbePin = Z_PROBE_PIN;
+	InitZProbe();
-	else
+}
-		zProbePin = -1;
+
+inline int Platform::GetZProbeType() const
+{
+	return zProbeType;
 }
 
 inline void Platform::PollZHeight()
 {
-	if(zProbeCount >= 5)
+	uint16_t currentReading = GetRawZHeight();
+	if (zProbeType == 2)
 	{
-		zProbeValue = zProbeSum/5;
+		// Reverse the modulation, ready for next time
-		zProbeSum = 0;
+		digitalWrite(zProbeModulationPin, (zProbeCount & 1) ? HIGH : LOW);
-		zProbeCount = 0;
 	}
-	zProbeSum += GetRawZHeight();
+	if (zProbeCount & 1)
-	zProbeCount++;
+	{
+		zProbeOffSum = zProbeOffSum - zProbeReadings[zProbeCount] + currentReading;
+	}
+	else
+	{
+		zProbeOnSum = zProbeOnSum - zProbeReadings[zProbeCount] + currentReading;
+	}
+	zProbeReadings[zProbeCount] = currentReading;
+	zProbeCount = (zProbeCount + 1) % NumZProbeReadingsAveraged;
 }