   pinMode(HEATER_PIN, OUTPUT);
   heaterOff();
   
-//  tempZero = 20.5;
-  tempZero = getTempZero();
-	temp = getTemperature();
+  for (uint8_t i = 0; i < tempAvg.getSize(); i++)
+    tempAvg.push(getTemperature());
+  tempZero = tempAvg.getAverage();
   tempRiseTarget = TARGET;
 
 	pid.init();
-	pidDrive = updatePID(&pid, tempZero + tempRiseTarget - temp, temp);
+	pidDrive = updatePID(&pid, getErr(), getPos());
 	heaterDutyCycle = DUTY_INIT;
 
 	timeZero = millis();

	return ( sum * 0.0048828125 / (float)TEMPERATURE_DATA_POINT_COUNT - 0.5 ) * 100.0 + 5.0;
}

#define TEMPZERO_DATA_POINT_COUNT 100
#define TEMPZERO_DELAY_MS 20
float getTempZero()
{
  float sum = 0;
  for (uint8_t i = 0; i < TEMPZERO_DATA_POINT_COUNT; i++)
  {
    delay(TEMPZERO_DELAY_MS);
    sum += getTemperature();
  }

#ifdef DEBUG
  float result = sum / (float)TEMPZERO_DATA_POINT_COUNT;
  Serial.print(F("Baseline temperature: "));
  Serial.print(result);
  Serial.println(F(" degrees Celsius"));
  return result;
#else
  return sum / (float)TEMPZERO_DATA_POINT_COUNT;
#endif
}

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

	return pTerm + dTerm + iTerm;
}

#define TEMPERATURE_DATA_POINT_COUNT 10
float getTemperature()
{
  uint32_t sum = 0;
