Index: firmware/App/Controllers/Heaters.c
===================================================================
diff -u -ref5ef16518466c92687b869cabfa16eaf3365dc0 -r166450594d134da4e3e380c1c8879602a5b86407
--- firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision ef5ef16518466c92687b869cabfa16eaf3365dc0)
+++ firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision 166450594d134da4e3e380c1c8879602a5b86407)
@@ -42,7 +42,9 @@
 #define INITIAL_DUTY_CYCLE_SMALL_PRIMARY_HEATER 0.10
 #define INITIAL_DUTY_CYCLE_TRIMMER_HEATER 0.10
 
-typedef enum heaters_self_tesst_states
+#define DELTA_TEMP_TO_PWM_DUTY_CYCLE_CONVERSION  0.03
+
+typedef enum heaters_self_test_states
 {
     HEATERS_SELF_TEST_START = 0,
     HEATERS_SELF_TEST_START_SMALL_PRIMARY_AND_TRIMMER,
@@ -65,12 +67,22 @@
     NUM_OF_TRIMMER_HEATER_EXEC_STATES
 } TRIMMER_HEATER_EXEC_STATES_T;
 
+typedef enum name_of_heaters
+{
+    PRIMARY_HEATER = 0,
+    TRIMMER_HEATER,
+    NUM_OF_HEATERS
+} NAME_OF_HEATER_T;
+
 // Private variables
 
 static SELF_TEST_STATUS_T         heatersSelfTestResult;
 static HEATERS_SELF_TEST_STATES_T heatersSelfTestState;
 static PRIMARY_HEATERS_EXEC_STATES_T      primaryHeatersExecState;
 static TRIMMER_HEATER_EXEC_STATES_T trimmerHeaterExecState;
+
+static F32 primaryHeaterTargetTemperature;
+static F32 trimmerHeaterTargetTemperature;
 static F32 smallPrimaryHeaterPWMDutyCycle;
 static F32 mainPrimaryHeaterPWMDutyCycle;
 static F32 trimmerHeaterPWMDutyCycle;
@@ -80,9 +92,10 @@
 static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateOff ( void );
 static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateControlToTarget ( void );
 
-static void setMainPrimaryHeaterPWM( F32 pwm );
+static void setMainPrimaryHeaterPWM ( F32 pwm );
 static void setSmallPrimaryHeaterPWM ( F32 pwm );
-static void setTrimmerHeaterPWM( F32 pwm );
+static void setTrimmerHeaterPWM ( F32 pwm );
+static void resetHeaterState ( NAME_OF_HEATER_T heater );
 
 // TODo GPIO for enabling the primary heater
 #define PRIMARY_HEATER_MIBSPI1_PORT_MASK     0x00000002  // (CS1 - re-purposed as input GPIO)
@@ -96,15 +109,46 @@
     heatersSelfTestState    = HEATERS_SELF_TEST_START;
     primaryHeatersExecState = PRIMARY_HEATERS_EXEC_STATE_OFF;
     trimmerHeaterExecState  = TRIMMER_HEATER_EXEC_STATE_OFF;
+
+    primaryHeaterTargetTemperature = 0.0;
+    trimmerHeaterTargetTemperature = 0.0;
     smallPrimaryHeaterPWMDutyCycle = 0.0;
     mainPrimaryHeaterPWMDutyCycle = 0.0;
     trimmerHeaterPWMDutyCycle = 0.0;
+
+    // initialize the PI controller for the primary heaters
+    initializePIController( PI_CONTROLLER_ID_PRIMARY_HEATER, HEATERS_MIN_DUTY_CYCLE,
+                            PRIMARY_HEATERS_P_COEFFICIENT, PRIMARY_HEATERS_I_COEFFICIENT,
+                            HEATERS_MIN_DUTY_CYCLE,
+                            MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE + SMALL_PRIMAY_HEATER_MAX_DUTY_CYCLE );
 }
 
-void startPrimaryHeater ( void )
+void setPrimaryHeaterTargetTemperature ( F32 targetTemp )
 {
-    setMainPrimaryHeaterPWM ( 0.5 );
+    primaryHeaterTargetTemperature = targetTemp;
+}
+
+void setTrimmerHeaterTargetTemperature ( F32 targetTemp )
+{
+    trimmerHeaterTargetTemperature = targetTemp;
+}
+
+BOOL startPrimaryHeater ( void )
+{
+    BOOL status = FALSE;
+    // TODO: remove this code. For testing only
+    setMainPrimaryHeaterPWM ( 0.8 );
     setSmallPrimaryHeaterPWM ( 0.25 );
+    // TODO: remove this code. for testing only
+
+    if ( primaryHeaterTargetTemperature != 0.0 )
+    {
+        resetHeaterState ( PRIMARY_HEATER );
+        primaryHeatersExecState = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
+        status = TRUE;
+    }
+
+    return status;
 }
 
 void startTrimmerHeater ( void )
@@ -114,10 +158,15 @@
 
 void stopPrimaryHeater ( void )
 {
-    setMainPrimaryHeaterPWM ( 0.0 );
-    setSmallPrimaryHeaterPWM ( 0.0 );
+    setMainPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
+    setSmallPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
 }
 
+void stopTrimmerHeater ( void )
+{
+    setTrimmerHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
+}
+
 SELF_TEST_STATUS_T execHeatersSelfTest ( void )
 {
     switch ( heatersSelfTestState )
@@ -146,7 +195,6 @@
     return heatersSelfTestResult;
 }
 
-
 void execPrimaryHeaters ( void )
 {
     switch ( primaryHeatersExecState )
@@ -191,43 +239,78 @@
     }
 }
 
-static void setMainPrimaryHeaterPWM( F32 pwm )
-{
-    etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( pwm * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
-}
+// Private functions
 
-static void setSmallPrimaryHeaterPWM ( F32 pwm )
-{
-    etpwmSetCmpB( etpwmREG1, (U32)( (S32)( ( pwm * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
-}
-static void setTrimmerHeaterPWM( F32 pwm )
-{
-    etpwmSetCmpA( etpwmREG3, (U32)( (S32)( ( pwm * (F32)(etpwmREG3->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
-}
-
 static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateOff ( void )
 {
     PRIMARY_HEATERS_EXEC_STATES_T state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
 
     return state;
 }
+
 static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateControlToTarget ( void )
 {
     PRIMARY_HEATERS_EXEC_STATES_T state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
 
+    // TODO do we need to have a counter for the controller?
+    F32 outletTemp = getTemperatureValue ( TEMPSENSORS_OUTLET_PRIMARY_HEATER_TEMP_SENSOR );
+    F32 newPWM = runPIController ( PI_CONTROLLER_ID_PRIMARY_HEATER, primaryHeaterTargetTemperature, outletTemp );
+
+    if ( newPWM >= MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE )
+    {
+        setMainPrimaryHeaterPWM ( MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE );
+        newPWM = newPWM - MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE;
+        setSmallPrimaryHeaterPWM ( newPWM );
+    }
+    else
+    {
+
+    }
+
+
     // TODO remove this code for testing
     if ( TOGGLEPRIMAYHEATER() )
     {
         startPrimaryHeater();
     }
-
     if ( !TOGGLEPRIMAYHEATER() )
     {
         stopPrimaryHeater();
     }
     // TODO remove this code for testing
 
-
     return state;
 }
 
+static void setMainPrimaryHeaterPWM( F32 pwm )
+{
+    etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( pwm * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
+}
+
+static void setSmallPrimaryHeaterPWM ( F32 pwm )
+{
+    etpwmSetCmpB( etpwmREG1, (U32)( (S32)( ( pwm * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
+}
+
+static void setTrimmerHeaterPWM( F32 pwm )
+{
+    etpwmSetCmpA( etpwmREG3, (U32)( (S32)( ( pwm * (F32)(etpwmREG3->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
+}
+
+static void resetHeaterState ( NAME_OF_HEATER_T heater )
+{
+    if ( heater == PRIMARY_HEATER )
+    {
+        F32 inletTemperatrue = getTemperatureValue ( TEMPSENSORS_INLET_PRIMARY_HEATER_TEMP_SENSOR );
+        F32 initialPWMDutyCycle = fabs(primaryHeaterTargetTemperature - inletTemperatrue) * DELTA_TEMP_TO_PWM_DUTY_CYCLE_CONVERSION;
+        resetPIController( PI_CONTROLLER_ID_PRIMARY_HEATER, initialPWMDutyCycle );
+    }
+    else if ( heater == TRIMMER_HEATER )
+    {
+        // TODO setup the trimmer heater
+    }
+}
+
+
+
+