Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -re9f7c0ae1796e7014436bb79e6b429f185ae5f24 -r98e9ba4e9e7e2bf97ad5240c2a77efeefe9605e1 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision e9f7c0ae1796e7014436bb79e6b429f185ae5f24) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 98e9ba4e9e7e2bf97ad5240c2a77efeefe9605e1) @@ -94,6 +94,9 @@ 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 void publishValvesData( void ); @@ -524,6 +527,66 @@ /*********************************************************************//** * @brief + * The getValveActualEncoderPosition function handles the current and target + * position + * @details \b Inputs: currentPosition, targetPosition, positionDelta + * @details \b Outputs: result + * @return none + *************************************************************************/ +static S32 getValveActualEncoderPosition( VALVE_T valve ) +{ + S32 result = 0; + + if ( valve < NUM_OF_VALVES ) + { + result = getPinchValvePosition( valve ); + } + + 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 ); + } + + 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