Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r9224712d825c7ecee623abc78d704fbd3c2dbde8 -r361e8cae2edec18e5ba1a26690788b5c30ce4833 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 9224712d825c7ecee623abc78d704fbd3c2dbde8) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 361e8cae2edec18e5ba1a26690788b5c30ce4833) @@ -57,17 +57,19 @@ #define INITIAL_ENERGIZED_EDGE_UPPER_RANGE 13000U #define INITIAL_ENERGIZED_EDGE_LOWER_RANGE 9000U +#define AIR_TRAP_VALVE_GPIO_PIN 0x12 + #define STEP_CHANGE_IN_COUNTS 1000 -//#define MAX_POS_DEVIATION_FROM_TARGET_COUNTS 1000 //TODO test this tolerance -//#define VALVE_TRANSITION_TIMEOUT_MS 5000 //TODO test the timeout + #define MAX_DEVIATION_FROM_TARGET_IN_COUNTS 150U #define MAX_ALLOWED_FAILED_HOMINGS 3U -#define HOMING_EDGE_DETECTION_TARGET_COUNTER 12U +#define HOMING_EDGE_DETECTION_TIME_INTERVAL ( TASK_PRIORITY_INTERVAL + 2U ) ///< The time that the valve must be at the edge to be considered for edge detection #define VALVES_CURRENT_THRESHOLD_AMPS 1U #define OVER_CURRENT_COUNTER 100U #define OUT_OF_RANGE_COUNTER 100U +#define VALVE_TRANSITION_TIMEOUT_MS 3000U //TODO experiment with this -#define NEXT_STEP_COMD_TIME_INTERVAL_COUNTER 10U +#define NEXT_STEP_COMD_TIME_INTERVAL_COUNTER ( MS_PER_SECOND / ( 10 * TASK_PRIORITY_INTERVAL ) ) // TODO changed the interval count to every 10 calls in priority task which is 100 ms #define VALVES_DATA_PUB_INTERVAL ( MS_PER_SECOND / ( 10 * TASK_PRIORITY_INTERVAL ) ) @@ -87,11 +89,11 @@ typedef enum valve_Exec_States { VALVE_STATE_WAIT_FOR_POST = 0, - VALVE_STATE_IDLE, - VALVE_STATE_IN_TRANSITION, VALVE_STATE_HOMING_NOT_STARTED, VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE, VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE, + VALVE_STATE_IDLE, + VALVE_STATE_IN_TRANSITION, NUM_OF_VALVE_STATES, } VALVE_STATE_T; @@ -105,7 +107,7 @@ S16 targetPositionInCounts; BOOL hasTransitionBeenRequested; VALVE_STATE_T execState; - U32 transitionStartTime; //TODO REMOVE? + U32 transitionStartTime; S16 positions[ NUM_OF_VALVE_POSITIONS ]; F32 valveCurrent; U32 valveOverCurrentCounter; @@ -117,6 +119,7 @@ U32 homingEdgeDetectionCounter; BOOL hasHomingBeenRequested; BOOL hasValveBeenHomed; + BOOL hasHomingFailed; } VALVE_STATUS_T; #pragma pack(pop) @@ -134,12 +137,12 @@ static VALVE_SELF_TEST_STATE_T handleValveSelfTestConfirmEnable( void ); // Exec function prototypes -static void handleValveStateWaitForPost( VALVE_T valve ); -static void handleValveStateIdle( VALVE_T valve ); -static void handleValveStateInTransition( VALVE_T valve ); -static void handleValveStateHomingNotStarted( VALVE_T valve ); -static void handleValveStateHomingFindEnergizedEdge( VALVE_T valve ); -static void handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ); +static VALVE_STATE_T handleValveStateWaitForPost( VALVE_T valve ); +static VALVE_STATE_T handleValveStateHomingNotStarted( VALVE_T valve ); +static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve ); +static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ); +static VALVE_STATE_T handleValveStateIdle( VALVE_T valve ); +static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve ); // Private function prototypes static void execMonitorValves( void ); @@ -160,18 +163,14 @@ *************************************************************************/ void initValves( void ) { - U08 i; - - // To set the NHET direction to output - //gioSetDirection( hetPORT1, 1 ); - + VALVE_T valve; valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES; valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS; // Initialize some of the variables - for ( i = 0; i < NUM_OF_VALVES; i++ ) + for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) { - valvesStatus[ i ].execState = VALVE_STATE_WAIT_FOR_POST; + valvesStatus[ valve ].execState = VALVE_STATE_WAIT_FOR_POST; } } @@ -234,12 +233,12 @@ * The setValveBloodTrap function set the blood trap valve to open or close * position. * @details - * Inputs: TODO - * Outputs: TODO - * @param: state : Enumeration of open or close - * @return: TODO : What should we return? + * Inputs: none + * Outputs: none + * @param: state enumeration of open or close for valve + * @return: none *************************************************************************/ -BOOL setValveBloodTrap( OPN_CLS_STATE_T state ) +void setValveAirTrap( OPN_CLS_STATE_T state ) { BOOL energized = FALSE; @@ -248,9 +247,7 @@ energized = TRUE; } - gioSetBit( hetPORT1, 0x12, energized ); - - return TRUE; + gioSetBit( hetPORT1, AIR_TRAP_VALVE_GPIO_PIN, energized ); } /*********************************************************************//** @@ -295,60 +292,59 @@ *************************************************************************/ void execValves( void ) { - U08 i; - VALVE_STATE_T state; VALVE_T valve; + VALVE_STATE_T state; execMonitorValves(); - // TODO uncomment the for loop - //for ( i = 0; i < NUM_OF_VALVES; i++ ) + // TODO un-comment the for loop + for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) { //TODO FOR TESTING ONLY REMOVE - valve = VDI; - i = 0; + //valve = VDI; + //i = 0; //TODO FOR TESTING ONLY REMOVE - state = valvesStatus[ i ].execState; //valve = (VALVE_T)i; - // VDi state machine - switch ( state ) + switch ( valvesStatus[ valve ].execState ) { case VALVE_STATE_WAIT_FOR_POST: - handleValveStateWaitForPost( valve ); + state = handleValveStateWaitForPost( valve ); break; - case VALVE_STATE_IDLE: + case VALVE_STATE_HOMING_NOT_STARTED: - handleValveStateIdle( valve ); + state = handleValveStateHomingNotStarted( valve ); break; - case VALVE_STATE_IN_TRANSITION: + case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: - handleValveStateInTransition( valve ); + state = handleValveStateHomingFindEnergizedEdge( valve ); break; - case VALVE_STATE_HOMING_NOT_STARTED: + case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: - handleValveStateHomingNotStarted( valve ); + state = handleValveStateHomingFindDeenergizedEdge( valve ); break; - case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: + case VALVE_STATE_IDLE: - handleValveStateHomingFindEnergizedEdge( valve ); + state = handleValveStateIdle( valve ); break; - case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: + case VALVE_STATE_IN_TRANSITION: - handleValveStateHomingFindDeenergizedEdge( valve ); + state = handleValveStateInTransition( valve ); break; default: break; } + valvesStatus[ valve ].execState = state; + publishValvesData( valve ); } } @@ -369,10 +365,6 @@ // Enable the PID controller of all of the valves enableFPGAValvesPIDControl( ALL_VALVES_ENABLE_PID_INTERFACE ); - /*(enableFPGAValvesPIDControl( VDI_ENABLE_PID_INTERFACE ); - enableFPGAValvesPIDControl( VDO_ENABLE_PID_INTERFACE ); - enableFPGAValvesPIDControl( VBA_ENABLE_PID_INTERFACE ); - enableFPGAValvesPIDControl( VBV_ENABLE_PID_INTERFACE );*/ return state; } @@ -414,7 +406,7 @@ * Outputs: valveSelfTestState (the next exec state of a valve) * @return: none *************************************************************************/ -static void handleValveStateWaitForPost( VALVE_T valve ) +static VALVE_STATE_T handleValveStateWaitForPost( VALVE_T valve ) { VALVE_STATE_T state = VALVE_STATE_WAIT_FOR_POST; @@ -423,130 +415,19 @@ state = VALVE_STATE_HOMING_NOT_STARTED; } - valvesStatus[ valve ].execState = state; + return state; } /*********************************************************************//** * @brief - * The handleValveStateIdle function handles the idle state of the valves \n - * state machine. - * @details - * Inputs: valveSelfTestState, valveSelfTestState - * Outputs: valveSelfTestState (the next exec state of a valve) - * @return: none - *************************************************************************/ -static void handleValveStateIdle( VALVE_T valve ) -{ - VALVE_STATE_T state = VALVE_STATE_IDLE; - - if ( valvesStatus[ valve ].hasHomingBeenRequested ) - { - state = VALVE_STATE_HOMING_NOT_STARTED; - } - else if ( valvesStatus[ valve ].hasValveBeenHomed && - valvesStatus[ valve ].hasTransitionBeenRequested ) - { - if ( valvesStatus[ valve ].currentPosition != valvesStatus[ valve ].commandedPosition ) - { - VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; - S16 nextStep = valvesStatus[ valve ].positions[commandedPositionEnum ]; - // Just set the valves to transition so it will not be in a known position for a while - valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); - //valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - setFPGAValue( valve, nextStep ); - - state = VALVE_STATE_IN_TRANSITION; - } - } - - valvesStatus[ valve ].execState = state; -} - -/*********************************************************************//** - * @brief - * The handleValveStateInTransition function handles the in transition \n - * state of the state machine. - * @details - * Inputs: valvesStatus (commanded position, current position, current position \n - * in counts, has transition been requested flag) - * Outputs: valvesStatus - * @return: none - *************************************************************************/ -static void handleValveStateInTransition( VALVE_T valve ) -{ - VALVE_STATE_T state = VALVE_STATE_IN_TRANSITION; - - S16 nextStep = 0; - VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; - VALVE_POSITION_T currentPositionEnum = valvesStatus[ valve ].currentPosition; - - // Get the corresponding counts of the positions - S16 commandedPositionCounts = valvesStatus[ valve ].positions[ commandedPositionEnum ]; - S16 currentPositionCounts = valvesStatus[ valve ].currentPositionInCounts; - - // Check if the valve is within range of the commanded position - if ( fabs( fabs(commandedPositionCounts) - fabs(currentPositionCounts) ) <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) - { - // The valve is in range. Set the current position to the commanded position - valvesStatus[ valve ].currentPosition = commandedPositionEnum; - // Set the transition request to false as it has been serviced - valvesStatus[ valve ].hasTransitionBeenRequested = FALSE; - // Go back to Idle state - state = VALVE_STATE_IDLE; - } - // Check if the valve's transition time has timed out - /*else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) - { - // TODO raise an alarm - valvesStatus[ valve ].hasTransitionBeenRequested = FALSE; - // Go back to Idle state - state = VALVE_STATE_IDLE; - }*/ - // If everything is good, decide what is the next step - else - { - // If the valve is currently in position A and its commanded position is position B, - // subtract the defined number of steps for the next transition - // If the valve is currently in position A and its commanded position is position C, - // add the defined number of steps for the next transition - if ( currentPositionEnum == VALVE_POSITION_A_INSERT_EJECT ) - { - if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) - { - nextStep = valvesStatus[ valve ].currentPositionInCounts - STEP_CHANGE_IN_COUNTS; - } - if ( commandedPositionEnum == VALVE_POSITION_C_CLOSE ) - { - nextStep = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS; - } - } - // If the valve is currently in position B, add the defined number of steps for the next transition - else if ( currentPositionEnum == VALVE_POSITION_B_OPEN ) - { - nextStep = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS; - } - // If the valve is currently in position C, subtract the defined number of steps for the next transition - else if ( currentPositionEnum == VALVE_POSITION_C_CLOSE ) - { - nextStep = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS; - } - - setFPGAValue( valve, nextStep ); - } - - valvesStatus[ valve ].execState = state; -} - -/*********************************************************************//** - * @brief * The handleValveStateHomingNotStarted function handles homing not started \n * state of the state machine. * @details * Inputs: valvesStatus, valveSelfTestState * Outputs: valvesStatus * @return: none *************************************************************************/ -static void handleValveStateHomingNotStarted( VALVE_T valve ) +static VALVE_STATE_T handleValveStateHomingNotStarted( VALVE_T valve ) { VALVE_STATE_T state = VALVE_STATE_HOMING_NOT_STARTED; @@ -557,18 +438,18 @@ // If homing has been requested or POST is completed and the door has been close for the specified // period of time, start the homing if ( ( valvesStatus[ valve ].hasHomingBeenRequested || valveSelfTestState == VALVE_SELF_TEST_COMPLETE ) - && isDoorClosed ) + && isDoorClosed && ! valvesStatus[ valve ].hasHomingFailed ) { // Get ready for the energized state valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = STEP_CHANGE_IN_COUNTS; + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPosition + STEP_CHANGE_IN_COUNTS; valvesStatus[ valve ].hasValveBeenHomed = FALSE; valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; } - valvesStatus[ valve ].execState = state; + return state; } /*********************************************************************//** @@ -580,17 +461,22 @@ * Outputs: valvesStatus * @return: none *************************************************************************/ -static void handleValveStateHomingFindEnergizedEdge( VALVE_T valve ) +static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve ) { - VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + BOOL hasStatusChanged = FALSE; + VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; + S16 deltaPosition = currentPosition - targetPosition; - S16 absDeltaPosition = fabs( fabs(currentPosition) - fabs(targetPosition) ); + //S16 absDeltaPosition = fabs( fabs(currentPosition) - fabs(targetPosition) ); + //if ( absDeltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + // ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) TODO Remove - if ( absDeltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && - ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) + // If there has not been any major travel for a certain period of time + if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) { // Current position (positive or negative) will be stored in the Position B of the current valve valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition; @@ -600,17 +486,23 @@ state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } - else if ( absDeltaPosition <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + else if ( currentPosition >= targetPosition ) { - if ( ++valvesStatus[ valve ].valveRestCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest? - { - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = currentPosition + STEP_CHANGE_IN_COUNTS; - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); - } + hasStatusChanged = TRUE; } + else if ( currentPosition < targetPosition && deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + { + hasStatusChanged = TRUE; + } - valvesStatus[ valve ].execState = state; + if ( ++valvesStatus[ valve ].valveRestCounter >= 0 && hasStatusChanged ) //TODO add a #define for the rest time. Do we need the rest? + { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition + STEP_CHANGE_IN_COUNTS; + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + } + + return state; } /*********************************************************************//** @@ -622,22 +514,26 @@ * Outputs: valvesStatus * @return: none *************************************************************************/ -static void handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ) +static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ) { - VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + BOOL hasStatusChanged = FALSE; + VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; + S16 deltaPosition = currentPosition - targetPosition; - S16 absDeltaPosition = fabs( fabs(currentPosition) - fabs(targetPosition) ); + //S16 absDeltaPosition = fabs( fabs(currentPosition) - fabs(targetPosition) ); + //if ( absDeltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + // ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) TODO remove - if ( absDeltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && - ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) + if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) { S16 energizedEdge = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; - S16 absEnergizedDelta = fabs( fabs(energizedEdge) - fabs(currentPosition) ); + S16 deltaEdges = energizedEdge - currentPosition; - if ( absEnergizedDelta <= INITIAL_ENERGIZED_EDGE_UPPER_RANGE && absEnergizedDelta >= INITIAL_ENERGIZED_EDGE_LOWER_RANGE ) + if ( deltaEdges <= INITIAL_ENERGIZED_EDGE_UPPER_RANGE && deltaEdges >= INITIAL_ENERGIZED_EDGE_LOWER_RANGE ) { S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; // Positions B and C will have an offset from the edge to make sure each time the valve will not hit the edge @@ -647,8 +543,7 @@ valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ] = ( valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] + valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] ) / 2; - // Set the current position to Position C and the commanded position to Position A - // Request a transition + // Set the current position to Position C and the commanded position to Position A // Request a transition valvesStatus[ valve ].hasValveBeenHomed = TRUE; valvesStatus[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; valvesStatus[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; @@ -659,34 +554,162 @@ // Idle will initiate a transition state = VALVE_STATE_IDLE; } - // Max number of failed homing. Fault + // Max number of failed homing. Fault and go back to homing not started else if ( ++valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) { //TODO Alarm valvesStatus[ valve ].hasValveBeenHomed = FALSE; - state = VALVE_STATE_IDLE; + valvesStatus[ valve ].hasHomingFailed = TRUE; + state = VALVE_STATE_HOMING_NOT_STARTED; } else { state = VALVE_STATE_HOMING_NOT_STARTED; } } - else if ( absDeltaPosition <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + else if ( currentPosition > targetPosition && deltaPosition <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { - if ( ++valvesStatus[ valve ].valveRestCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest? + hasStatusChanged = TRUE; + } + else if ( currentPosition < targetPosition ) + { + hasStatusChanged = TRUE; + } + + if ( ++valvesStatus[ valve ].valveRestCounter >= 0 && hasStatusChanged ) //TODO add a #define for the rest time. Do we need the rest? + { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + } + + return state; +} + +/*********************************************************************//** + * @brief + * The handleValveStateIdle function handles the idle state of the valves \n + * state machine. + * @details + * Inputs: valveSelfTestState, valveSelfTestState + * Outputs: valveSelfTestState (the next exec state of a valve) + * @return: none + *************************************************************************/ +static VALVE_STATE_T handleValveStateIdle( VALVE_T valve ) +{ + VALVE_STATE_T state = VALVE_STATE_IDLE; + + if ( valvesStatus[ valve ].hasHomingBeenRequested ) + { + state = VALVE_STATE_HOMING_NOT_STARTED; + } + else if ( valvesStatus[ valve ].hasValveBeenHomed && + valvesStatus[ valve ].hasTransitionBeenRequested ) + { + if ( valvesStatus[ valve ].currentPosition != valvesStatus[ valve ].commandedPosition ) { - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; + S16 nextStep = valvesStatus[ valve ].positions[commandedPositionEnum ]; + // Just set the valves to transition so it will not be in a known position for a while + valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); + setFPGAValue( valve, nextStep ); + + state = VALVE_STATE_IN_TRANSITION; } } - valvesStatus[ valve ].execState = state; + return state; } /*********************************************************************//** * @brief + * The handleValveStateInTransition function handles the in transition \n + * state of the state machine. + * @details + * Inputs: valvesStatus (commanded position, current position, current position \n + * in counts, has transition been requested flag) + * Outputs: valvesStatus + * @return: none + *************************************************************************/ +static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve ) +{ + S16 nextStep = 0; + VALVE_STATE_T state = VALVE_STATE_IN_TRANSITION; + + VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; + VALVE_POSITION_T currentPositionEnum = valvesStatus[ valve ].currentPosition; + + // Get the corresponding counts of the positions + S16 commandedPosition = valvesStatus[ valve ].positions[ commandedPositionEnum ]; + S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; + S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; + + // Check if the valve is within range of the commanded position + //if ( fabs( fabs(commandedPositionCounts) - fabs(currentPositionCounts) ) <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) TODO Remove + if ( abs( currentPosition - commandedPosition ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + { + // The valve is in range. Set the current position to the commanded position + valvesStatus[ valve ].currentPosition = commandedPositionEnum; + // Set the transition request to false as it has been serviced + valvesStatus[ valve ].hasTransitionBeenRequested = FALSE; + // Go back to Idle state + state = VALVE_STATE_IDLE; + } + // Check if the valve's transition time has timed out + else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) + { + // TODO raise an alarm + valvesStatus[ valve ].hasTransitionBeenRequested = FALSE; + // Go back to Idle state + state = VALVE_STATE_IDLE; + } + //TODO not sure if this works or not, Test this + else if ( currentPosition - targetPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + { + if ( currentPosition - commandedPosition <= STEP_CHANGE_IN_COUNTS ) + { + nextStep = abs( currentPosition - commandedPosition ); + } + else + { + nextStep = STEP_CHANGE_IN_COUNTS; + } + + // If the valve is currently in position A and its commanded position is position B, + // add the defined number of steps for the next transition + // If the valve is currently in position A and its commanded position is position C, + // subtract the defined number of steps for the next transition + if ( currentPositionEnum == VALVE_POSITION_A_INSERT_EJECT ) + { + if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; + } + if ( commandedPositionEnum == VALVE_POSITION_C_CLOSE ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; + } + } + // If the valve is currently in position B, subtract the defined number of steps for the next transition + else if ( currentPositionEnum == VALVE_POSITION_B_OPEN ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; + } + // If the valve is currently in position C, add the defined number of steps for the next transition + else if ( currentPositionEnum == VALVE_POSITION_C_CLOSE ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; + } + + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + } + + return state; +} + +/*********************************************************************//** + * @brief * The execMonitorValves function handles monitoring of the valves. The \n * function calls other functions to monitor the values. * @details @@ -696,7 +719,7 @@ *************************************************************************/ static void execMonitorValves( void ) { - // TODO check enable status of the valves here too + // Check if the valves are still enabled areValvesEnabled(); // Get the current position of the valves in counts and store them @@ -835,7 +858,7 @@ *************************************************************************/ static void convertAndMonitorValvesCurrent( void ) { - U08 i; + VALVE_T valve; U16 currentInADC = 0; currentInADC = getFPGAValveDialyzerInletCurrentCounts(); @@ -850,21 +873,22 @@ currentInADC = getFPGAValveBloodArterialCurrentCounts(); valvesStatus[ VBV ].valveCurrent = ( (F32)currentInADC / ADC_TO_CURRENT_CONVERSION_CONSTANT ) - 1; - // The current of all the valves - for ( i = 0; i < NUM_OF_VALVES; i++) + // Check the current of all the valves + for ( valve = VDI; valve < NUM_OF_VALVES; valve++) { // Check if the current is over the threshold for the defined amount of time - if ( fabs(valvesStatus[ i ].valveCurrent) > VALVES_CURRENT_THRESHOLD_AMPS && - ++valvesStatus[ i ].valveOverCurrentCounter > OVER_CURRENT_COUNTER ) + // Absolute value was used to cover both positive and negative ranges + if ( abs(valvesStatus[ valve ].valveCurrent) > VALVES_CURRENT_THRESHOLD_AMPS && + ++valvesStatus[ valve ].valveOverCurrentCounter > OVER_CURRENT_COUNTER ) { //TODO fault } // If the current is below the threshold again and the counter for the time is greater than - // zero, set the counter to zero - else if ( fabs(valvesStatus[ i ].valveCurrent) < VALVES_CURRENT_THRESHOLD_AMPS && - valvesStatus[ i ].valveOverCurrentCounter > 0 ) + // zero, set the counter to zero. Absolute value is used here to cover both positive and negative ranges + else if ( abs(valvesStatus[ valve ].valveCurrent) < VALVES_CURRENT_THRESHOLD_AMPS && + valvesStatus[ valve ].valveOverCurrentCounter > 0 ) { - valvesStatus[ i ].valveOverCurrentCounter = 0; + valvesStatus[ valve ].valveOverCurrentCounter = 0; } } } @@ -883,7 +907,7 @@ *************************************************************************/ static void getAndMonitorValvesCurrentPosition( void ) { - U08 i; + VALVE_T valve; S16 currentPostion = 0; S16 commandedPoistion = 0; VALVE_POSITION_T commandedPositionEnum; @@ -895,29 +919,31 @@ valvesStatus[ VBV ].currentPositionInCounts = getFPGAValveBloodVenousPosition(); // Check the position of each valve - for ( i = 0; i < NUM_OF_VALVES; i++ ) + for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) { // Check the valve is in Idle state, otherwise the position should not be checked - if ( valvesStatus[ i ].execState == VALVE_STATE_IDLE ) + if ( valvesStatus[ valve ].execState == VALVE_STATE_IDLE ) { - currentPostion = fabs( valvesStatus[ i ].currentPositionInCounts ); - commandedPositionEnum = valvesStatus[ i ].commandedPosition; - commandedPoistion = fabs( valvesStatus[ i ].positions[ commandedPositionEnum ] ); + currentPostion = valvesStatus[ valve ].currentPositionInCounts; + commandedPositionEnum = valvesStatus[ valve ].commandedPosition; + commandedPoistion = valvesStatus[ valve ].positions[ commandedPositionEnum ]; // Check if the current position has deviated from the position it is supposed to be in // for more than a certain amount of time. If it has, raise an alarm - if ( fabs( currentPostion - commandedPoistion ) > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && - ++valvesStatus[ i ].valvePositionOutOfRangeCounter > OUT_OF_RANGE_COUNTER ) + // Absolute value is used for comparison to cover +/- from the commanded position + if ( abs( currentPostion - commandedPoistion ) > MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { - // TODO fault + valvesStatus[ valve ].valvePositionOutOfRangeCounter++; } - // If the deviation came back to within the maximum amount and the time counter was > 0, - // set the counter to 0 - else if ( fabs( currentPostion - commandedPoistion ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS && - valvesStatus[ i ].valvePositionOutOfRangeCounter > 0 ) + + if ( valvesStatus[ valve ].valvePositionOutOfRangeCounter > OUT_OF_RANGE_COUNTER ) { - valvesStatus[ i ].valvePositionOutOfRangeCounter = 0; + //TODO fault } + else + { + valvesStatus[ valve ].valvePositionOutOfRangeCounter = 0; + } } } } @@ -957,6 +983,7 @@ // Check the counter if ( ++valvesStatus[ valve ].valveDataPublishCounter > getPublishValvesDataInterval() ) { + HD_VALVE_DATA_T valveData; valveData.valveID = (U32)valve; valveData.state = (U32)valvesStatus[ valve ].execState; valveData.currentPosID = (U32)valvesStatus[ valve ].currentPosition; @@ -1010,6 +1037,7 @@ return result; } + /*********************************************************************//** * @brief * The testResetValvesDataPublishInterval function resets the override \n @@ -1033,6 +1061,7 @@ return result; } + /*********************************************************************//** * @brief * The testSetValvesPositionOverride function overrides the valves \n @@ -1089,4 +1118,5 @@ return result; } + /**@}*/