Refactored active/standby tool and heater setting code to save space

10 files changed, 57 insertions, 152 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index c233b5d5..ee9c1c01 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -3323,11 +3323,11 @@ GCodeResult GCodes::SetOrReportOffsets(GCodeBuffer &gb, const StringRef& reply,
 				break;
 
 			case 1:			// turn heaters to standby, except any that are used by a different active tool
-				tool->HeatersToStandby();
+				tool->HeatersToActiveOrStandby(false);
 				break;
 
 			case 2:			// set heaters to their active temperatures, except any that are used by a different active tool
-				tool->HeatersToActive();
+				tool->HeatersToActiveOrStandby(true);
 				break;
 			}
 		}
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 096cb5e1..a36ac792 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -2002,7 +2002,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 							else
 							{
 								heat.SetActiveTemperature(currentHeater, temperature);		// may throw
-								result = heat.Activate(currentHeater, reply);
+								result = heat.SetActiveOrStandby(currentHeater, nullptr, true, reply);
 							}
 						}
 					}
@@ -2047,14 +2047,8 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					const int8_t bedHeater = reprap.GetHeat().GetBedHeater(index);
 					if (bedHeater >= 0)
 					{
-						if (gb.Seen('S') && gb.GetIValue() == 1)
-						{
-							result = reprap.GetHeat().Activate(bedHeater, reply);
-						}
-						else
-						{
-							reprap.GetHeat().Standby(bedHeater, nullptr);
-						}
+						const bool setActive = gb.Seen('S') && gb.GetIValue() == 1;
+						result = reprap.GetHeat().SetActiveOrStandby(bedHeater, nullptr, setActive, reply);
 					}
 				}
 				break;
@@ -2097,7 +2091,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 						}
 
 						reprap.GetHeat().SetActiveTemperature(heater, temperature);		// may throw
-						result = reprap.GetHeat().Activate(heater, reply);
+						result = reprap.GetHeat().SetActiveOrStandby(heater, nullptr, true, reply);
 						if (cancelWait || reprap.GetHeat().HeaterAtSetTemperature(heater, waitWhenCooling, TEMPERATURE_CLOSE_ENOUGH))
 						{
 							cancelWait = isWaiting = false;
diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp
index ad90f237..451c72fc 100644
--- a/src/Heating/Heat.cpp
+++ b/src/Heating/Heat.cpp
@@ -776,12 +776,17 @@ float Heat::GetTargetTemperature(int heater) const noexcept
 				: 0.0;
 }
 
-GCodeResult Heat::Activate(int heater, const StringRef& reply) noexcept
+GCodeResult Heat::SetActiveOrStandby(int heater, const Tool *tool, bool active, const StringRef& reply) noexcept
 {
 	const auto h = FindHeater(heater);
 	if (h.IsNotNull())
 	{
-		return h->Activate(reply);
+		const GCodeResult rslt = h->SetActiveOrStandby(active, reply);
+		if (rslt == GCodeResult::ok && !active)
+		{
+			lastStandbyTools[heater] = tool;
+		}
+		return rslt;
 	}
 	reply.printf("Heater %d not found", heater);
 	return GCodeResult::error;
@@ -827,16 +832,6 @@ void Heat::SwitchOffAllLocalFromISR() noexcept
 	}
 }
 
-void Heat::Standby(int heater, const Tool *tool) noexcept
-{
-	const auto h = FindHeater(heater);
-	if (h.IsNotNull())
-	{
-		h->Standby();
-		lastStandbyTools[heater] = tool;
-	}
-}
-
 void Heat::FeedForwardAdjustment(unsigned int heater, float fanPwmChange, float extrusionChange) const noexcept
 {
 	const auto h = FindHeater(heater);
diff --git a/src/Heating/Heat.h b/src/Heating/Heat.h
index 8327f2a7..6e7576b7 100644
--- a/src/Heating/Heat.h
+++ b/src/Heating/Heat.h
@@ -123,8 +123,9 @@ public:
 
 	void SetActiveTemperature(int heater, float t) THROWS(GCodeException) { SetTemperature(heater, t, true); }
 	void SetStandbyTemperature(int heater, float t) THROWS(GCodeException) { SetTemperature(heater, t, false); }
-	GCodeResult Activate(int heater, const StringRef& reply) noexcept;	// Turn on a heater
-	void Standby(int heater, const Tool* tool) noexcept;				// Set a heater to standby
+	void SetTemperature(int heater, float t, bool activeNotStandby) THROWS(GCodeException);
+
+	GCodeResult SetActiveOrStandby(int heater, const Tool *tool, bool active, const StringRef& reply) noexcept;	// Turn a heater on
 	void SwitchOff(int heater) noexcept;								// Turn off a specific heater
 	void FeedForwardAdjustment(unsigned int heater, float fanPwmChange, float extrusionChange) const noexcept;
 	void SetExtrusionFeedForward(unsigned int heater, float pwm) const noexcept;
@@ -163,7 +164,6 @@ private:
 	ReadLockedPointer<Heater> FindHeater(int heater) const noexcept;
 	void DeleteSensor(unsigned int sn) noexcept;
 	void InsertSensor(TemperatureSensor *newSensor) noexcept;
-	void SetTemperature(int heater, float t, bool activeNotStandby) THROWS(GCodeException);
 
 #if SUPPORT_REMOTE_COMMANDS
 	void SendHeatersStatus(CanMessageBuffer& buf) noexcept;
diff --git a/src/Heating/Heater.cpp b/src/Heating/Heater.cpp
index 0760deff..31287fc9 100644
--- a/src/Heating/Heater.cpp
+++ b/src/Heating/Heater.cpp
@@ -569,29 +569,18 @@ const char* Heater::GetSensorName() const noexcept
 	return (sensor.IsNotNull()) ? sensor->GetSensorName() : nullptr;
 }
 
-GCodeResult Heater::Activate(const StringRef& reply) noexcept
+GCodeResult Heater::SetActiveOrStandby(bool setActive, const StringRef& reply) noexcept
 {
 	if (GetMode() != HeaterMode::fault)
 	{
-		active = true;
+		active = setActive;
 		isBedOrChamber = reprap.GetHeat().IsBedOrChamberHeater(heaterNumber);
 		return SwitchOn(reply);
 	}
-	reply.printf("Can't activate heater %u while in fault state", heaterNumber);
+	reply.printf("Can't turn heater %u on while in fault state", heaterNumber);
 	return GCodeResult::error;
 }
 
-void Heater::Standby() noexcept
-{
-	if (GetMode() != HeaterMode::fault)
-	{
-		active = false;
-		String<1> dummy;
-		isBedOrChamber = reprap.GetHeat().IsBedOrChamberHeater(heaterNumber);
-		(void)SwitchOn(dummy.GetRef());
-	}
-}
-
 void Heater::SetTemperature(float t, bool activeNotStandby) THROWS(GCodeException)
 {
 	if (t > GetHighestTemperatureLimit())
diff --git a/src/Heating/Heater.h b/src/Heating/Heater.h
index 284e6783..43869f38 100644
--- a/src/Heating/Heater.h
+++ b/src/Heating/Heater.h
@@ -71,8 +71,7 @@ public:
 	void SetTemperature(float t, bool activeNotStandby) THROWS(GCodeException);
 	float GetActiveTemperature() const noexcept { return activeTemperature; }
 	float GetStandbyTemperature() const noexcept { return standbyTemperature; }
-	GCodeResult Activate(const StringRef& reply) noexcept;				// Switch from idle to active
-	void Standby() noexcept;											// Switch from active to idle
+	GCodeResult SetActiveOrStandby(bool setActive, const StringRef& reply) noexcept;	// Switch from idle to active or standby
 	GCodeResult StartAutoTune(GCodeBuffer& gb, const StringRef& reply, FansBitmap fans) THROWS(GCodeException);
 																		// Start an auto tune cycle for this heater
 	void GetAutoTuneStatus(const StringRef& reply) const noexcept;		// Get the auto tune status or last result
diff --git a/src/Platform/RepRap.cpp b/src/Platform/RepRap.cpp
index df5c1756..8236819e 100644
--- a/src/Platform/RepRap.cpp
+++ b/src/Platform/RepRap.cpp
@@ -1101,18 +1101,15 @@ void RepRap::DeleteTool(int toolNumber) noexcept
 void RepRap::SelectTool(int toolNumber, bool simulating) noexcept
 {
 	ReadLockedPointer<Tool> const newTool = GetTool(toolNumber);
-	if (!simulating)
+	if (!simulating && currentTool != nullptr && currentTool != newTool.Ptr())
 	{
-		if (currentTool != nullptr && currentTool != newTool.Ptr())
-		{
-			currentTool->Standby();
-		}
-		if (newTool.IsNotNull())
-		{
-			newTool->Activate();
-		}
+		currentTool->Standby();
+	}
+	currentTool = newTool.Ptr();					// must do this first so that Activate() will always work
+	if (!simulating && newTool.IsNotNull())
+	{
+		newTool->Activate();
 	}
-	currentTool = newTool.Ptr();
 }
 
 void RepRap::PrintTool(int toolNumber, const StringRef& reply) const noexcept
diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp
index 8408b458..d115504d 100644
--- a/src/Tools/Tool.cpp
+++ b/src/Tools/Tool.cpp
@@ -421,31 +421,12 @@ bool Tool::AllHeatersAtHighTemperature(bool forExtrusion) const noexcept
 	return true;
 }
 
-// Activate this tool
+// Activate this tool. Must set the current tool to be this tool first, otherwise the heater temperature may not get set.
 void Tool::Activate() noexcept
 {
-	for (size_t heater = 0; heater < heaterCount; heater++)
-	{
-		String<StringLength100> message;
-		GCodeResult ret;
-		try
-		{
-			reprap.GetHeat().SetActiveTemperature(heaters[heater], activeTemperatures[heater]);
-			reprap.GetHeat().SetStandbyTemperature(heaters[heater], standbyTemperatures[heater]);
-			ret = reprap.GetHeat().Activate(heaters[heater], message.GetRef());
-		}
-		catch (const GCodeException& exc)
-		{
-			exc.GetMessage(message.GetRef(), nullptr);
-			ret = GCodeResult::error;
-		}
-		if (ret != GCodeResult::ok)
-		{
-			reprap.GetPlatform().MessageF((ret == GCodeResult::warning) ? WarningMessage : ErrorMessage, "%s\n", message.c_str());
-		}
-	}
+	HeatersToActiveOrStandby(true);
 
-	if (spindleNumber > -1)
+	if (spindleNumber >= 0)
 	{
 		Spindle& spindle = reprap.GetPlatform().AccessSpindle(spindleNumber);
 
@@ -458,30 +439,23 @@ void Tool::Activate() noexcept
 	state = ToolState::active;
 }
 
-void Tool::HeatersToStandby() const noexcept
+void Tool::Standby() noexcept
 {
-	const Tool * const currentTool = reprap.GetCurrentTool();
-	for (size_t heater = 0; heater < heaterCount; heater++)
+	HeatersToActiveOrStandby(false);
+
+	// NIST Standard M6 says "When the tool change is complete: * The spindle will be stopped. [...]"
+	// We don't have M6 but Tn already does tool change so we need
+	// to make sure the spindle is off
+	if (spindleNumber >= 0)
 	{
-		// Don't switch a heater to standby if the active tool is using it and is different from this tool
-		if (currentTool == this || currentTool == nullptr || !currentTool->UsesHeater(heater))
-		{
-			try
-			{
-				reprap.GetHeat().SetStandbyTemperature(heaters[heater], standbyTemperatures[heater]);
-				reprap.GetHeat().Standby(heaters[heater], this);
-			}
-			catch (const GCodeException& exc)
-			{
-				String<StringLength100> message;
-				exc.GetMessage(message.GetRef(), nullptr);
-				reprap.GetPlatform().Message(ErrorMessage, message.c_str());
-			}
-		}
+		Spindle& spindle = reprap.GetPlatform().AccessSpindle(spindleNumber);
+		spindle.SetState(SpindleState::stopped);
 	}
+
+	state = ToolState::standby;
 }
 
-void Tool::HeatersToActive() const noexcept
+void Tool::HeatersToActiveOrStandby(bool active) const noexcept
 {
 	const Tool * const currentTool = reprap.GetCurrentTool();
 	for (size_t heater = 0; heater < heaterCount; heater++)
@@ -493,8 +467,8 @@ void Tool::HeatersToActive() const noexcept
 			GCodeResult ret;
 			try
 			{
-				reprap.GetHeat().SetActiveTemperature(heaters[heater], activeTemperatures[heater]);
-				ret = reprap.GetHeat().Activate(heaters[heater], message.GetRef());
+				reprap.GetHeat().SetTemperature(heaters[heater], ((active) ? activeTemperatures[heater] : standbyTemperatures[heater]), active);
+				ret = reprap.GetHeat().SetActiveOrStandby(heaters[heater], this, active, message.GetRef());
 			}
 			catch (const GCodeException& exc)
 			{
@@ -522,22 +496,6 @@ void Tool::HeatersToOff() const noexcept
 	}
 }
 
-void Tool::Standby() noexcept
-{
-	HeatersToStandby();
-
-	// NIST Standard M6 says "When the tool change is complete: * The spindle will be stopped. [...]"
-	// We don't have M6 but Tn already does tool change so we need
-	// to make sure the spindle is off
-	if (spindleNumber > -1)
-	{
-		Spindle& spindle = reprap.GetPlatform().AccessSpindle(spindleNumber);
-		spindle.SetState(SpindleState::stopped);
-	}
-
-	state = ToolState::standby;
-}
-
 // May be called from ISR
 bool Tool::ToolCanDrive(bool extrude) noexcept
 {
@@ -650,47 +608,18 @@ float Tool::GetToolHeaterStandbyTemperature(size_t heaterNumber) const noexcept
 	return (heaterNumber < heaterCount) ? standbyTemperatures[heaterNumber] : 0.0;
 }
 
-void Tool::SetToolHeaterActiveTemperature(size_t heaterNumber, float temp) THROWS(GCodeException)
-{
-	if (heaterNumber < heaterCount)
-	{
-		const int8_t heater = heaters[heaterNumber];
-		const Tool * const currentTool = reprap.GetCurrentTool();
-		const bool setHeater = (currentTool == nullptr || currentTool == this);
-		if (temp <= NEARLY_ABS_ZERO)								// temperatures close to ABS_ZERO turn off the heater
-		{
-			activeTemperatures[heaterNumber] = 0;
-			if (setHeater)
-			{
-				reprap.GetHeat().SwitchOff(heater);
-			}
-		}
-		else
-		{
-			if (temp <= reprap.GetHeat().GetLowestTemperatureLimit(heater) || temp >= reprap.GetHeat().GetHighestTemperatureLimit(heater))
-			{
-				throw GCodeException(-1, -1, "Requested temperature out of range");
-			}
-			activeTemperatures[heaterNumber] = temp;
-			if (setHeater)
-			{
-				reprap.GetHeat().SetActiveTemperature(heater, temp);
-			}
-		}
-	}
-}
-
-void Tool::SetToolHeaterStandbyTemperature(size_t heaterNumber, float temp) THROWS(GCodeException)
+void Tool::SetToolHeaterActiveOrStandbyTemperature(size_t heaterNumber, float temp, bool active) THROWS(GCodeException)
 {
 	if (heaterNumber < heaterCount)
 	{
+		float& relevantTemperature = (active) ? activeTemperatures[heaterNumber] : standbyTemperatures[heaterNumber];
 		const int8_t heater = heaters[heaterNumber];
 		const Tool * const currentTool = reprap.GetCurrentTool();
 		const Tool * const lastStandbyTool = reprap.GetHeat().GetLastStandbyTool(heater);
-		const bool setHeater = (currentTool == nullptr || currentTool == this || lastStandbyTool == nullptr || lastStandbyTool == this);
+		const bool setHeater = (currentTool == nullptr || currentTool == this || (!active && (lastStandbyTool == nullptr || lastStandbyTool == this)));
 		if (temp <= NEARLY_ABS_ZERO)								// temperatures close to ABS_ZERO turn off the heater
 		{
-			standbyTemperatures[heaterNumber] = 0;
+			relevantTemperature = 0;
 			if (setHeater)
 			{
 				reprap.GetHeat().SwitchOff(heater);
@@ -702,10 +631,10 @@ void Tool::SetToolHeaterStandbyTemperature(size_t heaterNumber, float temp) THRO
 			{
 				throw GCodeException(-1, -1, "Requested temperature out of range");
 			}
-			standbyTemperatures[heaterNumber] = temp;
+			relevantTemperature = temp;
 			if (setHeater)
 			{
-				reprap.GetHeat().SetStandbyTemperature(heater, temp);
+				reprap.GetHeat().SetTemperature(heater, temp, active);
 			}
 		}
 	}
diff --git a/src/Tools/Tool.h b/src/Tools/Tool.h
index 99b0ae3a..db86ffc6 100644
--- a/src/Tools/Tool.h
+++ b/src/Tools/Tool.h
@@ -103,8 +103,9 @@ public:
 
 	float GetToolHeaterActiveTemperature(size_t heaterNumber) const noexcept;
 	float GetToolHeaterStandbyTemperature(size_t heaterNumber) const noexcept;
-	void SetToolHeaterActiveTemperature(size_t heaterNumber, float temp) THROWS(GCodeException);
-	void SetToolHeaterStandbyTemperature(size_t heaterNumber, float temp) THROWS(GCodeException);
+	void SetToolHeaterActiveTemperature(size_t heaterNumber, float temp) THROWS(GCodeException) { SetToolHeaterActiveOrStandbyTemperature(heaterNumber, temp, true); }
+	void SetToolHeaterStandbyTemperature(size_t heaterNumber, float temp) THROWS(GCodeException) { SetToolHeaterActiveOrStandbyTemperature(heaterNumber, temp, false); }
+
 	GCodeResult SetFirmwareRetraction(GCodeBuffer& gb, const StringRef& reply, OutputBuffer*& outBuf) THROWS(GCodeException);
 	GCodeResult GetSetFeedForward(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);
 
@@ -117,8 +118,8 @@ public:
 	void SetFansPwm(float f) const noexcept;
 
 	void HeatersToOff() const noexcept;
-	void HeatersToActive() const noexcept;
-	void HeatersToStandby() const noexcept;
+	void HeatersToActiveOrStandby(bool active) const noexcept;
+
 
 	void ApplyFeedForward(float extrusionSpeed) const noexcept;
 	void StopFeedForward() const noexcept;
@@ -144,6 +145,7 @@ protected:
 private:
 	Tool() noexcept : next(nullptr), filament(nullptr), name(nullptr), state(ToolState::off) { }
 
+	void SetToolHeaterActiveOrStandbyTemperature(size_t heaterNumber, float temp, bool active) THROWS(GCodeException);
 	void SetTemperatureFault(int8_t dudHeater) noexcept;
 	void ResetTemperatureFault(int8_t wasDudHeater) noexcept;
 	bool AllHeatersAtHighTemperature(bool forExtrusion) const noexcept;
diff --git a/src/Version.h b/src/Version.h
index 04ee444e..616b78e9 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -10,7 +10,7 @@
 
 #ifndef VERSION
 // Note: the complete VERSION string must be in standard version number format and must not contain spaces! This is so that DWC can parse it.
-# define MAIN_VERSION	"3.4.0beta7+3"
+# define MAIN_VERSION	"3.4.0beta7+4"
 # ifdef USE_CAN0
 #  define VERSION_SUFFIX	"(CAN0)"
 # else