 #define TEMP_ADC_PIN A3
 #define HEATER_PIN 8
 
-#define SAMPLE_DELAY_MS 250L
+#define SAMPLE_DELAY_MS 400L
 #define HEATER_PERIOD_MS 10000L
 #define PID_UPDATE_PERIOD_MS 125L
 
 //#define OPEN_LOOP_MODE
-#define P_MODE
-//#define I_MODE
-//#define PI_MODE
-//#define PD_MODE
-//#define PID_MODE
+#define PID_MODE
 
 #	define DUTY_INIT 0.0
 
-#define PGAIN 0.1
+#define PGAIN 5
 #define IGAIN 0
 #define DGAIN 0
 #define IMIN 0
-#define IMAX 100
-
-#define TARGET 23.5 // 21.5C ambient + 23.5C rise = 45C target
+#define IMAX 10000
+
+#define TARGET 23.5 // ~20.5C ambient + 23.5C rise = 44C target
 
 #define TEMP_TO_DUTY_SLOPE 0.1
 #define TEMP_TO_DUTY_OFFSET 0.0


  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

  heaterIsOn = false;
}

float getErr()
{
  return ( tempZero + tempRiseTarget - tempAvg.getAverage() ) / tempRiseTarget;
}

float getPos()
{
  return ( tempAvg.getAverage() - tempZero ) / tempRiseTarget;
}

const char comma PROGMEM = ',';
#define printCsv(x) Serial.print(x); Serial.write(comma)
void printData(SPid * pid)
{
	printCsv( (millis() - timeZero) / (float)1000 );
	printCsv(tempAvg.getAverage());
	printCsv(tempZero + tempRiseTarget);
  printCsv(tempZero);
	printCsv(heaterDutyCycle);
	printCsv(pid->pGain);
	printCsv(pid->iGain);
	Serial.println(pid->dGain);
}

void printDataHeader()
{
	printCsv(F("time [sec]"));
	printCsv(F("temperature"));
	printCsv(F("target temperature"));
  printCsv(F("baseline"));
	printCsv(F("heater duty cycle"));
	printCsv(F("pGain"));
	printCsv(F("iGain"));
	Serial.println(F("dGain"));
}

void setup()
