Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -reaff654b897f641b874e3ba30a22cbda3779e4a7 -ra811cae0392f1d0f976650ff167945e614b3b854 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision eaff654b897f641b874e3ba30a22cbda3779e4a7) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision a811cae0392f1d0f976650ff167945e614b3b854) @@ -17,13 +17,15 @@ #include "AlarmMgmtTD.h" #include "CpldInterface.h" +#include "FpgaTD.h" #include "Messaging.h" #include "OperationModes.h" #include "PersistentAlarm.h" #include "TaskGeneral.h" #include "Timers.h" #include "Valves.h" + /** * @addtogroup Valves * @{ @@ -33,19 +35,22 @@ #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 1000 //10 ///< Minimum encoder position delta from target position to end transition state. +#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_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 3000U ///< Valves transition time out in ms +#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 VALVE_FORCE_HOME TRUE ///< Force valve to home even if already homed - /// Valve controller states typedef enum Valve_Control_States { VALVE_STATE_WAIT_FOR_POST = 0, ///< Valve state wait for POST + VALVE_STATE_RESET_VALVE, + VALVE_STATE_RESET_ENCODER, + VALVE_STATE_ENABLE_VALVE, VALVE_STATE_HOMING_NOT_STARTED, ///< Valve state homing not started VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE, ///< Valve state homing find energized edge VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE, ///< Valve state homing find de-energized edge @@ -69,6 +74,7 @@ S16 positionsABC[ NUM_OF_VALVE_POSITIONS ]; ///< Valve positions (in encoder counts) for positions A, B, and C U32 overCurrentCounter; ///< Valve over current counter U32 positionOutOfRangeCounter; ///< Valve position out of range counter + BOOL haveValvesBeenReset; // Homing variables U32 numberOfFailedHomings; ///< Valve number of failed homing U32 homingEdgeDetectionCounter; ///< Valve homing counter when there is not much travel @@ -103,6 +109,9 @@ // ********** private function prototypes ********** static VALVE_STATE_T handleValvesWait4PostState( VALVE_T valve ); +static VALVE_STATE_T handleValvesResetValve( VALVE_T valve ); +static VALVE_STATE_T handleValvesResetEncoder( VALVE_T valve ); +static VALVE_STATE_T handleValvesEnableValve( VALVE_T valve ); static VALVE_STATE_T handleValvesNotHomedState( VALVE_T valve ); static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ); static VALVE_STATE_T handleValvesFindDeenergizedEdgeState( VALVE_T valve ); @@ -260,6 +269,18 @@ currentValveStates[ valve ].controlState = handleValvesWait4PostState( valve ); break; + case VALVE_STATE_RESET_VALVE: + currentValveStates[ valve ].controlState = handleValvesResetValve( valve ); + break; + + case VALVE_STATE_RESET_ENCODER: + currentValveStates[ valve ].controlState = handleValvesResetEncoder( valve ); + break; + + case VALVE_STATE_ENABLE_VALVE: + currentValveStates[ valve ].controlState = handleValvesEnableValve( valve ); + break; + case VALVE_STATE_HOMING_NOT_STARTED: currentValveStates[ valve ].controlState = handleValvesNotHomedState( valve ); break; @@ -311,7 +332,7 @@ { if ( getCurrentOperationMode() != MODE_INIT ) { - nextState = VALVE_STATE_HOMING_NOT_STARTED; + nextState = VALVE_STATE_RESET_VALVE; } } else @@ -322,6 +343,79 @@ return nextState; } +static VALVE_STATE_T handleValvesResetValve( VALVE_T valve ) +{ + VALVE_STATE_T nextState = VALVE_STATE_RESET_ENCODER; + U08 resetValve = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_ENABLE_ENCODER | FPGA_PINCH_VALVES_DISABLE | + FPGA_PINCH_VALVES_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; + + switch ( valve ) + { + case H1_VALV: + setH1Control( resetValve ); + break; + + case H19_VALV: + setH19Control( resetValve ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) + break; + } + + return nextState; +} + + +static VALVE_STATE_T handleValvesResetEncoder( VALVE_T valve ) +{ + VALVE_STATE_T nextState = VALVE_STATE_ENABLE_VALVE; + U08 resetEnc = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_RESET_ENCODER | FPGA_PINCH_VALVES_DISABLE | + FPGA_PINCH_VALVES_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; + + switch ( valve ) + { + case H1_VALV: + setH1Control( resetEnc ); + break; + + case H19_VALV: + setH19Control( resetEnc ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) + break; + } + + return nextState; +} + +static VALVE_STATE_T handleValvesEnableValve( VALVE_T valve ) +{ + VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; + U08 enableValve = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_ENABLE_ENCODER | FPAG_PINCH_VALVES_ENABLE | + FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; + + switch ( valve ) + { + case H1_VALV: + setH1Control( enableValve ); + break; + + case H19_VALV: + setH19Control( enableValve ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) + break; + } + + return nextState; +} + /*********************************************************************//** * @brief * The handleValvesNotHomedState function handles the Not Homed state @@ -347,6 +441,7 @@ currentValveStates[ valve ].targetEncPosition = mag; currentValveStates[ valve ].hasValveBeenHomed = FALSE; currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + currentValveStates[ valve ].transitionStartTime = 0; setValveCmdChangePosition( valve, (U16)mag, MOTOR_DIR_FORWARD ); nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; } @@ -377,18 +472,26 @@ { S16 curPos = currentValveStates[ valve ].currentEncPosition; - // have we found foward edge of travel? + // have we found forward edge of travel? if ( fabs( curPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) { - U16 mag = ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS + VALVE_HOME_MIN_POS_CHG; + if ( 0 == currentValveStates[ valve ].transitionStartTime ) + { + currentValveStates[ valve ].transitionStartTime = getMSTimerCount(); + } + else if ( TRUE == didTimeout( currentValveStates[ valve ].transitionStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) + { + U16 mag = ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS + VALVE_HOME_MIN_POS_CHG; - // Record edge position - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos - VALVE_HOME_BACK_OFF_EDGE; - // command valve to move to de-energized edge (end of travel) - currentValveStates[ valve ].homingEdgeDetectionCounter = 0; - currentValveStates[ valve ].targetEncPosition = 0; - setValveCmdChangePosition( valve, mag, MOTOR_DIR_REVERSE ); - nextState = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + // Record edge position + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos - VALVE_HOME_BACK_OFF_EDGE; + // command valve to move to de-energized edge (end of travel) + currentValveStates[ valve ].homingEdgeDetectionCounter = 0; + currentValveStates[ valve ].targetEncPosition = 0; + currentValveStates[ valve ].transitionStartTime = 0; + setValveCmdChangePosition( valve, mag, MOTOR_DIR_REVERSE ); + nextState = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + } } } else @@ -452,7 +555,7 @@ currentValveStates[ valve ].hasHomingBeenRequested = FALSE; SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_HOMING_FAILED, (U32)valve, (U32)edgeDelta ); } - nextState = VALVE_STATE_HOMING_NOT_STARTED; + nextState = VALVE_STATE_RESET_VALVE; } } } @@ -483,7 +586,7 @@ // handle a home request if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - nextState = VALVE_STATE_HOMING_NOT_STARTED; + nextState = VALVE_STATE_RESET_VALVE; } // handle a position change request else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) Index: firmware/App/Drivers/RotaryValve.c =================================================================== diff -u -r6b96f1a18ab2f93068daddbf24178fbbae7cc570 -ra811cae0392f1d0f976650ff167945e614b3b854 --- firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision 6b96f1a18ab2f93068daddbf24178fbbae7cc570) +++ firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision a811cae0392f1d0f976650ff167945e614b3b854) @@ -61,13 +61,13 @@ * @details \b Outputs: Rotary valves driver unit is initialized * @return none *************************************************************************/ -void initRotaryValvesDriver(void) +void initRotaryValvesDriver( void ) { U32 i; 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 + //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; @@ -81,8 +81,8 @@ commandValvePos[ H19_VALV ] = getH19EncoderPosition(); // set valve control bits once at startup - setH1Control( valveControl[ H1_VALV ] ); - setH19Control( valveControl[ H19_VALV ] ); + //setH1Control( valveControl[ H1_VALV ] ); + //setH19Control( valveControl[ H19_VALV ] ); } /*********************************************************************//** Index: firmware/App/Drivers/RotaryValve.h =================================================================== diff -u -r0e4a62ba24d906d9e5b33b293f93405b8c0391ce -ra811cae0392f1d0f976650ff167945e614b3b854 --- firmware/App/Drivers/RotaryValve.h (.../RotaryValve.h) (revision 0e4a62ba24d906d9e5b33b293f93405b8c0391ce) +++ firmware/App/Drivers/RotaryValve.h (.../RotaryValve.h) (revision a811cae0392f1d0f976650ff167945e614b3b854) @@ -35,7 +35,7 @@ #define ROTARY_VALVE_MICROSTEP_FRACTION 8 ///< Rotary valve motors configured for 1/8 step microstepping. #define ROTARY_VALVE_GEAR_RATIO 3.7F ///< Rotary valve motor gear ratio is 3.7:1. -#define ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS 1798 ///< Rotary valve swing arm travel range is appx. micro steps. +#define ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS 1760 ///< Rotary valve swing arm travel range is appx. micro steps. /// rotary pinch valve names typedef enum valveNames Index: firmware/App/Services/FpgaTD.h =================================================================== diff -u -r37294a71c0b3e3ca67ccb10b85536d124d3ef028 -ra811cae0392f1d0f976650ff167945e614b3b854 --- firmware/App/Services/FpgaTD.h (.../FpgaTD.h) (revision 37294a71c0b3e3ca67ccb10b85536d124d3ef028) +++ firmware/App/Services/FpgaTD.h (.../FpgaTD.h) (revision a811cae0392f1d0f976650ff167945e614b3b854) @@ -33,8 +33,11 @@ // Bit definitions for pinch valve control register #define FPGA_PINCH_VALVES_1_8_STEP 0x07 ///< Bit mask for configuring pinch valve for 1/8 step microstepping. +#define FPGA_PINCH_VALVES_ENABLE_ENCODER 0x00 #define FPGA_PINCH_VALVES_RESET_ENCODER 0x08 ///< Bit mask for configuring pinch valve to reset the encoder. +#define FPAG_PINCH_VALVES_ENABLE 0x00 #define FPGA_PINCH_VALVES_DISABLE 0x10 ///< Bit mask for configuring pinch valve to be disabled. +#define FPGA_PINCH_VALVES_RESET 0x00 ///< Bit mask for configuring pinch valve to be in reset. #define FPGA_PINCH_VALVES_NOT_RESET 0x20 ///< Bit mask for configuring pinch valve to not be in reset. #define FPGA_PINCH_VALVES_NOT_SLEEP 0x40 ///< Bit mask for configuring pinch valve to not be in sleep mode. #define FPGA_PINCH_VALVES_NEW_POS_CMD 0x80 ///< Bit mask for configuring pinch valve to accept a new position command.