 void heaterOn()
 {
 	digitalWrite(HEATER_PIN, HIGH);
+  heaterIsOn = true;
   DEBUG1(F("Heater turned on."));
 }
 
 void heaterOff()
 {
 	digitalWrite(HEATER_PIN, LOW);
-}
-
-float temp, tempSetPoint;
-float heaterDutyCycle;
-uint32_t timeZero, sampleTime, heaterOnTime, heaterPeriodTime;
-long sampleTimeLeft, heaterOnTimeLeft, heaterPeriodTimeLeft;
-SPid pid;
-float pidDrive;
+  heaterIsOn = false;
+}
 
 const char comma PROGMEM = ',';
 #define printCsv(x) Serial.print(x); Serial.write(comma)
 void printData(SPid * pid)
 {
-	printCsv((millis() - timeZero) / 1000UL);
+	printCsv( (millis() - timeZero) / (float)1000 );
 	printCsv(temp);
-	printCsv(tempSetPoint);
+	printCsv(tempRiseTarget);
 	printCsv(heaterDutyCycle);
 	printCsv(pid->pGain);
 	printCsv(pid->iGain);

    DEBUG0(Serial.println(heaterPeriodTime - millis()));
	}

	delay(100);
}
#endif // OPEN_LOOP_MODE

#ifdef P_MODE
void loop()
{
	//
}
#endif // P_MODE

#ifdef PI_MODE
void loop()
{
	//
}
#endif // PI_MODE

#ifdef PD_MODE
void loop()
{
	//
}
#endif // PD_MODE

#ifdef PID_MODE
void loop()
{
	heaterToggleTimeLeft = heaterToggleTime - millis();
	sampleTimeLeft = sampleTime - millis();

	if (heaterToggleTimeLeft < 0)
	{
		heaterToggleCount++;
		if (heaterDutyCycle * heaterToggleCount > 0.95)
		{
			heaterOn();
			heaterToggleCount = 0;
		}
		else
			heaterOff();

		heaterToggleTime = millis() + HEATER_TOGGLE_DELAY_MS;
	}

	if (sampleTimeLeft < 0)
	{
		temp = getTemperature();
		drive = updatePID(&pid, tempSetPoint - temp, temp);

		if (drive > DRIVE_MAX)
			heaterDutyCycle = 1.0;
		else if (drive > DRIVE_MIN)
			heaterDutyCycle = drive / 100;
		else
			heaterDutyCycle = 0.0;

		sampleTime = millis() + SAMPLE_DELAY_MS;

		// ...
	}
}
#endif // PID_MODE

	Serial.println();
	printDataHeader();

  pinMode(HEATER_PIN, OUTPUT);
  heaterOff();
  
//  tempZero = 20.5;
  tempZero = getTempZero();
	temp = getTemperature();
  tempRiseTarget = TARGET;

	pid.init();
	pidDrive = updatePID(&pid, tempZero + tempRiseTarget - temp, temp);
	heaterDutyCycle = DUTY_INIT;

	timeZero = millis();
