Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r8fc5ef2c8ca8e8438b1eaf388b6b9ee5190ae76b -r0eb08506bbe69f0cfc272e0a329591ffdeb3fc95 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 8fc5ef2c8ca8e8438b1eaf388b6b9ee5190ae76b) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 0eb08506bbe69f0cfc272e0a329591ffdeb3fc95) @@ -39,7 +39,7 @@ #define VALVE_HOME_MIN_POS_CHG 3 ///< Minimum encoder position change to indicate a home operation is still moving toward edge. #define VALVE_HOME_BACK_OFF_EDGE 3 ///< Encoder counts to back off of detected edge position. #define MAX_HOME_FULL_TRAVEL_DIFF 10U ///< Maximum allowed difference in full travel encoder counts between expected and measured during home operation. -#define VALVE_TRANSITION_TIMEOUT_MS ( 30 * MS_PER_SECOND ) ///< Valves transition time out in ms. +#define VALVE_TRANSITION_TIMEOUT_MS ( 3 * MS_PER_SECOND ) ///< Valves transition time out in ms. #define HOMING_EDGE_DETECTION_TIMEOUT_MS ( 0.5 * MS_PER_SECOND ) ///< Valves homing edge detection timeout in milliseconds. #define VALVE_FORCE_HOME TRUE ///< Force valve to home even if already homed. #define ZERO_ENC_DEBOUNCE_THRESHOLD_CNT 5 ///< Valves zero encoder debounce threshold count. @@ -91,6 +91,7 @@ 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 BOOL isValveAtCommandedPosition( VALVE_T valve ); static void publishValvesData( void ); /*********************************************************************//** @@ -155,13 +156,12 @@ { BOOL result = FALSE; - ( void )cartridge; - if ( valve < NUM_OF_VALVES ) { if ( ( TRUE == force ) || ( FALSE == currentValveStates[ valve ].hasValveBeenHomed ) ) { - if ( ( FALSE == isPinchValveBusy( valve ) ) && ( FALSE == currentValveStates[ valve ].hasHomingBeenRequested ) ) + if ( ( FALSE == isPinchValveBusy( valve ) ) && ( FALSE == currentValveStates[ valve ].hasHomingBeenRequested ) && + ( FALSE == currentValveStates[ valve ].homingOperationActive ) ) { currentValveStates[ valve ].hasHomingBeenRequested = TRUE; currentValveStates[ valve ].hasTransitionBeenRequested = FALSE; @@ -200,7 +200,8 @@ { if ( ( TRUE == currentValveStates[ valve ].hasValveBeenHomed ) && ( FALSE == isPinchValveBusy( valve ) ) && - ( FALSE == currentValveStates[ valve ].hasTransitionBeenRequested ) ) + ( FALSE == currentValveStates[ valve ].hasTransitionBeenRequested ) && + ( FALSE == currentValveStates[ valve ].homingOperationActive ) ) { currentValveStates[ valve ].pendingCommandedPosition = position; currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; @@ -375,7 +376,8 @@ { VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_HOMED; - if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) + if ( ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) && + ( FALSE == currentValveStates[ valve ].homingOperationActive ) ) { if ( TRUE == homePinchValve( valve ) ) { @@ -384,14 +386,40 @@ currentValveStates[ valve ].homingOperationActive = TRUE; currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); - nextState = VALVE_STATE_IN_TRANSITION; } } + else if ( ( TRUE == currentValveStates[ valve ].homingOperationActive ) && ( FALSE == isPinchValveBusy( valve ) ) ) + { + currentValveStates[ valve ].homingOperationActive = FALSE; + if ( TRUE == isPinchValveHomed( valve ) ) + { + currentValveStates[ valve ].hasValveBeenHomed = TRUE; + currentValveStates[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_A_INSERT_EJECT; + nextState = VALVE_STATE_IDLE; + } + else + { + currentValveStates[ valve ].hasValveBeenHomed = FALSE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + nextState = VALVE_STATE_HOMING_NOT_HOMED; + } + } + else if ( ( TRUE == currentValveStates[ valve ].homingOperationActive ) && + ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) && + ( TRUE != getCPLDACPowerLossDetected() ) ) + { + currentValveStates[ valve ].homingOperationActive = FALSE; + currentValveStates[ valve ].hasValveBeenHomed = FALSE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_TRANSITION_TIMEOUT, (U32)valve, (U32)getPinchValvePosition( valve ) ) + } + return nextState; } - /*********************************************************************//** * @brief * The handleValvesIdleState function handles the Idle state of the state @@ -407,23 +435,17 @@ if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - if ( TRUE == homePinchValve( valve ) ) - { - currentValveStates[ valve ].hasHomingBeenRequested = FALSE; - currentValveStates[ valve ].hasValveBeenHomed = FALSE; - currentValveStates[ valve ].homingOperationActive = TRUE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); - nextState = VALVE_STATE_IN_TRANSITION; - } + currentValveStates[ valve ].hasValveBeenHomed = FALSE; + currentValveStates[ valve ].homingOperationActive = FALSE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + nextState = VALVE_STATE_HOMING_NOT_HOMED; } else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) { if ( TRUE == movePinchValveToStoredPosition( valve, currentValveStates[ valve ].pendingCommandedPosition ) ) { currentValveStates[ valve ].hasTransitionBeenRequested = FALSE; currentValveStates[ valve ].commandedPosition = currentValveStates[ valve ].pendingCommandedPosition; - currentValveStates[ valve ].homingOperationActive = FALSE; currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); nextState = VALVE_STATE_IN_TRANSITION; @@ -448,34 +470,23 @@ if ( FALSE == isPinchValveBusy( valve ) ) { - if ( TRUE == currentValveStates[ valve ].homingOperationActive ) + if ( TRUE == isValveAtCommandedPosition( valve ) ) { - currentValveStates[ valve ].homingOperationActive = FALSE; - - if ( TRUE == isPinchValveHomed( valve ) ) - { - currentValveStates[ valve ].hasValveBeenHomed = TRUE; - currentValveStates[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_A_INSERT_EJECT; - nextState = VALVE_STATE_IDLE; - } - else - { - currentValveStates[ valve ].hasValveBeenHomed = FALSE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - nextState = VALVE_STATE_HOMING_NOT_HOMED; - } + currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; + nextState = VALVE_STATE_IDLE; } else { - currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; - nextState = VALVE_STATE_IDLE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + currentValveStates[ valve ].hasValveBeenHomed = FALSE; + nextState = VALVE_STATE_HOMING_NOT_HOMED; } } else if ( ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) && ( TRUE != getCPLDACPowerLossDetected() ) ) { - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + currentValveStates[ valve ].hasValveBeenHomed = FALSE; SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_TRANSITION_TIMEOUT, (U32)valve, (U32)getPinchValvePosition( valve ) ) @@ -484,22 +495,47 @@ activateSafetyShutdown(); } - if ( TRUE == currentValveStates[ valve ].homingOperationActive ) - { - currentValveStates[ valve ].hasValveBeenHomed = FALSE; - nextState = VALVE_STATE_HOMING_NOT_HOMED; - } - else - { - nextState = VALVE_STATE_IDLE; - } + nextState = VALVE_STATE_HOMING_NOT_HOMED; } return nextState; } /*********************************************************************//** * @brief + * The isValveAtCommandedPosition function handles the Transition state + * of the state machine for a given valve. + * @details \b Inputs: currentValveStates[] + * @details \b Outputs: currentValveStates[] + * @param valve ID of valve for which to handle the Transition state + * @return next state of the state machine for the given valve + *************************************************************************/ +static BOOL isValveAtCommandedPosition( VALVE_T valve ) +{ + BOOL result = FALSE; + S32 currentPosition; + S32 targetPosition; + S32 positionDelta; + + currentPosition = (S32)getPinchValvePosition( valve ); + targetPosition = (S32)getPinchValveStoredPosition( valve, currentValveStates[ valve ].commandedPosition ); + + positionDelta = currentPosition - targetPosition; + if ( positionDelta < 0 ) + { + positionDelta = -positionDelta; + } + + if ( positionDelta <= VALVE_TRANSITION_MIN_TGT_DELTA ) + { + result = TRUE; + } + + return result; +} + +/*********************************************************************//** + * @brief * The publishValvesData function constructs and sends the valves data * broadcast message. * @details \b Message \b Sent: MSG_ID_TD_VALVES_DATA @@ -521,10 +557,9 @@ data.state = (U32)currentValveStates[ valve ].controlState; data.currentPosID = (U32)currentValveStates[ valve ].currentPosition; data.cmdPos = currentValveStates[ valve ].commandedPosition; -// data.posA = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ]; -// data.posB = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ]; -// data.posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - data.maxHomingPos = currentValveStates[ valve ].currentMaxEncPositionCnt; + data.posA = (S16)getPinchValveStoredPosition( valve, VALVE_POSITION_A_INSERT_EJECT ); + data.posB = (S16)getPinchValveStoredPosition( valve, VALVE_POSITION_B_OPEN ); + data.posC = (S16)getPinchValveStoredPosition( valve, VALVE_POSITION_C_CLOSE ); broadcastData( MSG_ID_TD_VALVES_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&data, sizeof( TD_VALVE_DATA_T ) ); } @@ -580,7 +615,7 @@ memcpy( &payload, message->payload, sizeof(VALVE_PINCH_SET_CMD_PAYLOAD_T) ); if ( ( (VALVE_T)payload.valve < NUM_OF_VALVES ) && ( (VALVE_POSITION_T)payload.pos < NUM_OF_VALVE_POSITIONS ) && - ( (VALVE_POSITION_T)payload.pos < NUM_OF_VALVE_POSITIONS ) ) ) + ( (VALVE_POSITION_T)payload.pos < NUM_OF_VALVE_POSITIONS ) ) { result = setValvePosition( (VALVE_T)payload.valve, (VALVE_POSITION_T)payload.pos ); }