 	}
 };
 
-float temp, tempRiseTarget, tempZero;
+#define ROLLING_AVG_DATA_POINTS 200
+#define TEMP_AVERAGING_INITIAL_VALUE (float)21
+class RollingAverage
+{
+public:
+  RollingAverage() : val(), average(0) {}
+  RollingAverage(float initialValue) : val(), average(initialValue)
+  {
+    for (uint8_t i = 0; i < ROLLING_AVG_DATA_POINTS; i++)
+      val[i] = initialValue;
+  }
+
+  void push(float value)
+  {
+    for (uint8_t i = 1; i < ROLLING_AVG_DATA_POINTS; i++)
+      val[i-1] = val[i];
+    val[ROLLING_AVG_DATA_POINTS - 1] = value;
+    updateAverage();
+  }
+
+  void set(float value)
+  {
+    for (uint8_t i = 1; i < ROLLING_AVG_DATA_POINTS; i++)
+      val[i] = value;
+    average = value;
+  }
+
+  float getAverage() { return average; }
+
+  uint8_t getSize() { return ROLLING_AVG_DATA_POINTS; }
+
+private:
+  void updateAverage()
+  {
+    float sum = 0;
+    for (uint8_t i = 0; i < ROLLING_AVG_DATA_POINTS; i++)
+      sum += val[i];
+    average = sum / (float)ROLLING_AVG_DATA_POINTS;
+  }
+  
+  float val[ROLLING_AVG_DATA_POINTS];
+  float average;
+};
+
+float tempRiseTarget, tempZero;
+RollingAverage tempAvg(TEMP_AVERAGING_INITIAL_VALUE);
 float heaterDutyCycle;
 uint32_t timeZero, sampleTime, heaterOnTime, heaterPeriodTime, pidUpdateTime;
 long sampleTimeLeft, heaterOnTimeLeft, heaterPeriodTimeLeft, pidUpdateTimeLeft;


  if (pidUpdateTimeLeft < 0)
  {
    pidDrive = updatePID(&pid, tempZero + tempRiseTarget - temp, temp - tempZero);
    heaterDutyCycle = tempChangeToDuty(pidDrive);
    pidUpdateTime = millis() + PID_UPDATE_PERIOD_MS;
    
    DEBUG2(F("pidDrive        = "), pidDrive);
    DEBUG2(F("heaterDutyCycle = "), heaterDutyCycle);
  }
}
#endif // P_MODE

#ifdef I_MODE
void loop()
{
  sampleTimeLeft = sampleTime - millis();
  pidUpdateTimeLeft = pidUpdateTime - millis();
  heaterOnTimeLeft = heaterOnTime - millis();
  heaterPeriodTimeLeft = heaterPeriodTime - millis();

  if (sampleTimeLeft < 0)
  {
    temp = getTemperature();
    printData(&pid);
    sampleTime = millis() + SAMPLE_DELAY_MS;
  }

  if (heaterOnTimeLeft < 0 && heaterIsOn)
    heaterOff();

  if (heaterPeriodTimeLeft < 0)
  {
    heaterOn();
    heaterOnTime = millis() + (uint32_t)(heaterDutyCycle * HEATER_PERIOD_MS);
    heaterPeriodTime = millis() + HEATER_PERIOD_MS;
  }

  if (pidUpdateTimeLeft < 0)
  {
    pidDrive = updatePID(&pid, tempZero + tempRiseTarget - temp, temp - tempZero);
    heaterDutyCycle = tempChangeToDuty(pidDrive);
    pidUpdateTime = millis() + PID_UPDATE_PERIOD_MS;
    
    DEBUG2(F("pidDrive        = "), pidDrive);
    DEBUG2(F("heaterDutyCycle = "), heaterDutyCycle);
  }
}
#endif // I_MODE

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

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

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

}
#endif // OPEN_LOOP_MODE

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