Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r7ce686fa967206c341805b79916298ae187854de -r9224712d825c7ecee623abc78d704fbd3c2dbde8 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 7ce686fa967206c341805b79916298ae187854de) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 9224712d825c7ecee623abc78d704fbd3c2dbde8) @@ -1,5 +1,8 @@ // Includes +#include "reg_het.h" +#include "gio.h" + #include "Valves.h" #include "FPGA.h" #include "Timers.h" @@ -50,16 +53,17 @@ #define ADC_TO_CURRENT_CONVERSION_CONSTANT 2048.0 #define INITIAL_EDGE_OFFSET_READ_COUNT 100 -#define INITIAL_DEENERGIZED_POSITION ( 0 - INITIAL_EDGE_OFFSET_READ_COUNT ) -#define INITIAL_ENERGIZED_POSITION ( 12000 + INITIAL_EDGE_OFFSET_READ_COUNT ) +#define INITIAL_ENERGIZED_EDGE_UPPER_RANGE 13000U +#define INITIAL_ENERGIZED_EDGE_LOWER_RANGE 9000U + #define STEP_CHANGE_IN_COUNTS 1000 -#define MAX_POS_DEVIATION_FROM_TARGET_COUNTS 1000 //TODO test this tolerance -#define VALVE_TRANSITION_TIMEOUT_MS 5000 //TODO test the timeout -#define MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS 150U -#define MAX_ALLOWED_FAILED_HOMINGS 3U -#define HOMING_EDGE_DETECTION_TARGET_COUNTER 12U -#define VALVES_CURRENT_THRESHOLD_AMPS 1U +//#define MAX_POS_DEVIATION_FROM_TARGET_COUNTS 1000 //TODO test this tolerance +//#define VALVE_TRANSITION_TIMEOUT_MS 5000 //TODO test the timeout +#define MAX_DEVIATION_FROM_TARGET_IN_COUNTS 150U +#define MAX_ALLOWED_FAILED_HOMINGS 3U +#define HOMING_EDGE_DETECTION_TARGET_COUNTER 12U +#define VALVES_CURRENT_THRESHOLD_AMPS 1U #define OVER_CURRENT_COUNTER 100U #define OUT_OF_RANGE_COUNTER 100U @@ -158,6 +162,9 @@ { U08 i; + // To set the NHET direction to output + //gioSetDirection( hetPORT1, 1 ); + valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES; valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS; @@ -234,7 +241,15 @@ *************************************************************************/ BOOL setValveBloodTrap( OPN_CLS_STATE_T state ) { - //TODO fill up the function + BOOL energized = FALSE; + + if ( state == STATE_OPEN ) + { + energized = TRUE; + } + + gioSetBit( hetPORT1, 0x12, energized ); + return TRUE; } @@ -463,14 +478,14 @@ S16 nextStep = 0; VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; - VALVE_POSITION_T currentPositionEnum = valvesStatus[ valve ].currentPosition; + VALVE_POSITION_T currentPositionEnum = valvesStatus[ valve ].currentPosition; // Get the corresponding counts of the positions - S16 commandedPositionCounts = fabs( valvesStatus[ valve ].positions[ commandedPositionEnum ] ); - S16 currentPositionCounts = fabs( valvesStatus[ valve ].currentPositionInCounts ); + S16 commandedPositionCounts = valvesStatus[ valve ].positions[ commandedPositionEnum ]; + S16 currentPositionCounts = valvesStatus[ valve ].currentPositionInCounts; // Check if the valve is within range of the commanded position - if ( fabs( commandedPositionCounts - currentPositionCounts ) <= STEP_CHANGE_IN_COUNTS - 1 ) + if ( fabs( fabs(commandedPositionCounts) - fabs(currentPositionCounts) ) <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { // The valve is in range. Set the current position to the commanded position valvesStatus[ valve ].currentPosition = commandedPositionEnum; @@ -546,7 +561,6 @@ { // Get ready for the energized state valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].numberOfFailedHomings = 0; valvesStatus[ valve ].targetPositionInCounts = STEP_CHANGE_IN_COUNTS; valvesStatus[ valve ].hasValveBeenHomed = FALSE; valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; @@ -573,7 +587,9 @@ S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - if ( fabs( fabs(targetPosition) - fabs(currentPosition) ) > MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS && + S16 absDeltaPosition = fabs( fabs(currentPosition) - fabs(targetPosition) ); + + if ( absDeltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) { // Current position (positive or negative) will be stored in the Position B of the current valve @@ -584,7 +600,7 @@ state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } - else if ( fabs( fabs(currentPosition) - fabs(targetPosition) ) <= MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS ) + else if ( absDeltaPosition <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { if ( ++valvesStatus[ valve ].valveRestCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest? { @@ -613,30 +629,50 @@ S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - if ( fabs( fabs(targetPosition) - fabs(currentPosition) ) > MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS && + S16 absDeltaPosition = fabs( fabs(currentPosition) - fabs(targetPosition) ); + + if ( absDeltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) { - S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; - // Positions B and C will have an offset from the edge to make sure each time the valve will not hit the edge - valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = positionB - INITIAL_EDGE_OFFSET_READ_COUNT; - 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; + S16 energizedEdge = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; + S16 absEnergizedDelta = fabs( fabs(energizedEdge) - fabs(currentPosition) ); - // Set the current position to Position C and the commanded position to Position A - // Request a transition - valvesStatus[ valve ].hasValveBeenHomed = TRUE; - valvesStatus[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; - valvesStatus[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; - valvesStatus[ valve ].hasTransitionBeenRequested = TRUE; - valvesStatus[ valve ].hasHomingBeenRequested = FALSE; + if ( absEnergizedDelta <= INITIAL_ENERGIZED_EDGE_UPPER_RANGE && absEnergizedDelta >= INITIAL_ENERGIZED_EDGE_LOWER_RANGE ) + { + S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; + // Positions B and C will have an offset from the edge to make sure each time the valve will not hit the edge + valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = positionB - INITIAL_EDGE_OFFSET_READ_COUNT; + 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; - // Idle will initiate a transition - state = VALVE_STATE_IDLE; + // Set the current position to Position C and the commanded position to Position A + // Request a transition + valvesStatus[ valve ].hasValveBeenHomed = TRUE; + valvesStatus[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; + valvesStatus[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; + valvesStatus[ valve ].hasTransitionBeenRequested = TRUE; + valvesStatus[ valve ].hasHomingBeenRequested = FALSE; + valvesStatus[ valve ].numberOfFailedHomings = 0; + + // Idle will initiate a transition + state = VALVE_STATE_IDLE; + } + // Max number of failed homing. Fault + else if ( ++valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) + { + //TODO Alarm + valvesStatus[ valve ].hasValveBeenHomed = FALSE; + state = VALVE_STATE_IDLE; + } + else + { + state = VALVE_STATE_HOMING_NOT_STARTED; + } + } - else if ( fabs( fabs(currentPosition) - fabs(targetPosition) ) <= MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS ) + else if ( absDeltaPosition <= MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { if ( ++valvesStatus[ valve ].valveRestCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest? { @@ -870,14 +906,14 @@ // Check if the current position has deviated from the position it is supposed to be in // for more than a certain amount of time. If it has, raise an alarm - if ( fabs( currentPostion - commandedPoistion ) > MAX_POS_DEVIATION_FROM_TARGET_COUNTS && + if ( fabs( currentPostion - commandedPoistion ) > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && ++valvesStatus[ i ].valvePositionOutOfRangeCounter > OUT_OF_RANGE_COUNTER ) { // TODO fault } // If the deviation came back to within the maximum amount and the time counter was > 0, // set the counter to 0 - else if ( fabs( currentPostion - commandedPoistion ) < MAX_POS_DEVIATION_FROM_TARGET_COUNTS && + else if ( fabs( currentPostion - commandedPoistion ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS && valvesStatus[ i ].valvePositionOutOfRangeCounter > 0 ) { valvesStatus[ i ].valvePositionOutOfRangeCounter = 0;