Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r9d11c51da60da3f8d2917433938a583f8a105318 -r1c6d80a873aa1bd11e28a8955ff38866fc5717f4 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 9d11c51da60da3f8d2917433938a583f8a105318) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 1c6d80a873aa1bd11e28a8955ff38866fc5717f4) @@ -43,7 +43,9 @@ #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. -#define POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT 32 ///< Position D partial close from zero position in counts. +#define VALVE_OFFEST_FROM_EDG_CNT 32 ///< Valves offset from the edge. +#define POS_C_FROM_ZERO_CNT VALVE_OFFEST_FROM_EDG_CNT ///< Position C from zero position in counts. +#define POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT ( POS_C_FROM_ZERO_CNT + 32 ) ///< Position D partial close from zero position in counts. /// Valve controller states typedef enum Valve_Control_States @@ -59,16 +61,6 @@ 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 { @@ -89,7 +81,6 @@ BOOL hasHomingFailed; ///< 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. } VALVE_STATUS_T; /// Payload record structure for pinch valve set position request @@ -411,14 +402,16 @@ if ( valveEncPosistion >= ZERO_ENC_DEBOUNCE_THRESHOLD_CNT ) { - // Keep reseting the debouce timer until the encoder count is less than the encoder count is less than the number + // Keep reseting the debounce timer until the encoder count is less than the encoder count is less than the number currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); } else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, ZERO_ENC_DEBOUNCE_TIMEOUT_MS ) ) { + // Getting ready for homing enable the homing bit upon transitioning to homing not started + setValveHomingEnableDisable( valve, TRUE ); // Position C is hard coded as count 8. // Position D is hard coded as an offset form 0. - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = ROTARY_VALVE_MICROSTEP_FRACTION; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = POS_C_FROM_ZERO_CNT; currentValveStates[ valve ].positionsABC[ VALVE_POSITION_D_PARTIAL_CLOSE ] = POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT; nextState = VALVE_STATE_HOMING_NOT_STARTED; } @@ -445,25 +438,7 @@ { currentValveStates[ valve ].proposedEnergizedPos = ROTARY_VALVE_INIT_FULL_SWING_COUNTS; - //switch ( currentValveStates[ valve ].valveHomingStatus ) - //{ - // case VALVE_HOMING_INIT: - // currentValveStates[ valve ].proposedEnergizedPos = 1880; //ROTARY_VALVE_INIT_FULL_SWING_COUNTS; - // break; - // - // 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; - //} - + // TODO remove this? What is the criteria for the homing failure? if ( ( currentValveStates[ valve ].proposedEnergizedPos <= ROTARY_VALVE_ENERGIZED_EDGE_MIN_COUNTS ) || ( currentValveStates[ valve ].proposedEnergizedPos >= ROTARY_VALVE_ENERGIZED_EDGE_MAX_COUNTS ) ) { @@ -497,69 +472,51 @@ static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; - S16 curPos = currentValveStates[ valve ].currentEncPosition; + S16 currPos = currentValveStates[ valve ].currentEncPosition; + BOOL hasEdgeBeenReached = FALSE; checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_FIND_ENERGIZED_EDGE ); // have we found forward edge of travel? - if ( abs( curPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) + if ( abs( currPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) { if ( 0 == currentValveStates[ valve ].valveOpsStartTime ) { currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); } else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) { - //if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) - //{ - // 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 - { - S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - S16 posA = posC + ( ( curPos - posC ) / 2 ); - - // If position A was calculated to not be a division of 8 it is rounded down to be a division of 8 - posA = ( posA % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( posA >> 3 ) << 3 : posA; - - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos; - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posA; - 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; - } + hasEdgeBeenReached = TRUE; } } - else if ( curPos <= 0 ) + else if ( currPos <= 0 ) { - switch ( currentValveStates[ valve ].valveHomingStatus ) - { - case VALVE_HOMING_INIT: - case VALVE_HOMING_FORWARD: - currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_REVERSE; - break; + // Lost the shaft. Get the maximum value in the special register as the maximum. + S16 maxEncPosition = getValveMaximumEncoderPosition( valve ); - default: - // If init or forward modes, then reverse again. - break; - } + hasEdgeBeenReached = TRUE; + currPos = maxEncPosition - VALVE_OFFEST_FROM_EDG_CNT; + } - currentValveStates[ valve ].hasValveBeenReset = FALSE; - nextState = VALVE_STATE_RESET_VALVE; + if ( TRUE == hasEdgeBeenReached ) + { + // Done with homing disable the homing bit + setValveHomingEnableDisable( valve, FALSE ); + + S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + S16 posA = posC + ( ( currPos - posC ) / 2 ); + + // If position A was calculated to not be a division of 8 it is rounded down to be a division of 8 + posA = ( posA % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( posA >> 3 ) << 3 : posA; + + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = currPos; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posA; + currentValveStates[ valve ].hasValveBeenHomed = TRUE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_B_OPEN; + currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; + currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; + currentValveStates[ valve ].hasHomingBeenRequested = FALSE; + nextState = VALVE_STATE_IDLE; } return nextState; @@ -583,26 +540,8 @@ // handle a home request if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - if ( VALVE_HOMING_COMPLETE == currentValveStates[ valve ].valveHomingStatus ) - { - S16 tgtPos = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - S16 mag = currentValveStates[ valve ].currentEncPosition - 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_INIT; - currentValveStates[ valve ].hasValveBeenReset = FALSE; - nextState = VALVE_STATE_IN_TRANSITION; - // Command FPGA to move valve to commanded position - setValveCmdChangePosition( valve, (U16)(mag), MOTOR_DIR_REVERSE ); - } - else if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) - { - currentValveStates[ valve ].hasValveBeenReset = FALSE; - nextState = VALVE_STATE_HOMING_NOT_STARTED; - } + // Transition to homing not started. Reseting and enabling the valve and encoder is not needed + nextState = VALVE_STATE_HOMING_NOT_STARTED; } // handle a position change request else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) @@ -650,7 +589,7 @@ if ( abs( delta ) < VALVE_TRANSITION_MIN_TGT_DELTA ) { currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; - nextState = ( VALVE_HOMING_COMPLETE != currentValveStates[ valve ].valveHomingStatus ? VALVE_STATE_HOMING_NOT_STARTED : VALVE_STATE_IDLE ); + nextState = VALVE_STATE_IDLE; } // is transition taking too long? else if ( ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) &&