Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r361e8cae2edec18e5ba1a26690788b5c30ce4833 -r85457b793bef9f18e2d04cbc8f901c138eae3609 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 361e8cae2edec18e5ba1a26690788b5c30ce4833) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 85457b793bef9f18e2d04cbc8f901c138eae3609) @@ -61,9 +61,9 @@ #define STEP_CHANGE_IN_COUNTS 1000 -#define MAX_DEVIATION_FROM_TARGET_IN_COUNTS 150U +#define MAX_DEVIATION_FROM_TARGET_IN_COUNTS 150 #define MAX_ALLOWED_FAILED_HOMINGS 3U -#define HOMING_EDGE_DETECTION_TIME_INTERVAL ( TASK_PRIORITY_INTERVAL + 2U ) ///< The time that the valve must be at the edge to be considered for edge detection +#define HOMING_EDGE_DETECTION_TIME_INTERVAL ( MS_PER_SECOND / ( 2 * TASK_PRIORITY_INTERVAL ) ) ///< The time that the valve must be at the edge to be considered for edge detection #define VALVES_CURRENT_THRESHOLD_AMPS 1U #define OVER_CURRENT_COUNTER 100U #define OUT_OF_RANGE_COUNTER 100U @@ -145,6 +145,7 @@ static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve ); // Private function prototypes +static void setValveNextStep( VALVE_T valve ); static void execMonitorValves( void ); static BOOL areValvesEnabled( void ); static void setFPGAValue( VALVE_T valve, S16 position ); @@ -298,15 +299,12 @@ execMonitorValves(); // TODO un-comment the for loop - for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) + //for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) { //TODO FOR TESTING ONLY REMOVE - //valve = VDI; - //i = 0; + valve = VBA; //TODO FOR TESTING ONLY REMOVE - //valve = (VALVE_T)i; - switch ( valvesStatus[ valve ].execState ) { case VALVE_STATE_WAIT_FOR_POST: @@ -442,7 +440,7 @@ { // Get ready for the energized state valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPosition + STEP_CHANGE_IN_COUNTS; + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS; valvesStatus[ valve ].hasValveBeenHomed = FALSE; valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); @@ -463,20 +461,17 @@ *************************************************************************/ static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve ) { - BOOL hasStatusChanged = FALSE; - VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - S16 deltaPosition = currentPosition - targetPosition; + S16 deltaPosition = targetPosition - currentPosition; - //S16 absDeltaPosition = fabs( fabs(currentPosition) - fabs(targetPosition) ); - //if ( absDeltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && - // ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) TODO Remove + valvesStatus[ valve ].homingEdgeDetectionCounter++; // If there has not been any major travel for a certain period of time - if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS && - ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) + if ( deltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) { // Current position (positive or negative) will be stored in the Position B of the current valve valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition; @@ -486,22 +481,16 @@ state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } - else if ( currentPosition >= targetPosition ) + else if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { - hasStatusChanged = TRUE; + if ( valvesStatus[ valve ].valveRestCounter >= 0 )//TODO add a #define for the rest time. Do we need the rest? + { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition + STEP_CHANGE_IN_COUNTS; + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + } } - else if ( currentPosition < targetPosition && deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) - { - hasStatusChanged = TRUE; - } - if ( ++valvesStatus[ valve ].valveRestCounter >= 0 && hasStatusChanged ) //TODO add a #define for the rest time. Do we need the rest? - { - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = currentPosition + STEP_CHANGE_IN_COUNTS; - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); - } - return state; } @@ -516,19 +505,16 @@ *************************************************************************/ static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ) { - BOOL hasStatusChanged = FALSE; VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - S16 deltaPosition = currentPosition - targetPosition; + S16 deltaPosition = targetPosition - currentPosition; - //S16 absDeltaPosition = fabs( fabs(currentPosition) - fabs(targetPosition) ); - //if ( absDeltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && - // ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) TODO remove + valvesStatus[ valve ].homingEdgeDetectionCounter++; - if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS && - ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) + if ( abs(deltaPosition) > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) { S16 energizedEdge = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; S16 deltaEdges = energizedEdge - currentPosition; @@ -541,7 +527,7 @@ valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] = currentPosition + INITIAL_EDGE_OFFSET_READ_COUNT; // Position A is the average of the Position B that was read last time and position C that was the target of this state valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ] = ( valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] + - valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] ) / 2; + abs(valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ]) ) / 2; // Set the current position to Position C and the commanded position to Position A // Request a transition valvesStatus[ valve ].hasValveBeenHomed = TRUE; @@ -555,7 +541,7 @@ state = VALVE_STATE_IDLE; } // Max number of failed homing. Fault and go back to homing not started - else if ( ++valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) + else if ( valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) { //TODO Alarm valvesStatus[ valve ].hasValveBeenHomed = FALSE; @@ -564,26 +550,21 @@ } else { + valvesStatus[ valve ].numberOfFailedHomings++; state = VALVE_STATE_HOMING_NOT_STARTED; } } - else if ( currentPosition > targetPosition && deltaPosition <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + else if ( abs(deltaPosition) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { - hasStatusChanged = TRUE; + if ( valvesStatus[ valve ].valveRestCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest? + { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + } } - else if ( currentPosition < targetPosition ) - { - hasStatusChanged = TRUE; - } - if ( ++valvesStatus[ valve ].valveRestCounter >= 0 && hasStatusChanged ) //TODO add a #define for the rest time. Do we need the rest? - { - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); - } - return state; } @@ -609,11 +590,9 @@ { if ( valvesStatus[ valve ].currentPosition != valvesStatus[ valve ].commandedPosition ) { - VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; - S16 nextStep = valvesStatus[ valve ].positions[commandedPositionEnum ]; // Just set the valves to transition so it will not be in a known position for a while valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); - setFPGAValue( valve, nextStep ); + setValveNextStep( valve ); state = VALVE_STATE_IN_TRANSITION; } @@ -634,19 +613,16 @@ *************************************************************************/ static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve ) { - S16 nextStep = 0; VALVE_STATE_T state = VALVE_STATE_IN_TRANSITION; VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; - VALVE_POSITION_T currentPositionEnum = valvesStatus[ valve ].currentPosition; // Get the corresponding counts of the positions S16 commandedPosition = valvesStatus[ valve ].positions[ commandedPositionEnum ]; S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; // Check if the valve is within range of the commanded position - //if ( fabs( fabs(commandedPositionCounts) - fabs(currentPositionCounts) ) <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) TODO Remove if ( abs( currentPosition - commandedPosition ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { // The valve is in range. Set the current position to the commanded position @@ -667,42 +643,7 @@ //TODO not sure if this works or not, Test this else if ( currentPosition - targetPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { - if ( currentPosition - commandedPosition <= STEP_CHANGE_IN_COUNTS ) - { - nextStep = abs( currentPosition - commandedPosition ); - } - else - { - nextStep = STEP_CHANGE_IN_COUNTS; - } - - // If the valve is currently in position A and its commanded position is position B, - // add the defined number of steps for the next transition - // If the valve is currently in position A and its commanded position is position C, - // subtract the defined number of steps for the next transition - if ( currentPositionEnum == VALVE_POSITION_A_INSERT_EJECT ) - { - if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) - { - valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; - } - if ( commandedPositionEnum == VALVE_POSITION_C_CLOSE ) - { - valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; - } - } - // If the valve is currently in position B, subtract the defined number of steps for the next transition - else if ( currentPositionEnum == VALVE_POSITION_B_OPEN ) - { - valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; - } - // If the valve is currently in position C, add the defined number of steps for the next transition - else if ( currentPositionEnum == VALVE_POSITION_C_CLOSE ) - { - valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; - } - - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + setValveNextStep( valve ); } return state; @@ -994,7 +935,7 @@ valveData.posA = valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ]; valveData.posB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; - broadcastHDValves(); + broadcastHDValves( &valveData ); valvesStatus[ valve ].valveDataPublishCounter = 0; } @@ -1007,6 +948,52 @@ // TODO REMOVE } +static void setValveNextStep( VALVE_T valve ) +{ + S16 nextStep = 0; + VALVE_POSITION_T currentPositionEnum = valvesStatus[ valve ].currentPosition; + VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; + S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; + S16 commandedPosition = valvesStatus[ valve ].positions[ commandedPositionEnum ]; + + if ( abs( currentPosition - commandedPosition ) <= STEP_CHANGE_IN_COUNTS ) + { + nextStep = abs( currentPosition - commandedPosition ); + } + else + { + nextStep = STEP_CHANGE_IN_COUNTS; + } + + // If the valve is currently in position A and its commanded position is position B, + // add the defined number of steps for the next transition + // If the valve is currently in position A and its commanded position is position C, + // subtract the defined number of steps for the next transition + if ( currentPositionEnum == VALVE_POSITION_A_INSERT_EJECT ) + { + if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; + } + if ( commandedPositionEnum == VALVE_POSITION_C_CLOSE ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; + } + } + // If the valve is currently in position B, subtract the defined number of steps for the next transition + else if ( currentPositionEnum == VALVE_POSITION_B_OPEN ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; + } + // If the valve is currently in position C, add the defined number of steps for the next transition + else if ( currentPositionEnum == VALVE_POSITION_C_CLOSE ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; + } + + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); +} + /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ @@ -1118,5 +1105,4 @@ return result; } - /**@}*/