 
 	temp = getTemperature();
 	pinMode(HEATER_PIN, OUTPUT);
-	heaterOn();
+	heaterOff();
+
+  tempSetPoint = TARGET;
 
 	pid.init();
-	pidDrive = updatePID(&pid, 0, temp);
-#ifdef DUTY_INIT
+	pidDrive = updatePID(&pid, tempSetPoint - temp, temp);
 	heaterDutyCycle = DUTY_INIT;
-#else
-	heaterDutyCycle = pidDrive / 100;
-#endif
 
 	timeZero = millis();
 	sampleTime = millis();			// sample (etc) immediately
 	heaterPeriodTime = millis();	// start new period immediately
 }
 
-#ifdef SET_POINT_MODE
+#ifdef OPEN_LOOP_MODE
 void loop()
 {
 	sampleTimeLeft = sampleTime - millis();


	delay(100);
}
#endif // SET_POINT_MODE

#ifdef P_MODE
void loop()

  for (uint8_t i = 0; i<TEMPERATURE_DATA_POINT_COUNT; i++)
    sum += analogRead(TEMP_ADC_PIN);
	return ( sum * 0.0048828125 / (float)TEMPERATURE_DATA_POINT_COUNT - 0.5 ) * 100.0 + 5.0;
}

float tempChangeToDuty(float tempChange)
{
  return min(1.0, max(0.0, tempChange * TEMP_TO_DUTY_SLOPE + TEMP_TO_DUTY_OFFSET));
}

void heaterOn()
