Index: firmware/App/Drivers/PinchValve.c =================================================================== diff -u -r365afa6b051eb4066db3adda1eeac77149aae7ba -r4c738395b1627aa36f30c14c2e374acf0e88f5f8 --- firmware/App/Drivers/PinchValve.c (.../PinchValve.c) (revision 365afa6b051eb4066db3adda1eeac77149aae7ba) +++ firmware/App/Drivers/PinchValve.c (.../PinchValve.c) (revision 4c738395b1627aa36f30c14c2e374acf0e88f5f8) @@ -29,63 +29,60 @@ // ********** private definitions ********** -#define PINCH_VALVE_COMMAND_TIMEOUT_MS 1000 ///< Maximum FPGA command execution time. +#define PINCH_VALVE_COMMAND_TIMEOUT_MS 1000 ///< Maximum FPGA command execution time in milliseconds. +#define PINCH_VALVE_MOTION_TIMEOUT_MS 10000 ///< Maximum allowed valve motion time in milliseconds -#define PINCH_VALVE_HOME_OFFSET 20 ///< Offset (in encoder counts) from end of travel edge to set position B and C to. +#define PINCH_VALVE_HOME_OFFSET 20 ///< Offset (in encoder counts) from end of travel edge to set position B and C. -#define PINCH_VALVE_MOTION_TIMEOUT_MS 10000 +#define PINCH_VALVE_FPGA_CMD_START_BIT_MASK 0x01 ///< FPGA command register bit 0 used to start a command transaction +#define PINCH_VALVE_FPGA_CMD_WRITE_ONLY_BIT_MASK 0x02 ///< FPGA command register bit 1 indicating a write only command. +#define PINCH_VALVE_FPGA_CMD_TX_COUNT_SHIFT 2 ///< Bit shift for the FPGA command transmit word count field +#define PINCH_VALVE_FPGA_CMD_RX_COUNT_SHIFT 5 ///< Bit shift for the FPGA command receive word count field. +#define PINCH_VALVE_FPGA_CMD_WORD_COUNT_MASK 0x07 ///< Three bit mask used for FPGA transmit and receive word counts -#define PINCH_VALVE_FPGA_CMD_START_BIT_MASK 0x01 ///< Bit 0 starts a command transaction. (TODO: To change the comments later) -#define PINCH_VALVE_FPGA_CMD_WRITE_ONLY_BIT_MASK 0x02 ///< Bit 1 selects a write-only command. -#define PINCH_VALVE_FPGA_CMD_TX_COUNT_SHIFT 2 ///< Bits 4-2 contain transmit word count. -#define PINCH_VALVE_FPGA_CMD_RX_COUNT_SHIFT 5 ///< Bits 7-5 contain receive word count. -#define PINCH_VALVE_FPGA_CMD_WORD_COUNT_MASK 0x07 ///< Three-bit FPGA word-count mask. +#define PINCH_VALVE_SPI_DONE_BIT_MASK 0x01 ///< SPI status bit 0 indicating command completion +#define PINCH_VALVE_SPI_TX_COUNT_MASK 0x0E ///< SPI status mask for the transmitted word count +#define PINCH_VALVE_SPI_TX_COUNT_SHIFT 1 ///< Bit shift for the SPI transmitted word count +#define PINCH_VALVE_SPI_RX_COUNT_MASK 0x30 ///< SPI status mask for the received-word count. +#define PINCH_VALVE_SPI_RX_COUNT_SHIFT 4 ///< Bit shift for the SPI received word count +#define PINCH_VALVE_SPI_CONTROLLER_FAULT_BIT_MASK 0x40 ///< SPI status bit 6 indicating a controller fault. -#define PINCH_VALVE_SPI_DONE_BIT_MASK 0x01 ///< SPI command completed. -#define PINCH_VALVE_SPI_TX_COUNT_MASK 0x0E ///< SPI status bits 3-1: transmitted words. -#define PINCH_VALVE_SPI_TX_COUNT_SHIFT 1 -#define PINCH_VALVE_SPI_RX_COUNT_MASK 0x30 ///< SPI status bits 5-4: received words. -#define PINCH_VALVE_SPI_RX_COUNT_SHIFT 4 -#define PINCH_VALVE_SPI_CONTROLLER_FAULT_BIT_MASK 0x40 ///< SPI status bit 6: controller fault. +#define PINCH_VALVE_ENABLE_BIT_MASK 0x00 ///< Value used to enable normal pinch valve controller +#define PINCH_VALVE_RESET_BIT_MASK 0x01 ///< Bit mask used to reset the pinch valve controller. -#define PINCH_VALVE_ENABLE_BIT_MASK 0x00 ///< Enable controller. -#define PINCH_VALVE_RESET_BIT_MASK 0x01 ///< Reset controller. +#define PINCH_VALVE_MAX_INPUT_WORDS 3 ///< Maximum number of command input words, excluding the command header. +#define PINCH_VALVE_MAX_OUTPUT_WORDS 3 ///< Maximum number of command output words. +#define PINCH_VALVE_MAX_TRANSMIT_WORDS 4 ///< Maximum number of transmitted words, including the command header +#define PINCH_VALVE_PMD_CMD_SET_POSITION 0x0010 ///< Command used to set the requested target position +#define PINCH_VALVE_PMD_CMD_SET_VELOCITY 0x0011 ///< Command used to set the requested movement velocity +#define PINCH_VALVE_PMD_CMD_UPDATE 0x001A ///< Command used to apply previous written motion parameters +#define PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS 0x0031 ///< Command used to read the event status register +#define PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS 0x0034 ///< Command used to clear selected event-status bits. +#define PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION 0x0037 ///< Command used to read the current encoder position. +#define PINCH_VALVE_PMD_CMD_SET_ACTUAL_POSITION 0x004D ///< Command used to assign the current encoder position +#define PINCH_VALVE_PMD_CMD_SET_PROFILE_MODE 0x00A0 ///< Command used to select the motion profile mode +#define PINCH_VALVE_PMD_CMD_GET_ACTIVITY_STATUS 0x00A6 ///< Command used to read the controller activity status. -#define PINCH_VALVE_MAX_INPUT_WORDS 3 ///< Maximum number of command input words. -#define PINCH_VALVE_MAX_OUTPUT_WORDS 3 ///< Maximum number of command output words. -#define PINCH_VALVE_MAX_TRANSMIT_WORDS 4 +#define PINCH_VALVE_PROFILE_MODE_VELOCITY 1 ///< Velocity profile mode +#define PINCH_VALVE_PROFILE_MODE_S_CURVE 2 ///< S-curve position profile mode value +#define PINCH_VALVE_RESET_EVENT_STATUS_VALUE 0xFFEE ///< Value used to reset the required event status bits -#define PINCH_VALVE_PMD_CMD_SET_POSITION 0x0010 -#define PINCH_VALVE_PMD_CMD_SET_VELOCITY 0x0011 -#define PINCH_VALVE_PMD_CMD_UPDATE 0x001A -#define PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS 0x0031 -#define PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS 0x0034 -#define PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION 0x0037 -#define PINCH_VALVE_PMD_CMD_SET_ACTUAL_POSITION 0x004D -#define PINCH_VALVE_PMD_CMD_SET_PROFILE_MODE 0x00A0 -#define PINCH_VALVE_PMD_CMD_GET_ACTIVITY_STATUS 0x00A6 +static const S32 H1_HOMING_VELOCITY_FIRST = -3342; ///< First H1 homing movement velocity ( Reverse Direction ) +static const S32 H1_HOMING_VELOCITY_SECOND = 3342; ///< Second H1 homing movement velocity ( Forward Direction ) +static const S32 H19_HOMING_VELOCITY_FIRST = 3342; ///< First H19 homing movement velocity ( Forward Direction ) +static const S32 H19_HOMING_VELOCITY_SECOND = -3342; ///< Second H19 homing movement velocity ( Reverse Direction ) +static const S32 POSITION_VELOCITY = 35000; ///< Velocity used for normal stored position movement -#define PINCH_VALVE_PROFILE_MODE_VELOCITY 1 -#define PINCH_VALVE_PROFILE_MODE_S_CURVE 2 -#define PINCH_VALVE_RESET_EVENT_STATUS_VALUE 0xFFEE +#define PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK 0x0001 ///< Event Status bit 0: motion complete. +#define PINCH_VALVE_EVENT_MOTION_ERROR_MASK 0x0010 ///< Event Status bit 4: motion error. -static const S32 H1_HOMING_VELOCITY_FIRST = -3342; -static const S32 H1_HOMING_VELOCITY_SECOND = 3342; -static const S32 H19_HOMING_VELOCITY_FIRST = 3342; -static const S32 H19_HOMING_VELOCITY_SECOND = -3342; -static const S32 POSITION_VELOCITY = 15000; - -#define PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK 0x0001 ///< Event Status bit 0: motion complete. -#define PINCH_VALVE_EVENT_MOTION_ERROR_MASK 0x0010 ///< Event Status bit 4: motion error. - - /// Pinch valve function state machine. typedef enum pinchValveFunctionStates { - PINCH_VALVE_FUNCTION_IDLE_STATE = 0, - PINCH_VALVE_FUNCTION_SEND_STATE, - PINCH_VALVE_FUNCTION_WAIT_STATE + PINCH_VALVE_FUNCTION_IDLE_STATE = 0, ///< Waits for a home or position request + PINCH_VALVE_FUNCTION_SEND_STATE, ///< Sends the current command to the command state machine. + PINCH_VALVE_FUNCTION_WAIT_STATE ///< Waits for the current command to finish } PINCH_VALVE_FUNCTION_STATE_T; /// Low-level FPGA command state machine. @@ -128,10 +125,10 @@ PINCH_VALVE_HOME_CMD_GET_EVENT_STATUS_SECOND, PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_C, PINCH_VALVE_HOME_CMD_RESET_EVENTS_COMPLETE, - PINCH_VALVE_HOME_CMD_SET_POSITION, PINCH_VALVE_HOME_CMD_SET_PROFILE_MODE_SECOND, PINCH_VALVE_HOME_CMD_SET_VELOCITY_THIRD, PINCH_VALVE_HOME_CMD_UPDATE_THIRD, + PINCH_VALVE_HOME_CMD_GET_EVENT_STATUS_THIRD, NUM_OF_PINCH_VALVE_HOMING_COMMANDS } PINCH_VALVE_HOMING_COMMAND_INDEX_T; @@ -167,10 +164,10 @@ { PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS, 0, 0, 0, 0, 1, FALSE }, { PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION, 0, 0, 0, 0, 2, FALSE }, { PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS, PINCH_VALVE_RESET_EVENT_STATUS_VALUE, 0, 0, 1, 0, TRUE }, - { PINCH_VALVE_PMD_CMD_SET_POSITION, 0, 0, 0, 2, 0, TRUE }, { PINCH_VALVE_PMD_CMD_SET_PROFILE_MODE, PINCH_VALVE_PROFILE_MODE_S_CURVE, 0, 0, 1, 0, TRUE }, { PINCH_VALVE_PMD_CMD_SET_VELOCITY, 0, 0, 0, 2, 0, TRUE }, - { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE } + { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE }, + { PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS, 0, 0, 0, 0, 1, FALSE } }; // This array is used for both H19_VALV @@ -190,10 +187,10 @@ { PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS, 0, 0, 0, 0, 1, FALSE }, { PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION, 0, 0, 0, 0, 2, FALSE }, { PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS, PINCH_VALVE_RESET_EVENT_STATUS_VALUE, 0, 0, 1, 0, TRUE }, - { PINCH_VALVE_PMD_CMD_SET_POSITION, 0, 0, 0, 2, 0, TRUE }, { PINCH_VALVE_PMD_CMD_SET_PROFILE_MODE, PINCH_VALVE_PROFILE_MODE_S_CURVE, 0, 0, 1, 0, TRUE }, { PINCH_VALVE_PMD_CMD_SET_VELOCITY, 0, 0, 0, 2, 0, TRUE }, - { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE } + { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE }, + { PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS, 0, 0, 0, 0, 1, FALSE } }; static PINCH_VALVE_COMMAND_T positionCommands[ NUM_OF_PINCH_VALVE_POSITION_COMMANDS ] = @@ -633,7 +630,25 @@ return result; } +/*********************************************************************//** +* @brief +* The isPinchValveHomed function indicates whether a command is active. +* @details \b Inputs: functionState, commandState +* @details \b Outputs: none +* @return TRUE if the driver is busy, FALSE otherwise. +*************************************************************************/ +BOOL isPinchValveHomed( VALVE_T valve ) +{ + BOOL result = FALSE; + if ( valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] != 0 ) + { + result = TRUE; + } + + return result; +} + /*********************************************************************//** * @brief * The execPinchValveFunction function executes the IDLE, SEND, and @@ -1175,7 +1190,7 @@ if ( PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION == activeCommand[ valve ].cmdHeader ) { - if ( TRUE == homingInProgress ) + if ( TRUE == homingInProgress[ valve ] ) { switch ( activeCommandIndex[ valve ] ) { @@ -1204,14 +1219,14 @@ 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 ] ); - } +// 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: @@ -1417,7 +1432,7 @@ if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValvePosition( position ) ) ) { - result = setPinchValvePosition( valve, valvePositions[ valve ][ position ] ); + result = setPinchValvePosition( valve, position ); } return result; Index: firmware/App/Drivers/PinchValve.h =================================================================== diff -u -r365afa6b051eb4066db3adda1eeac77149aae7ba -r4c738395b1627aa36f30c14c2e374acf0e88f5f8 --- firmware/App/Drivers/PinchValve.h (.../PinchValve.h) (revision 365afa6b051eb4066db3adda1eeac77149aae7ba) +++ firmware/App/Drivers/PinchValve.h (.../PinchValve.h) (revision 4c738395b1627aa36f30c14c2e374acf0e88f5f8) @@ -36,11 +36,11 @@ /// Valves positions typedef enum valveStatesNames { - VALVE_POSITION_NOT_IN_POSITION = 0, ///< Valve position is unknown (cannot be used as a command) - VALVE_POSITION_A_INSERT_EJECT, ///< Position A, Insert/Eject - VALVE_POSITION_B_OPEN, ///< Position B, Open - VALVE_POSITION_C_CLOSE, ///< Position C, Close - NUM_OF_VALVE_POSITIONS, ///< Number of valve positions + VALVE_POSITION_NOT_IN_POSITION = 0, ///< Valve position is unknown (cannot be used as a command) + VALVE_POSITION_A_INSERT_EJECT, ///< Position A, Insert/Eject + VALVE_POSITION_B_OPEN, ///< Position B, Open + VALVE_POSITION_C_CLOSE, ///< Position C, Close + NUM_OF_VALVE_POSITIONS, ///< Number of valve positions } VALVE_POSITION_T; /// rotary pinch valve names @@ -54,23 +54,25 @@ // ********** public function prototypes ********** -void initPinchValveDriver( void ); -void execPinchValveFunction( void ); -void execPinchValveCommand( void ); +void initPinchValveDriver( void ); ///< Initializes the pinch valve driver state, command data, and FPGA interface. +void execPinchValveFunction( void ); ///< Executes the pinch valve function state machine +void execPinchValveCommand( void ); ///< Executes the FPGA command state machine. -BOOL homePinchValve( VALVE_T valve ); -BOOL setPinchValvePosition( VALVE_T valve, VALVE_POSITION_T position ); -S32 getPinchValvePosition( VALVE_T valve ); +BOOL homePinchValve( VALVE_T valve ); ///< Starts the homing sequence for the selected pinch valve. -void setPinchValveEnableReset( VALVE_T valve, BOOL enable, BOOL reset ); -U08 getPinchValveEnableReset( VALVE_T valve ); +BOOL setPinchValvePosition( VALVE_T valve, VALVE_POSITION_T position ); ///< Starts a movement request to the selected stored valve position +S32 getPinchValvePosition( VALVE_T valve ); ///< Returns the latest actual encoder position for the selected valve. -BOOL isPinchValveBusy( VALVE_T valve ); +void setPinchValveEnableReset( VALVE_T valve, BOOL enable, BOOL reset ); ///< Sets the enable and reset control for the selected pinch valve +U08 getPinchValveEnableReset( VALVE_T valve ); ///< Returns the last enable and reset value written for the selected valve. -S32 getPinchValveStoredPosition( VALVE_T valve, VALVE_POSITION_T position ); -BOOL movePinchValveToStoredPosition( VALVE_T valve, VALVE_POSITION_T position ); +BOOL isPinchValveBusy( VALVE_T valve ); ///< Returns TRUE when the selected valve is executing or waiting for a command. +BOOL isPinchValveHomed( VALVE_T valve ); +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 + /**@}*/ #endif Index: firmware/App/Modes/ModeStandby.c =================================================================== diff -u -r365afa6b051eb4066db3adda1eeac77149aae7ba -r4c738395b1627aa36f30c14c2e374acf0e88f5f8 --- firmware/App/Modes/ModeStandby.c (.../ModeStandby.c) (revision 365afa6b051eb4066db3adda1eeac77149aae7ba) +++ firmware/App/Modes/ModeStandby.c (.../ModeStandby.c) (revision 4c738395b1627aa36f30c14c2e374acf0e88f5f8) @@ -149,15 +149,38 @@ if ( TRUE == startTest ) { startTest = FALSE; + startPos = TRUE; homePinchValve( H1_VALV ); homePinchValve( H19_VALV ); } - if ( TRUE == startPos ) + if ( ( TRUE == isPinchValveHomed( H1_VALV ) ) + && ( TRUE == isPinchValveHomed( H19_VALV ) ) && startPos == TRUE ) { - startPos = FALSE; - setPinchValvePosition( H1_VALV, VALVE_POSITION_C_CLOSE ); - setPinchValvePosition( H19_VALV, VALVE_POSITION_C_CLOSE ); + static U32 p = 1; + static U32 d = 0; + + if ( ( TRUE != isPinchValveBusy( H1_VALV ) ) + && ( TRUE != isPinchValveBusy( H19_VALV ) ) ) + { + if ( ++d > 40 ) + { + d = 0; + setPinchValvePosition( H1_VALV, p ); + setPinchValvePosition( H19_VALV, p ); + p++; + if ( p >= NUM_OF_VALVE_POSITIONS ) + { + p = 1; + } + } + } } +// if ( TRUE == startPos ) +// { +// startPos = FALSE; +// setPinchValvePosition( H1_VALV, VALVE_POSITION_B_OPEN ); +// setPinchValvePosition( H19_VALV, VALVE_POSITION_B_OPEN ); +// } #endif // State machine for standby mode