Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r8b819deb3e770c981eed221f63170dd842eedbe3 -r8de3c971a08a5e2d822a6a1d1d1c477b6d4cbd5b --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 8b819deb3e770c981eed221f63170dd842eedbe3) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 8de3c971a08a5e2d822a6a1d1d1c477b6d4cbd5b) @@ -33,18 +33,18 @@ // ********** private definitions ********** -#define VALVES_DATA_PUB_INTERVAL (TASK_GENERAL_INTERVAL/TASK_GENERAL_INTERVAL) // TODO setup //( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Valves data publish interval. +#define VALVES_DATA_PUB_INTERVAL ( 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. #define VALVE_HOME_BACK_OFF_EDGE 3 ///< Encoder counts to back off of detected edge position. -#define MAX_ALLOWED_FAILED_VALVE_HOMINGS 3U ///< Maximum allowed failed valve home attempts. #define MAX_HOME_FULL_TRAVEL_DIFF 10U ///< Maximum allowed difference in full travel encoder counts between expected and measured during home operation. #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_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_VENOUS_OFFSET_FROM_ZERO_CNT 32 ///< Position D venous offset from zero position in counts. /// Valve controller states typedef enum Valve_Control_States @@ -85,17 +85,12 @@ U32 overCurrentCounter; ///< Valve over current counter. U32 positionOutOfRangeCounter; ///< Valve position out of range counter. // Homing variables - 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 + 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. - - S16 testREMOVEARRAY[ 60 ]; - U32 indexREMOVE; } VALVE_STATUS_T; /// Payload record structure for pinch valve set position request @@ -148,11 +143,11 @@ // Initialize controller variables memset( ¤tValveStates[0], 0, sizeof( currentValveStates ) ); - valvesDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; - valvesDataPublishInterval.data = VALVES_DATA_PUB_INTERVAL; - valvesDataPublishInterval.ovData = VALVES_DATA_PUB_INTERVAL; + valvesDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; + valvesDataPublishInterval.data = VALVES_DATA_PUB_INTERVAL; + valvesDataPublishInterval.ovData = VALVES_DATA_PUB_INTERVAL; valvesDataPublishInterval.ovInitData = 0; - valvesDataPublishInterval.override = OVERRIDE_RESET; + valvesDataPublishInterval.override = OVERRIDE_RESET; } /*********************************************************************//** @@ -338,11 +333,7 @@ *************************************************************************/ static VALVE_STATE_T handleValvesWait4PostState( VALVE_T valve ) { -#ifndef TEST_PINCH_VALVES VALVE_STATE_T nextState = VALVE_STATE_WAIT_FOR_POST; -#else - VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; -#endif if ( getCurrentOperationMode() != MODE_INIT ) { @@ -418,9 +409,6 @@ // 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; } @@ -440,9 +428,6 @@ FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; S16 valveEncPosistion = getValveEncoderPosition( valve ); - currentValveStates[ valve ].testREMOVEARRAY[ currentValveStates[ valve ].indexREMOVE ] = valveEncPosistion; - currentValveStates[ valve ].indexREMOVE++; - if ( FALSE == currentValveStates[ valve ].hasValveBeenReset ) { switch ( valve ) @@ -462,15 +447,37 @@ currentValveStates[ valve ].hasValveBeenReset = TRUE; } + else + { + // First enable the valve and then set the position to 0 to clear the FPGA command regitsters + switch ( valve ) + { + case H1_VALV: + setH1Position( 0 ); + break; + case H19_VALV: + setH19Position( 0 ); + break; + + default: + // Do nothing as the fault mode is checked in the exec function + break; + } + } + 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 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; + // 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_D_VENOUS ] = POS_D_VENOUS_OFFSET_FROM_ZERO_CNT; + nextState = VALVE_STATE_HOMING_NOT_STARTED; } return nextState; @@ -510,11 +517,10 @@ 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; + // Command valve to move to energized edge (end of travel) + currentValveStates[ valve ].valveOpsStartTime = 0; + 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; @@ -563,9 +569,13 @@ else { S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + S16 posA = posC + ( curPos / 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 ) : posA; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos; - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posC + ( ( curPos - posC ) / 2 ); + 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; @@ -590,12 +600,6 @@ break; } - - //if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) - //{ - // currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_REVERSE; - //} - currentValveStates[ valve ].hasValveBeenReset = FALSE; nextState = VALVE_STATE_RESET_VALVE; } @@ -619,11 +623,25 @@ // handle a home request if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - currentValveStates[ valve ].hasValveBeenReset = FALSE; - currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_INIT; + 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 - // TODO Check if reset is not needed first - nextState = VALVE_STATE_RESET_VALVE; + // 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; + 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; + } } // handle a position change request else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) Index: firmware/App/Controllers/Valves.h =================================================================== diff -u -r8b819deb3e770c981eed221f63170dd842eedbe3 -r8de3c971a08a5e2d822a6a1d1d1c477b6d4cbd5b --- firmware/App/Controllers/Valves.h (.../Valves.h) (revision 8b819deb3e770c981eed221f63170dd842eedbe3) +++ firmware/App/Controllers/Valves.h (.../Valves.h) (revision 8de3c971a08a5e2d822a6a1d1d1c477b6d4cbd5b) @@ -40,6 +40,7 @@ VALVE_POSITION_A_INSERT_EJECT, ///< Position A, Insert/Eject VALVE_POSITION_B_OPEN, ///< Position B, Open VALVE_POSITION_C_CLOSE, ///< Position C, Close + VALVE_POSITION_D_VENOUS, ///< Position D, Venous NUM_OF_VALVE_POSITIONS, ///< Number of valve positions } VALVE_POSITION_T; @@ -52,9 +53,9 @@ U32 currentPosID; ///< Current position (enum) S16 currentPos; ///< Current position in encoder counts S16 cmdPos; ///< Commanded position (enum) - S16 posA; - S16 posB; - S16 posC; + S16 posA; ///< Position A (count) + S16 posB; ///< Position B (count) + S16 posC; ///< Position C (count) } TD_VALVE_DATA_T; #pragma pack(pop) Index: firmware/App/Drivers/RotaryValve.c =================================================================== diff -u -r8b819deb3e770c981eed221f63170dd842eedbe3 -r8de3c971a08a5e2d822a6a1d1d1c477b6d4cbd5b --- firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision 8b819deb3e770c981eed221f63170dd842eedbe3) +++ firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision 8de3c971a08a5e2d822a6a1d1d1c477b6d4cbd5b) @@ -67,7 +67,6 @@ for ( i = FIRST_VALVE; i < NUM_OF_VALVES; i++ ) { - //valveControl[ i ] = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; // enable valves, configure for 1/8 step control currentValveEncPosition[ i ].data = 0; currentValveEncPosition[ i ].ovData = 0; currentValveEncPosition[ i ].ovInitData = 0; @@ -79,10 +78,6 @@ } commandValvePos[ H1_VALV ] = getH1EncoderPosition(); commandValvePos[ H19_VALV ] = getH19EncoderPosition(); - - // set valve control bits once at startup - //setH1Control( valveControl[ H1_VALV ] ); - //setH19Control( valveControl[ H19_VALV ] ); } /*********************************************************************//**