Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r0e809b89fe509c2968c3445898466dd8dd9411a9 -r88faf0b2c2b71c6d6b75637a11f7d5de41e351af --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 0e809b89fe509c2968c3445898466dd8dd9411a9) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 88faf0b2c2b71c6d6b75637a11f7d5de41e351af) @@ -56,7 +56,7 @@ { VALVE_POSITION_T commandedPosition; ///< Valve commanded position enum. VALVE_POSITION_T currentPosition; ///< Valve current position enum. - S16 priorEncPosition; ///< Valve prior 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. @@ -165,7 +165,8 @@ { 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; @@ -306,7 +307,8 @@ break; default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_STATE, (U32)currentValveStates[ valve ].controlState ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_STATE, + (U32)currentValveStates[ valve ].controlState ) currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; currentValveStates[ valve ].hasValveBeenHomed = FALSE; currentValveStates[ valve ].controlState = VALVE_STATE_HOMING_NOT_HOMED; @@ -341,7 +343,7 @@ // // return valvesSelfTestResult; //} -// + /*********************************************************************//** * @brief * The handleValvesWait4PostState function handles the Wait for POST state @@ -375,45 +377,6 @@ * @param valve ID of valve for which to handle the Not Homed state * @return next state of the state machine for the given valve *************************************************************************/ -//static VALVE_STATE_T handleValvesNotHomedState( VALVE_T valve ) -//{ -// VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_HOMED; -// -// if ( ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) && -// ( FALSE == currentValveStates[ valve ].homingOperationActive ) ) -// { -// 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(); -// } -// } -// else if ( ( TRUE == currentValveStates[ valve ].homingOperationActive ) && ( TRUE == isPinchValveHomed( valve ) ) ) -// { -// currentValveStates[ valve ].homingOperationActive = FALSE; -// currentValveStates[ valve ].hasValveBeenHomed = TRUE; -// currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE; -// currentValveStates[ valve ].currentPosition = getCurrentValveABCPosition( valve ); -// nextState = VALVE_STATE_IDLE; -// } -// else if ( ( TRUE == currentValveStates[ valve ].homingOperationActive ) && -// ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, VALVE_HOMING_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; -//} - - static VALVE_STATE_T handleValvesNotHomedState( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_HOMED; @@ -437,26 +400,26 @@ if ( TRUE == movePinchValveToStoredPosition( valve, VALVE_POSITION_C_CLOSE ) ) { currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); - nextState = VALVE_STATE_IN_TRANSITION; + nextState = VALVE_STATE_IN_TRANSITION; } else { currentValveStates[ valve ].hasValveBeenHomed = FALSE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - nextState = VALVE_STATE_HOMING_NOT_HOMED; + 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_HOMING_TIMEOUT_MS ) ) && - ( TRUE != getCPLDACPowerLossDetected() ) ) + ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, VALVE_HOMING_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 ) ) - nextState = VALVE_STATE_HOMING_NOT_HOMED; + nextState = VALVE_STATE_HOMING_NOT_HOMED; } return nextState; @@ -528,8 +491,8 @@ } else { - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - // TODO - alarm + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + // TODO - alarm later nextState = VALVE_STATE_IDLE; } } Index: firmware/App/Drivers/PinchValve.c =================================================================== diff -u -r0e809b89fe509c2968c3445898466dd8dd9411a9 -r88faf0b2c2b71c6d6b75637a11f7d5de41e351af --- firmware/App/Drivers/PinchValve.c (.../PinchValve.c) (revision 0e809b89fe509c2968c3445898466dd8dd9411a9) +++ firmware/App/Drivers/PinchValve.c (.../PinchValve.c) (revision 88faf0b2c2b71c6d6b75637a11f7d5de41e351af) @@ -248,47 +248,52 @@ // ********** private function prototypes ********** +// Low level command state machine handlers static PINCH_VALVE_CMD_STATE_T handlePinchValveCmdIdleState( VALVE_T valve ); static PINCH_VALVE_CMD_STATE_T handlePinchValveCmdSetCmdState( VALVE_T valve ); static PINCH_VALVE_CMD_STATE_T handlePinchValveCmdXmitCmdState( VALVE_T valve ); static PINCH_VALVE_CMD_STATE_T handlePinchValveCmdWaitDoneState( VALVE_T valve ); static PINCH_VALVE_CMD_STATE_T handlePinchValveCmdReadWordsState( VALVE_T valve ); static PINCH_VALVE_CMD_STATE_T handlePinchValveCmdErrorState( VALVE_T valve ); +// High level function state machine handlers static PINCH_VALVE_FUNCTION_STATE_T handlePinchValveFunctionIdleState( VALVE_T valve ); static PINCH_VALVE_FUNCTION_STATE_T handlePinchValveFunctionSendState( VALVE_T valve ); static PINCH_VALVE_FUNCTION_STATE_T handlePinchValveFunctionWaitState( VALVE_T valve ); +// Command processing functions static BOOL setPinchValveCommand( VALVE_T valve, PINCH_VALVE_COMMAND_T command ); - static BOOL writePinchValveCommandRegisters( VALVE_T valve ); static BOOL transmitPinchValveCommand( VALVE_T valve ); static void readPinchValveCommandResponse( VALVE_T valve ); static PINCH_VALVE_CMD_STATE_T processPinchValveCommandOutput( VALVE_T valve ); static void clearPinchValveFpgaCommand( VALVE_T valve ); +// FPGA command status and validation functions static BOOL setPinchValveFpgaCommandByte( const PINCH_VALVE_COMMAND_T *command, U08 *fpgaCommand ); static BOOL isPinchValveCommandDone( VALVE_T valve ); static BOOL isPinchValveControllerFaultActive( VALVE_T valve ); static BOOL didPinchValveErrorCountChange( VALVE_T valve ); static BOOL arePinchValveWordCountsValid( VALVE_T valve ); - static U08 readPinchValveErrorCount( VALVE_T valve ); +static BOOL isPinchValveCommandResponseReady( VALVE_T valve ); + +// Parameter validation and data conversion functions static BOOL isValidPinchValve( VALVE_T valve ); static BOOL isValidPinchValveCommand( const PINCH_VALVE_COMMAND_T *command ); +static BOOL isValidPinchValvePosition( VALVE_POSITION_T position ); static void setSigned32CommandWords( PINCH_VALVE_COMMAND_T *command, S32 value ); static S32 getSigned32OutputValue( VALVE_T valve ); +// Command and function state reset functions static void clearActivePinchValveCommand( VALVE_T valve ); static void clearPinchValveCommandRegisters( VALVE_T valve ); static void resetPinchValveFunctionSequence( VALVE_T valve ); +// Motion status evaluation functions static BOOL isPinchValveMotionComplete( U16 status ); static BOOL isPinchValveMotionError( U16 status ); -static BOOL isValidPinchValvePosition( VALVE_POSITION_T position ); -static BOOL isPinchValveCommandResponseReady( VALVE_T valve ); - /*********************************************************************//** * @brief * The initPinchValveDriver function initializes the pinch valve @@ -411,6 +416,42 @@ /*********************************************************************//** * @brief +* The execPinchValveFunction function executes the pinch valve +* homing and position command sequence state machine. +* @details \b Inputs: functionState[] +* @details \b Outputs: functionState[] +* @return current state (sub-mode) +*************************************************************************/ +void execPinchValveFunction( void ) +{ + VALVE_T valve; + + for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) + { + switch ( functionState[ valve ] ) + { + case PINCH_VALVE_FUNCTION_IDLE_STATE: + functionState[ valve ] = handlePinchValveFunctionIdleState( valve ); + break; + + case PINCH_VALVE_FUNCTION_SEND_STATE: + functionState[ valve ] = handlePinchValveFunctionSendState( valve ); + break; + + case PINCH_VALVE_FUNCTION_WAIT_STATE: + functionState[ valve ] = handlePinchValveFunctionWaitState( valve ); + break; + + default: + commandFailed[ valve ] = TRUE; + resetPinchValveFunctionSequence( valve ); + break; + } + } +} + +/*********************************************************************//** +* @brief * The handlePinchValveCmdIdleState function handles the command * IDLE state. * @details \b Inputs: valve @@ -584,42 +625,6 @@ /*********************************************************************//** * @brief -* The execPinchValveFunction function executes the pinch valve -* homing and position command sequence state machine. -* @details \b Inputs: functionState[] -* @details \b Outputs: functionState[] -* @return current state (sub-mode) -*************************************************************************/ -void execPinchValveFunction( void ) -{ - VALVE_T valve; - - for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) - { - switch ( functionState[ valve ] ) - { - case PINCH_VALVE_FUNCTION_IDLE_STATE: - functionState[ valve ] = handlePinchValveFunctionIdleState( valve ); - break; - - case PINCH_VALVE_FUNCTION_SEND_STATE: - functionState[ valve ] = handlePinchValveFunctionSendState( valve ); - break; - - case PINCH_VALVE_FUNCTION_WAIT_STATE: - functionState[ valve ] = handlePinchValveFunctionWaitState( valve ); - break; - - default: - commandFailed[ valve ] = TRUE; - resetPinchValveFunctionSequence( valve ); - break; - } - } -} - -/*********************************************************************//** -* @brief * The handlePinchValveFunctionIdleState function starts a pending homing or * stored position command sequence. * @details \b Inputs: valve, pendingValveHomeRequest[], @@ -933,6 +938,50 @@ /*********************************************************************//** * @brief +* The getPinchValveStoredPosition function returns the stored encoder count +* for the selected pinch valve position. +* @details \b Inputs: valve, position, valvePositions[] +* @details \b Outputs: none +* @param valve H1_VALV or H19_VALV. +* @param position Stored valve position to read. +* @return Stored signed encoder position, or 0 for invalid input. +*************************************************************************/ +S32 getPinchValveStoredPosition( VALVE_T valve, VALVE_POSITION_T position ) +{ + S32 result = 0; + + if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValvePosition( position ) ) ) + { + result = valvePositions[ valve ][ position ]; + } + + return result; +} + +/*********************************************************************//** +* @brief +* The movePinchValveToStoredPosition function requests movement to a stored +* pinch valve position. +* @details \b Inputs: valve, position +* @details \b Outputs: none +* @param valve H1_VALV or H19_VALV. +* @param position Stored valve position to command. +* @return TRUE if the movement request is accepted, FALSE otherwise +*************************************************************************/ +BOOL movePinchValveToStoredPosition( VALVE_T valve, VALVE_POSITION_T position ) +{ + BOOL result = FALSE; + + if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValvePosition( position ) ) ) + { + result = setPinchValvePosition( valve, position ); + } + + return result; +} + +/*********************************************************************//** +* @brief * The setPinchValveCommand function loads the selected command into the * command state machine. * @details \b Inputs: valve, command, commandState[] @@ -1067,6 +1116,127 @@ /*********************************************************************//** * @brief +* The processPinchValveCommandOutput function processes the active command +* response and determines the next low level command state. +* @details \b Inputs: valve, activeCommand[], activeCommandIndex[], +* commandOutputWord1[], motionStartTime[], homingInProgress[], valvePositions[] +* @details \b Outputs: motionStartTime[], activityStatus[], eventStatus[], +* currentValvePosition[], valvePositions[] +* @param valve H1_VALV or H19_VALV. +* @return next pinch valve command state +*************************************************************************/ +static PINCH_VALVE_CMD_STATE_T processPinchValveCommandOutput( VALVE_T valve ) +{ + PINCH_VALVE_CMD_STATE_T result = PINCH_VALVE_CMD_IDLE_STATE; + S32 positionB; + S32 positionC; + + if ( TRUE == isValidPinchValve( valve ) ) + { + switch ( activeCommand[ valve ].cmdHeader ) + { + case PINCH_VALVE_PMD_CMD_UPDATE: + motionStartTime[ valve ] = getMSTimerCount(); + break; + + case PINCH_VALVE_PMD_CMD_GET_ACTIVITY_STATUS: + activityStatus[ valve ] = commandOutputWord1[ valve ]; + break; + + case PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS: + eventStatus[ valve ] = commandOutputWord1[ valve ]; + // TODO - if not motion complete or motion error, stay in this state, otherwise idle + // During homing, Motion Complete bit 0 and Motion Error bit 4, both be set when the valve reaches the mechanical stop. + // Therefore, check Motion Complete first. + + if ( TRUE == isPinchValveMotionComplete( eventStatus[ valve ] ) ) + { + result = PINCH_VALVE_CMD_IDLE_STATE; + } + else if ( TRUE == isPinchValveMotionError( eventStatus[ valve ] ) ) + { + result = PINCH_VALVE_CMD_ERROR_STATE; + } + else if ( TRUE == didTimeout( motionStartTime[ valve ], PINCH_VALVE_MOTION_TIMEOUT_MS ) ) + { + result = PINCH_VALVE_CMD_ERROR_STATE; + } + else + { + // This is to check whether the motion is still running and repeating the same to Get Event Status + result = PINCH_VALVE_CMD_SET_CMD_STATE; + } + break; + + default: + break; + } + + if ( PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION == activeCommand[ valve ].cmdHeader ) + { + if ( TRUE == homingInProgress[ valve ] ) + { + switch ( activeCommandIndex[ valve ] ) + { + case PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_B: + currentValvePosition[ valve ] = getSigned32OutputValue( valve ); + if ( H1_VALV == valve ) + { + valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = currentValvePosition[ valve ] + H1_VALVE_POSITION_B_OFFSET_COUNTS; + } + else + { + valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = currentValvePosition[ valve ] - H19_VALVE_POSITION_B_OFFSET_COUNTS; + } + break; + + case PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_C: + currentValvePosition[ valve ] = getSigned32OutputValue( valve ); + if ( H1_VALV == valve ) + { + valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = currentValvePosition[ valve ] - H1_VALVE_POSITION_C_OFFSET_COUNTS; + } + else + { + valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = currentValvePosition[ valve ] + H19_VALVE_POSITION_C_OFFSET_COUNTS; + } + positionB = valvePositions[ valve ][ VALVE_POSITION_B_OPEN ]; + positionC = valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ]; + valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] = positionB + ( ( positionC - positionB ) / 2 ); +// if ( H1_VALV == valve ) +// { +// setSigned32CommandWords( &h1HomingCommands[ PINCH_VALVE_HOME_CMD_SET_POSITION ], valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] ); +// } +// else if (H19_VALV == valve ) +// { +// setSigned32CommandWords( &h19HomingCommands[ PINCH_VALVE_HOME_CMD_SET_POSITION ], valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] ); +// } + break; + + default: + break; + } + } + else + { + switch ( activeCommandIndex[ valve ] ) + { + case PINCH_VALVE_POSITION_CMD_GET_ACTUAL_POSITION: + currentValvePosition[ valve ] = getSigned32OutputValue( valve ); + break; + + default: + break; + } + } + } + } + + return result; +} + +/*********************************************************************//** +* @brief * The clearPinchValveFpgaCommand function clears the active valve FPGA command * register. * @details \b Inputs: valve @@ -1216,6 +1386,7 @@ return result; } + /*********************************************************************//** * @brief * The readPinchValveErrorCount function reads the current error count for @@ -1243,6 +1414,48 @@ /*********************************************************************//** * @brief +* The isPinchValveCommandResponseReady function checks whether the FPGA +* command response is complete and contains the expected output words. +* @details \b Inputs: commandSpiStatus[], activeCommand[] +* @details \b Outputs: none +* @param valve H1_VALV or H19_VALV. +* @return TRUE if the command response is ready, FALSE otherwise. +*************************************************************************/ +static BOOL isPinchValveCommandResponseReady( VALVE_T valve ) +{ + BOOL result = FALSE; + U08 receivedCount; + + if ( TRUE == isValidPinchValve( valve ) ) + { + if ( TRUE == isPinchValveCommandDone( valve ) ) + { + // This is for the commands expecting no returned words, the done bit is sufficient. + if ( 0 == activeCommand[ valve ].outputWordCount ) + { + result = TRUE; + } + else + { + receivedCount = (U08)( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_RX_COUNT_MASK ) >> PINCH_VALVE_SPI_RX_COUNT_SHIFT ); + + if ( receivedCount == activeCommand[ valve ].outputWordCount ) + { + result = TRUE; + } + else + { + result = TRUE; // Test code remove else later + } + } + } + } + + return result; +} + +/*********************************************************************//** +* @brief * The isValidPinchValve function validates the selected valve ID. * @details \b Inputs: valve * @details \b Outputs: none @@ -1289,6 +1502,24 @@ /*********************************************************************//** * @brief +* The isValidPinchValvePosition function validates the selected stored valve +* position. +* @details \b Inputs: position +* @details \b Outputs: none +* @param position Valve position ID to validate. +* @return TRUE for Position A, B, or C, FALSE otherwise. +*************************************************************************/ +static BOOL isValidPinchValvePosition( VALVE_POSITION_T position ) +{ + BOOL result; + + result = ( ( position > VALVE_POSITION_NOT_IN_POSITION ) && ( position < NUM_OF_VALVE_POSITIONS ) ); + + return result; +} + +/*********************************************************************//** +* @brief * The setSigned32CommandWords function stores a signed 32 bit value in two * 16 bit command input words. * @details \b Inputs: command, value @@ -1328,128 +1559,6 @@ /*********************************************************************//** * @brief -* The processPinchValveCommandOutput function processes the active command -* response and determines the next low level command state. -* @details \b Inputs: valve, activeCommand[], activeCommandIndex[], -* commandOutputWord1[], commandOutputWord2[], commandOutputWord3[], -* motionStartTime[], homingInProgress[], valvePositions[] -* @details \b Outputs: motionStartTime[], activityStatus[], eventStatus[], -* currentValvePosition[], valvePositions[] -* @param valve H1_VALV or H19_VALV. -* @return next pinch valve command state -*************************************************************************/ -static PINCH_VALVE_CMD_STATE_T processPinchValveCommandOutput( VALVE_T valve ) -{ - PINCH_VALVE_CMD_STATE_T result = PINCH_VALVE_CMD_IDLE_STATE; - S32 positionB; - S32 positionC; - - if ( TRUE == isValidPinchValve( valve ) ) - { - switch ( activeCommand[ valve ].cmdHeader ) - { - case PINCH_VALVE_PMD_CMD_UPDATE: - motionStartTime[ valve ] = getMSTimerCount(); - break; - - case PINCH_VALVE_PMD_CMD_GET_ACTIVITY_STATUS: - activityStatus[ valve ] = commandOutputWord1[ valve ]; - break; - - case PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS: - eventStatus[ valve ] = commandOutputWord1[ valve ]; - // TODO - if not motion complete or motion error, stay in this state, otherwise idle - // During homing, Motion Complete bit 0 and Motion Error bit 4, both be set when the valve reaches the mechanical stop. - // Therefore, check Motion Complete first. - - if ( TRUE == isPinchValveMotionComplete( eventStatus[ valve ] ) ) - { - result = PINCH_VALVE_CMD_IDLE_STATE; - } - else if ( TRUE == isPinchValveMotionError( eventStatus[ valve ] ) ) - { - result = PINCH_VALVE_CMD_ERROR_STATE; - } - else if ( TRUE == didTimeout( motionStartTime[ valve ], PINCH_VALVE_MOTION_TIMEOUT_MS ) ) - { - result = PINCH_VALVE_CMD_ERROR_STATE; - } - else - { - // This is to check whether the motion is still running and repeating the same to Get Event Status - result = PINCH_VALVE_CMD_SET_CMD_STATE; - } - break; - - default: - break; - } - - if ( PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION == activeCommand[ valve ].cmdHeader ) - { - if ( TRUE == homingInProgress[ valve ] ) - { - switch ( activeCommandIndex[ valve ] ) - { - case PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_B: - currentValvePosition[ valve ] = getSigned32OutputValue( valve ); - if ( H1_VALV == valve ) - { - valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = currentValvePosition[ valve ] + H1_VALVE_POSITION_B_OFFSET_COUNTS; - } - else - { - valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = currentValvePosition[ valve ] - H19_VALVE_POSITION_B_OFFSET_COUNTS; - } - break; - - case PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_C: - currentValvePosition[ valve ] = getSigned32OutputValue( valve ); - if ( H1_VALV == valve ) - { - valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = currentValvePosition[ valve ] - H1_VALVE_POSITION_C_OFFSET_COUNTS; - } - else - { - valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = currentValvePosition[ valve ] + H19_VALVE_POSITION_C_OFFSET_COUNTS; - } - positionB = valvePositions[ valve ][ VALVE_POSITION_B_OPEN ]; - positionC = valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ]; - valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] = positionB + ( ( positionC - positionB ) / 2 ); -// if ( H1_VALV == valve ) -// { -// setSigned32CommandWords( &h1HomingCommands[ PINCH_VALVE_HOME_CMD_SET_POSITION ], valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] ); -// } -// else if (H19_VALV == valve ) -// { -// setSigned32CommandWords( &h19HomingCommands[ PINCH_VALVE_HOME_CMD_SET_POSITION ], valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] ); -// } - break; - - default: - break; - } - } - else - { - switch ( activeCommandIndex[ valve ] ) - { - case PINCH_VALVE_POSITION_CMD_GET_ACTUAL_POSITION: - currentValvePosition[ valve ] = getSigned32OutputValue( valve ); - break; - - default: - break; - } - } - } - } - - return result; -} - -/*********************************************************************//** -* @brief * The clearActivePinchValveCommand function clears the active command record * for the selected pinch valve. * @details \b Inputs: valve @@ -1564,108 +1673,4 @@ return result; } -/*********************************************************************//** -* @brief -* The isValidPinchValvePosition function validates the selected stored valve -* position. -* @details \b Inputs: position -* @details \b Outputs: none -* @param position Valve position ID to validate. -* @return TRUE for Position A, B, or C, FALSE otherwise. -*************************************************************************/ -static BOOL isValidPinchValvePosition( VALVE_POSITION_T position ) -{ - BOOL result; - - result = ( ( position > VALVE_POSITION_NOT_IN_POSITION ) && ( position < NUM_OF_VALVE_POSITIONS ) ); - - return result; -} - -/*********************************************************************//** -* @brief -* The getPinchValveStoredPosition function returns the stored encoder count -* for the selected pinch valve position. -* @details \b Inputs: valve, position, valvePositions[] -* @details \b Outputs: none -* @param valve H1_VALV or H19_VALV. -* @param position Stored valve position to read. -* @return Stored signed encoder position, or 0 for invalid input. -*************************************************************************/ -S32 getPinchValveStoredPosition( VALVE_T valve, VALVE_POSITION_T position ) -{ - S32 result = 0; - - if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValvePosition( position ) ) ) - { - result = valvePositions[ valve ][ position ]; - } - - return result; -} - -/*********************************************************************//** -* @brief -* The movePinchValveToStoredPosition function requests movement to a stored -* pinch valve position. -* @details \b Inputs: valve, position -* @details \b Outputs: none -* @param valve H1_VALV or H19_VALV. -* @param position Stored valve position to command. -* @return TRUE if the movement request is accepted, FALSE otherwise -*************************************************************************/ -BOOL movePinchValveToStoredPosition( VALVE_T valve, VALVE_POSITION_T position ) -{ - BOOL result = FALSE; - - if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValvePosition( position ) ) ) - { - result = setPinchValvePosition( valve, position ); - } - - return result; -} - -/*********************************************************************//** -* @brief -* The isPinchValveCommandResponseReady function checks whether the FPGA -* command response is complete and contains the expected output words. -* @details \b Inputs: commandSpiStatus[], activeCommand[] -* @details \b Outputs: none -* @param valve H1_VALV or H19_VALV. -* @return TRUE if the command response is ready, FALSE otherwise. -*************************************************************************/ -static BOOL isPinchValveCommandResponseReady( VALVE_T valve ) -{ - BOOL result = FALSE; - U08 receivedCount; - - if ( TRUE == isValidPinchValve( valve ) ) - { - if ( TRUE == isPinchValveCommandDone( valve ) ) - { - // This is for the commands expecting no returned words, the done bit is sufficient. - if ( 0 == activeCommand[ valve ].outputWordCount ) - { - result = TRUE; - } - else - { - receivedCount = (U08)( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_RX_COUNT_MASK ) >> PINCH_VALVE_SPI_RX_COUNT_SHIFT ); - - if ( receivedCount == activeCommand[ valve ].outputWordCount ) - { - result = TRUE; - } - else - { - result = TRUE; // Test code remove else later - } - } - } - } - - return result; -} - /**@}*/ Index: firmware/App/Drivers/PinchValve.h =================================================================== diff -u -r7bb46513e0563ac58f769f654ac67ae694f369ad -r88faf0b2c2b71c6d6b75637a11f7d5de41e351af --- firmware/App/Drivers/PinchValve.h (.../PinchValve.h) (revision 7bb46513e0563ac58f769f654ac67ae694f369ad) +++ firmware/App/Drivers/PinchValve.h (.../PinchValve.h) (revision 88faf0b2c2b71c6d6b75637a11f7d5de41e351af) @@ -55,8 +55,8 @@ // ********** public function prototypes ********** void initPinchValveDriver( void ); // Initializes the pinch valve driver state, command data, and FPGA. -void execPinchValveFunction( void ); // Executes the pinch valve function state machine void execPinchValveCommand( void ); // Executes the FPGA command state machine. +void execPinchValveFunction( void ); // Executes the pinch valve function state machine BOOL homePinchValve( VALVE_T valve ); // Starts the homing sequence for the selected pinch valve. @@ -68,7 +68,7 @@ BOOL isPinchValveBusy( VALVE_T valve ); // Returns TRUE when the selected valve is executing or waiting for a command. BOOL isPinchValveHomed( VALVE_T valve ); // Returns TRUE when the selected valve is activated -S32 getPinchValveStoredPosition( VALVE_T valve, VALVE_POSITION_T position ); // Returns the stored encoder position for the selected valve position. +S32 getPinchValveStoredPosition( VALVE_T valve, VALVE_POSITION_T position ); // Returns the stored encoder position for the selected valve position. BOOL movePinchValveToStoredPosition( VALVE_T valve, VALVE_POSITION_T position ); // Starts movement to a previously stored valve position /**@}*/