Index: firmware/App/Controllers/Valves.c
===================================================================
diff -u -rba6530b83f85452382325b97eae02d9e7e5c1cb1 -r660a411129a92b9519b0cd34d41dba79953616e5
--- firmware/App/Controllers/Valves.c	(.../Valves.c)	(revision ba6530b83f85452382325b97eae02d9e7e5c1cb1)
+++ firmware/App/Controllers/Valves.c	(.../Valves.c)	(revision 660a411129a92b9519b0cd34d41dba79953616e5)
@@ -94,10 +94,8 @@
 static VALVE_STATE_T handleValvesNotHomedState( VALVE_T valve );
 static VALVE_STATE_T handleValvesIdleState( VALVE_T valve );
 static VALVE_STATE_T handleValvesTransitionState( VALVE_T valve );
-static S32  getValveActualEncoderPosition( VALVE_T valve );
-static S32  getValveCommandedEncoderPosition( VALVE_T valve );
-static S32  getValvePositionError( VALVE_T valve );
 static BOOL isValveAtCommandedPosition( VALVE_T valve );
+static VALVE_POSITION_T getCurrentValveABCPosition( VALVE_T valve );
 static void publishValvesData( void );
 
 /*********************************************************************//**
@@ -399,7 +397,7 @@
         currentValveStates[ valve ].homingOperationActive = FALSE;
         currentValveStates[ valve ].hasValveBeenHomed = TRUE;
         currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE;
-        currentValveStates[ valve ].currentPosition   = VALVE_POSITION_C_CLOSE; // TODO: Later
+        currentValveStates[ valve ].currentPosition   = getCurrentValveABCPosition( valve );
         nextState = VALVE_STATE_IDLE;
     }
     else if ( ( TRUE == currentValveStates[ valve ].homingOperationActive ) &&
@@ -449,7 +447,7 @@
     }
     else
     {
-        if ( getValvePositionError( valve ) > 15 )
+        if ( isValveAtCommandedPosition( valve ) != TRUE )
         {
             setValvePosition( valve, currentValveStates[ valve ].commandedPosition );
         }
@@ -475,7 +473,7 @@
     {
         if ( TRUE == isValveAtCommandedPosition( valve ) )
         {
-            currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition;
+            currentValveStates[ valve ].currentPosition = getCurrentValveABCPosition( valve );
             nextState = VALVE_STATE_IDLE;
         }
         else
@@ -514,16 +512,11 @@
  *************************************************************************/
 static BOOL isValveAtCommandedPosition( VALVE_T valve )
 {
-    BOOL result = FALSE;
-    S32 currentPosition;
-    S32 targetPosition;
-    S32 positionDelta;
+    BOOL result         = FALSE;
+    S32 currentPosition = getPinchValvePosition( valve );
+    S32 targetPosition  = getPinchValveStoredPosition( valve, currentValveStates[ valve ].commandedPosition );
+    S32 positionDelta   = abs( currentPosition - targetPosition );
 
-    currentPosition = (S32)getPinchValvePosition( valve );
-    targetPosition  = (S32)getPinchValveStoredPosition( valve, currentValveStates[ valve ].commandedPosition );
-
-    positionDelta = abs( currentPosition - targetPosition );
-
     if ( positionDelta <= VALVE_TRANSITION_MIN_TGT_DELTA )
     {
         result = TRUE;
@@ -534,66 +527,36 @@
 
 /*********************************************************************//**
  * @brief
- * The getValveActualEncoderPosition function handles the current and target
- * position
- * @details \b Inputs: currentPosition, targetPosition, positionDelta
- * @details \b Outputs: result
- * @return none
+ * The getCurrentValveABCPosition function determines the current position
+ * (enum) for a given valve based on current encoder position.
+ * @details \b Inputs: currentValveStates[]
+ * @details \b Outputs: none
+ * @param valve ID of valve to get current position for
+ * @return Position of given valve (enum)
  *************************************************************************/
-static S32 getValveActualEncoderPosition( VALVE_T valve )
+static VALVE_POSITION_T getCurrentValveABCPosition( VALVE_T valve )
 {
-    S32 result = 0;
+    VALVE_POSITION_T result = VALVE_POSITION_NOT_IN_POSITION;
+    VALVE_POSITION_T p;
 
-    if ( valve < NUM_OF_VALVES )
+    for ( p = VALVE_POSITION_A_INSERT_EJECT; p < NUM_OF_VALVE_POSITIONS; p++ )
     {
-        result = getPinchValvePosition( valve );
-    }
+        S32 pos            = getPinchValveStoredPosition( valve, p );
+        S32 targetPosition = getPinchValveStoredPosition( valve, currentValveStates[ valve ].commandedPosition );
+        S32 positionDelta  = abs( pos - targetPosition );
 
-    return result;
-}
-
-/*********************************************************************//**
- * @brief
- * The getValveCommandedEncoderPosition function handles the current and target
- * position
- * @details \b Inputs: currentPosition, targetPosition, positionDelta
- * @details \b Outputs: result
- * @return none
- *************************************************************************/
-static S32 getValveCommandedEncoderPosition( VALVE_T valve )
-{
-    S32 result = 0;
-
-    if ( valve < NUM_OF_VALVES )
-    {
-        result = getPinchValveStoredPosition( valve, currentValveStates[ valve ].commandedPosition );
+        if ( positionDelta <= VALVE_TRANSITION_MIN_TGT_DELTA )
+        {
+            result = p;
+            break;
+        }
     }
 
     return result;
 }
 
 /*********************************************************************//**
  * @brief
- * The getValvePositionError function handles the current and target
- * position
- * @details \b Inputs: currentPosition, targetPosition, positionDelta
- * @details \b Outputs: result
- * @return none
- *************************************************************************/
-static S32 getValvePositionError( VALVE_T valve )
-{
-    S32 result = 0;
-
-    if ( valve < NUM_OF_VALVES )
-    {
-        result = abs( getValveActualEncoderPosition( valve ) - getValveCommandedEncoderPosition( valve ) );
-    }
-
-   return result;
-}
-
-/*********************************************************************//**
- * @brief
  * The publishValvesData function constructs and sends the valves data
  * broadcast message.
  * @details \b Message \b Sent: MSG_ID_TD_VALVES_DATA