Fix the WATCHPERIOD implementation. It did not work correctly in a multi...

Fix the WATCHPERIOD implementation. It did not work correctly in a multi extruder setup, it did not work after 32 seconds after startup (int16 used as millies timestamp). And it did not work if you gave an M104 or M109 when the target was already around the target setpoint. So on average, it did not work at all. The new implementation should be robust in detecting a failure to heat up.