Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -ra811cae0392f1d0f976650ff167945e614b3b854 -r8b819deb3e770c981eed221f63170dd842eedbe3 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision a811cae0392f1d0f976650ff167945e614b3b854) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 8b819deb3e770c981eed221f63170dd842eedbe3) @@ -33,7 +33,7 @@ // ********** private definitions ********** -#define VALVES_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Valves data publish interval. +#define VALVES_DATA_PUB_INTERVAL (TASK_GENERAL_INTERVAL/TASK_GENERAL_INTERVAL) // TODO setup //( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Valves data publish interval. #define DATA_PUBLISH_COUNTER_START_COUNT 13 ///< Valves data publish start counter. #define VALVE_TRANSITION_MIN_TGT_DELTA 10 ///< Minimum encoder position delta from target position to end transition state. #define VALVE_HOME_MIN_POS_CHG 3 ///< Minimum encoder position change to indicate a home operation is still moving toward edge. @@ -43,44 +43,59 @@ #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 +#define ZERO_ENC_DEBOUNCE_TIMEOUT_MS ( 0.25 * MS_PER_SECOND ) ///< Valves zero encoder debounce timeout in milliseconds. /// Valve controller states typedef enum Valve_Control_States { - VALVE_STATE_WAIT_FOR_POST = 0, ///< Valve state wait for POST - VALVE_STATE_RESET_VALVE, - VALVE_STATE_RESET_ENCODER, - VALVE_STATE_ENABLE_VALVE, - VALVE_STATE_HOMING_NOT_STARTED, ///< Valve state homing not started - VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE, ///< Valve state homing find energized edge - VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE, ///< Valve state homing find de-energized edge - VALVE_STATE_IDLE, ///< Valve state idle - VALVE_STATE_IN_TRANSITION, ///< Valve state in transition - NUM_OF_VALVE_STATES, ///< Number of valve exec states + VALVE_STATE_WAIT_FOR_POST = 0, ///< Valve state wait for POST. + VALVE_STATE_RESET_VALVE, ///< Valve state reset valve. + VALVE_STATE_RESET_ENCODER, ///< Valve state reset encoder. + VALVE_STATE_ENABLE_VALVE, ///< Valve state enable valve. + VALVE_STATE_HOMING_NOT_STARTED, ///< Valve state homing not started. + VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE, ///< Valve state homing find energized edge. + VALVE_STATE_IDLE, ///< Valve state idle. + VALVE_STATE_IN_TRANSITION, ///< Valve state in transition. + NUM_OF_VALVE_STATES, ///< Number of valve exec states. } VALVE_STATE_T; +/// Valve homing status +typedef enum Valve_Homing_Status +{ + VALVE_HOMING_INIT = 0, ///< Valve homing init. + VALVE_HOMING_FORWARD, ///< Valve homing forward. + VALVE_HOMING_REVERSE, ///< Valve homing reverse. + VALVE_HOMING_COMPLETE, ///< Valve homing complete. + NUM_OF_VALVE_HOMING_STATE, ///< Number of valve homing. +} VALVE_HOMING_STATUS_T; + /// Valve status structure typedef struct { - VALVE_POSITION_T commandedPosition; ///< Valve commanded position enum - VALVE_POSITION_T currentPosition; ///< Valve current position enum - VALVE_POSITION_T pendingCommandedPosition; ///< Valve pending position enum - S16 targetEncPosition; ///< Valve target position in encoder counts - S16 currentEncPosition; ///< Valve current position in encoder counts - S16 priorEncPosition; ///< Valve prior position in encoder counts - BOOL hasTransitionBeenRequested; ///< Valve transition request flag - VALVE_STATE_T controlState; ///< Valve control state - U32 transitionStartTime; ///< Valve transition start time - S16 positionsABC[ NUM_OF_VALVE_POSITIONS ]; ///< Valve positions (in encoder counts) for positions A, B, and C - U32 overCurrentCounter; ///< Valve over current counter - U32 positionOutOfRangeCounter; ///< Valve position out of range counter - BOOL haveValvesBeenReset; + VALVE_POSITION_T commandedPosition; ///< Valve commanded position enum. + VALVE_POSITION_T currentPosition; ///< Valve current position enum. + VALVE_POSITION_T pendingCommandedPosition; ///< Valve pending position enum. + S16 currentEncPosition; ///< Valve current position in encoder counts. + S16 priorEncPosition; ///< Valve prior position in encoder counts. + BOOL hasTransitionBeenRequested; ///< Valve transition request flag. + VALVE_STATE_T controlState; ///< Valve control state. + U32 valveOpsStartTime; ///< Valve transition start time. + S16 positionsABC[ NUM_OF_VALVE_POSITIONS ]; ///< Valve positions (in encoder counts) for positions A, B, and C. + U32 overCurrentCounter; ///< Valve over current counter. + U32 positionOutOfRangeCounter; ///< Valve position out of range counter. // Homing variables - U32 numberOfFailedHomings; ///< Valve number of failed homing - U32 homingEdgeDetectionCounter; ///< Valve homing counter when there is not much travel - BOOL hasHomingBeenRequested; ///< Valve homing request flag - BOOL hasValveBeenHomed; ///< Valve homing completed flag - BOOL hasHomingFailed; ///< Valve homing failed flag + U32 numberOfFailedHomings; // TODO remove? ///< Valve number of failed homing + U32 homingEdgeDetectionCounter; // TODO remove? ///< Valve homing counter when there is not much travel + BOOL hasHomingBeenRequested; ///< Valve homing request flag. + BOOL hasValveBeenHomed; ///< Valve homing completed flag. + BOOL hasHomingFailed; // TODO remove? ///< Valve homing failed flag + S16 proposedEnergizedPos; ///< Valve homing proposed energized position. + BOOL hasValveBeenReset; ///< Valve homing has valve been reset. + VALVE_HOMING_STATUS_T valveHomingStatus; ///< Valve homing status. + + S16 testREMOVEARRAY[ 60 ]; + U32 indexREMOVE; } VALVE_STATUS_T; /// Payload record structure for pinch valve set position request @@ -114,7 +129,6 @@ static VALVE_STATE_T handleValvesEnableValve( VALVE_T valve ); static VALVE_STATE_T handleValvesNotHomedState( VALVE_T valve ); static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ); -static VALVE_STATE_T handleValvesFindDeenergizedEdgeState( VALVE_T valve ); static VALVE_STATE_T handleValvesIdleState( VALVE_T valve ); static VALVE_STATE_T handleValvesTransitionState( VALVE_T valve ); @@ -127,7 +141,7 @@ * @details \b Outputs: Valves unit initialized * @return none *************************************************************************/ -void initValves(void) +void initValves( void ) { // Initialize driver initRotaryValvesDriver(); @@ -258,53 +272,56 @@ for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { - // update valve encoder positions - currentValveStates[ valve ].priorEncPosition = currentValveStates[ valve ].currentEncPosition; - currentValveStates[ valve ].currentEncPosition = getValveEncoderPosition( valve ); - - // execute valve state machine - switch ( currentValveStates[ valve ].controlState ) + if ( valve < NUM_OF_VALVES ) { - case VALVE_STATE_WAIT_FOR_POST: - currentValveStates[ valve ].controlState = handleValvesWait4PostState( valve ); - break; + // update valve encoder positions + currentValveStates[ valve ].priorEncPosition = currentValveStates[ valve ].currentEncPosition; + currentValveStates[ valve ].currentEncPosition = getValveEncoderPosition( valve ); - case VALVE_STATE_RESET_VALVE: - currentValveStates[ valve ].controlState = handleValvesResetValve( valve ); - break; + // execute valve state machine + switch ( currentValveStates[ valve ].controlState ) + { + case VALVE_STATE_WAIT_FOR_POST: + currentValveStates[ valve ].controlState = handleValvesWait4PostState( valve ); + break; - case VALVE_STATE_RESET_ENCODER: - currentValveStates[ valve ].controlState = handleValvesResetEncoder( valve ); - break; + case VALVE_STATE_RESET_VALVE: + currentValveStates[ valve ].controlState = handleValvesResetValve( valve ); + break; - case VALVE_STATE_ENABLE_VALVE: - currentValveStates[ valve ].controlState = handleValvesEnableValve( valve ); - break; + case VALVE_STATE_RESET_ENCODER: + currentValveStates[ valve ].controlState = handleValvesResetEncoder( valve ); + break; - case VALVE_STATE_HOMING_NOT_STARTED: - currentValveStates[ valve ].controlState = handleValvesNotHomedState( valve ); - break; + case VALVE_STATE_ENABLE_VALVE: + currentValveStates[ valve ].controlState = handleValvesEnableValve( valve ); + break; - case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: - currentValveStates[ valve ].controlState = handleValvesFindEnergizedEdgeState( valve ); - break; + case VALVE_STATE_HOMING_NOT_STARTED: + currentValveStates[ valve ].controlState = handleValvesNotHomedState( valve ); + break; - case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: - currentValveStates[ valve ].controlState = handleValvesFindDeenergizedEdgeState( valve ); - break; + case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: + currentValveStates[ valve ].controlState = handleValvesFindEnergizedEdgeState( valve ); + break; - case VALVE_STATE_IDLE: - currentValveStates[ valve ].controlState = handleValvesIdleState( valve ); - break; + case VALVE_STATE_IDLE: + currentValveStates[ valve ].controlState = handleValvesIdleState( valve ); + break; - case VALVE_STATE_IN_TRANSITION: - currentValveStates[ valve ].controlState = handleValvesTransitionState( valve ); - break; + case VALVE_STATE_IN_TRANSITION: + currentValveStates[ valve ].controlState = handleValvesTransitionState( valve ); + break; - default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_STATE, (U32)currentValveStates[ valve ].controlState ) - break; + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_STATE, (U32)currentValveStates[ valve ].controlState ) + break; + } } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) + } } publishValvesData(); @@ -314,7 +331,6 @@ * @brief * The handleValvesWait4PostState function handles the Wait for POST state * of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. * @details \b Inputs: current operating mode * @details \b Outputs: none * @param valve ID of valve for which to handle the Wait for POST state @@ -328,21 +344,23 @@ VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; #endif - if ( valve < NUM_OF_VALVES ) + if ( getCurrentOperationMode() != MODE_INIT ) { - if ( getCurrentOperationMode() != MODE_INIT ) - { - nextState = VALVE_STATE_RESET_VALVE; - } + nextState = VALVE_STATE_RESET_VALVE; } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) - } return nextState; } +/*********************************************************************//** + * @brief + * The handleValvesResetValve function handles the reset valve of the state + * machine for a given valve. + * @details \b Inputs: current operating mode + * @details \b Outputs: none + * @param valve ID of valve for which to handle the Wait for POST state + * @return next state of the state machine for the given valve + *************************************************************************/ static VALVE_STATE_T handleValvesResetValve( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_RESET_ENCODER; @@ -360,14 +378,22 @@ break; default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) + // Do nothing as the fault mode is checked in the exec function break; } return nextState; } - +/*********************************************************************//** + * @brief + * The handleValvesResetEncoder function handles the reset encoder of the state + * machine for a given valve. + * @details \b Inputs: current operating mode + * @details \b Outputs: none + * @param valve ID of valve for which to handle the Wait for POST state + * @return next state of the state machine for the given valve + *************************************************************************/ static VALVE_STATE_T handleValvesResetEncoder( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_ENABLE_VALVE; @@ -385,42 +411,75 @@ break; default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) + // Do nothing as the fault mode is checked in the exec function break; } + // Set the timer for the first debounce read + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + + memset( ¤tValveStates[ valve ].testREMOVEARRAY[0], 0, sizeof( 60 ) ); // TODO remove + currentValveStates[ valve ].indexREMOVE = 0; // TODO remove + return nextState; } +/*********************************************************************//** + * @brief + * The handleValvesEnableValve function handles the enable valve 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 Wait for POST state + * @return next state of the state machine for the given valve + *************************************************************************/ static VALVE_STATE_T handleValvesEnableValve( VALVE_T valve ) { - VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; + VALVE_STATE_T nextState = VALVE_STATE_ENABLE_VALVE; U08 enableValve = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_ENABLE_ENCODER | FPAG_PINCH_VALVES_ENABLE | FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; + S16 valveEncPosistion = getValveEncoderPosition( valve ); - switch ( valve ) + currentValveStates[ valve ].testREMOVEARRAY[ currentValveStates[ valve ].indexREMOVE ] = valveEncPosistion; + currentValveStates[ valve ].indexREMOVE++; + + if ( FALSE == currentValveStates[ valve ].hasValveBeenReset ) { - case H1_VALV: - setH1Control( enableValve ); - break; + switch ( valve ) + { + case H1_VALV: + setH1Control( enableValve ); + break; - case H19_VALV: - setH19Control( enableValve ); - break; + case H19_VALV: + setH19Control( enableValve ); + break; - default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) - break; + default: + // Do nothing as the fault mode is checked in the exec function + break; + } + + currentValveStates[ valve ].hasValveBeenReset = TRUE; } + if ( valveEncPosistion >= ZERO_ENC_DEBOUNCE_THRESHOLD_CNT ) + { + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + } + else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, ZERO_ENC_DEBOUNCE_TIMEOUT_MS ) ) + { + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = currentValveStates[ valve ].currentEncPosition + ROTARY_VALVE_MICROSTEP_FRACTION; + nextState = VALVE_STATE_HOMING_NOT_STARTED; + } + return nextState; } /*********************************************************************//** * @brief * The handleValvesNotHomedState function handles the Not Homed state * of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[] * @param valve ID of valve for which to handle the Not Homed state @@ -430,26 +489,36 @@ { VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; - if ( valve < NUM_OF_VALVES ) + if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) + switch ( currentValveStates[ valve ].valveHomingStatus ) { - U16 mag = ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS; + case VALVE_HOMING_INIT: + currentValveStates[ valve ].proposedEnergizedPos = ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS; + break; - // command valve to move to energized edge (end of travel) - currentValveStates[ valve ].homingEdgeDetectionCounter = 0; - currentValveStates[ valve ].targetEncPosition = mag; - currentValveStates[ valve ].hasValveBeenHomed = FALSE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - currentValveStates[ valve ].transitionStartTime = 0; - setValveCmdChangePosition( valve, (U16)mag, MOTOR_DIR_FORWARD ); - nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + case VALVE_HOMING_FORWARD: + currentValveStates[ valve ].proposedEnergizedPos += ROTARY_VALVE_MICROSTEP_FRACTION; + break; + + case VALVE_HOMING_REVERSE: + currentValveStates[ valve ].proposedEnergizedPos -= ROTARY_VALVE_MICROSTEP_FRACTION; + break; + + default: + // Do nothing as complete has nothing to do + break; } + + // command valve to move to energized edge (end of travel) + currentValveStates[ valve ].valveOpsStartTime = 0; + currentValveStates[ valve ].homingEdgeDetectionCounter = 0; // TODO remove? + currentValveStates[ valve ].hasValveBeenHomed = FALSE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + + setValveCmdChangePosition( valve, (U16)currentValveStates[ valve ].proposedEnergizedPos, MOTOR_DIR_FORWARD ); + nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE3, (U32)valve ) - } return nextState; } @@ -458,7 +527,6 @@ * @brief * The handleValvesFindEnergizedEdgeState function handles the Find Energized * Edge state of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[] * @param valve ID of valve for which to handle the Find Energized Edge state @@ -467,102 +535,70 @@ static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + S16 curPos = currentValveStates[ valve ].currentEncPosition; - if ( valve < NUM_OF_VALVES ) + // have we found forward edge of travel? + if ( abs( curPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) { - S16 curPos = currentValveStates[ valve ].currentEncPosition; - - // have we found forward edge of travel? - if ( fabs( curPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) + if ( 0 == currentValveStates[ valve ].valveOpsStartTime ) { - if ( 0 == currentValveStates[ valve ].transitionStartTime ) + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + } + else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) + { + if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) { - currentValveStates[ valve ].transitionStartTime = getMSTimerCount(); + S16 tgtPos = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + S16 mag = curPos - tgtPos; // calculate magnitude of position change + + // Set commanded position and transition start time stamp + // Transition back to position C to try the next energized position. It has to start from position C + currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE; + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_FORWARD; + nextState = VALVE_STATE_IN_TRANSITION; + // Command FPGA to move valve to commanded position + setValveCmdChangePosition( valve, (U16)(mag), MOTOR_DIR_REVERSE ); } - else if ( TRUE == didTimeout( currentValveStates[ valve ].transitionStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) + else { - U16 mag = ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS + VALVE_HOME_MIN_POS_CHG; + S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - // Record edge position - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos - VALVE_HOME_BACK_OFF_EDGE; - // command valve to move to de-energized edge (end of travel) - currentValveStates[ valve ].homingEdgeDetectionCounter = 0; - currentValveStates[ valve ].targetEncPosition = 0; - currentValveStates[ valve ].transitionStartTime = 0; - setValveCmdChangePosition( valve, mag, MOTOR_DIR_REVERSE ); - nextState = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posC + ( ( curPos - posC ) / 2 ); + currentValveStates[ valve ].hasValveBeenHomed = TRUE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_B_OPEN; + currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; + currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_COMPLETE; + currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; + currentValveStates[ valve ].hasHomingBeenRequested = FALSE; + nextState = VALVE_STATE_IDLE; } } } - else + else if ( curPos <= 0 ) { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE4, (U32)valve ) - } + switch ( currentValveStates[ valve ].valveHomingStatus ) + { + case VALVE_HOMING_INIT: + case VALVE_HOMING_FORWARD: + currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_REVERSE; + break; - return nextState; -} + default: + // If init or forward modes, then reverse again. + break; + } -/*********************************************************************//** - * @brief - * The handleValvesFindDeenergizedEdgeState function handles the Find - * De-energized Edge state of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. - * @details \b Inputs: - * @details \b Outputs: - * @param valve ID of valve for which to handle the Find De-energized Edge state - * @return next state of the state machine for the given valve - *************************************************************************/ -static VALVE_STATE_T handleValvesFindDeenergizedEdgeState( VALVE_T valve ) -{ - VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; - if ( valve < NUM_OF_VALVES ) - { - S16 curPos = currentValveStates[ valve ].currentEncPosition; + //if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) + //{ + // currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_REVERSE; + //} - // have we found forward edge of travel? - if ( fabs( currentValveStates[ valve ].priorEncPosition - curPos ) < VALVE_HOME_MIN_POS_CHG ) - { - S16 posC = curPos + VALVE_HOME_BACK_OFF_EDGE; - S16 edgeDelta = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] - posC; - U32 expDelta = abs( (S16)ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS - edgeDelta ); - - // verify edge to edge travel is reasonable - if ( expDelta <= MAX_HOME_FULL_TRAVEL_DIFF ) - { - // Record edge position - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = posC; - // calculate and record middle position - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] + \ - ( ( currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] ) / 2 ); - // Set the current position to Position C and the commanded position to Position A - currentValveStates[ valve ].hasValveBeenHomed = TRUE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; - currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; - currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; - currentValveStates[ valve ].hasHomingBeenRequested = FALSE; - currentValveStates[ valve ].numberOfFailedHomings = 0; - nextState = VALVE_STATE_IDLE; - } - // if home failed, attempt retry or fault if out of retries - else - { - currentValveStates[ valve ].numberOfFailedHomings++; - if ( currentValveStates[ valve ].numberOfFailedHomings > MAX_ALLOWED_FAILED_VALVE_HOMINGS ) - { - currentValveStates[ valve ].hasValveBeenHomed = FALSE; - currentValveStates[ valve ].hasHomingFailed = TRUE; - currentValveStates[ valve ].hasHomingBeenRequested = FALSE; - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_HOMING_FAILED, (U32)valve, (U32)edgeDelta ); - } - nextState = VALVE_STATE_RESET_VALVE; - } - } + currentValveStates[ valve ].hasValveBeenReset = FALSE; + nextState = VALVE_STATE_RESET_VALVE; } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE5, (U32)valve ) - } return nextState; } @@ -571,7 +607,6 @@ * @brief * The handleValvesIdleState function handles the Idle state of the state * machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[], cmd to FPGA * @param valve ID of valve for which to handle the Idle state @@ -581,38 +616,35 @@ { VALVE_STATE_T nextState = VALVE_STATE_IDLE; - if ( valve < NUM_OF_VALVES ) + // handle a home request + if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - // handle a home request - if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) + currentValveStates[ valve ].hasValveBeenReset = FALSE; + currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_INIT; + + // TODO Check if reset is not needed first + nextState = VALVE_STATE_RESET_VALVE; + } + // handle a position change request + else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) + { + currentValveStates[ valve ].hasTransitionBeenRequested = FALSE; + if ( currentValveStates[ valve ].currentPosition != currentValveStates[ valve ].pendingCommandedPosition ) { - nextState = VALVE_STATE_RESET_VALVE; - } - // handle a position change request - else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) - { - currentValveStates[ valve ].hasTransitionBeenRequested = FALSE; - if ( currentValveStates[ valve ].currentPosition != currentValveStates[ valve ].pendingCommandedPosition ) - { - S16 curPos = currentValveStates[ valve ].currentEncPosition; - S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].pendingCommandedPosition ]; - S16 mag = tgtPos - curPos; // calculate magnitude of position change - MOTOR_DIR_T dir = ( mag < 0 ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); // determine direction of position change + S16 curPos = currentValveStates[ valve ].currentEncPosition; + S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].pendingCommandedPosition ]; + S16 mag = tgtPos - curPos; // calculate magnitude of position change + MOTOR_DIR_T dir = ( mag < 0 ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); // determine direction of position change - // set commanded position and transition start time stamp - currentValveStates[ valve ].commandedPosition = currentValveStates[ valve ].pendingCommandedPosition; - currentValveStates[ valve ].transitionStartTime = getMSTimerCount(); - // command FPGA to move valve to commanded position - setValveCmdChangePosition( valve, (U16)( abs(mag) ), dir ); - // go on to transition state - nextState = VALVE_STATE_IN_TRANSITION; - } + // set commanded position and transition start time stamp + currentValveStates[ valve ].commandedPosition = currentValveStates[ valve ].pendingCommandedPosition; + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + // command FPGA to move valve to commanded position + setValveCmdChangePosition( valve, (U16)( abs(mag) ), dir ); + // go on to transition state + nextState = VALVE_STATE_IN_TRANSITION; } } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE6, (U32)valve ) - } return nextState; } @@ -621,7 +653,6 @@ * @brief * The handleValvesTransitionState function handles the Transition state * of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[] * @param valve ID of valve for which to handle the Transition state @@ -630,40 +661,32 @@ static VALVE_STATE_T handleValvesTransitionState( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_IN_TRANSITION; + S16 curPos = currentValveStates[ valve ].currentEncPosition; + S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].commandedPosition ]; + S16 delta = tgtPos - curPos; - if ( valve < NUM_OF_VALVES ) + // have we completed the transition? + if ( abs( delta ) < VALVE_TRANSITION_MIN_TGT_DELTA ) { - S16 curPos = currentValveStates[ valve ].currentEncPosition; - S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].commandedPosition ]; - S16 delta = tgtPos - curPos; + currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; + nextState = ( VALVE_HOMING_COMPLETE != currentValveStates[ valve ].valveHomingStatus ? VALVE_STATE_HOMING_NOT_STARTED : VALVE_STATE_IDLE ); + SEND_EVENT_WITH_2_U32_DATA( TD_EVENT_VALVE_POSITION_DELTA_REMOVE_TEMP, (U32)abs( delta ), 0 ); + } + // is transition taking too long? + else if ( ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) && + ( getCPLDACPowerLossDetected() != TRUE ) ) + { + // Go back to Idle state and set the valve position to not in position because it was not able to get to the target position + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + nextState = VALVE_STATE_IDLE; + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_TRANSITION_TIMEOUT, (U32)valve, (U32)abs( delta ) ); - // have we completed the transition? - if ( abs( delta ) < VALVE_TRANSITION_MIN_TGT_DELTA ) + // If the valve's commanded position is C, activate safety shutdown + if ( VALVE_POSITION_C_CLOSE == currentValveStates[ valve ].commandedPosition ) { - currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; - nextState = VALVE_STATE_IDLE; - SEND_EVENT_WITH_2_U32_DATA( TD_EVENT_VALVE_POSITION_DELTA_REMOVE_TEMP, (U32)abs( delta ), 0 ); + activateSafetyShutdown(); } - // is transition taking too long? - else if ( ( TRUE == didTimeout( currentValveStates[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) && - ( getCPLDACPowerLossDetected() != TRUE ) ) - { - // Go back to Idle state and set the valve position to not in position because it was not able to get to the target position - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - nextState = VALVE_STATE_IDLE; - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_TRANSITION_TIMEOUT, (U32)valve, (U32)abs( delta ) ); - - // If the valve's commanded position is C, activate safety shutdown - if ( VALVE_POSITION_C_CLOSE == currentValveStates[ valve ].commandedPosition ) - { - activateSafetyShutdown(); - } - } } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE7, (U32)valve ) - } return nextState; } @@ -716,7 +739,10 @@ data.state = (U32)currentValveStates[ valve ].controlState; data.currentPosID = (U32)currentValveStates[ valve ].currentPosition; data.currentPos = getValveEncoderPosition( valve ); - data.cmdPos = (U32)currentValveStates[ valve ].commandedPosition; + 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 ]; broadcastData( MSG_ID_TD_VALVES_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&data, sizeof( TD_VALVE_DATA_T ) ); }