Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r2a2d6a224532304e29f948fc43711ee6e99f915b -r2b8e15565dbb160eeaca94e3126fc3e8a079a982 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 2a2d6a224532304e29f948fc43711ee6e99f915b) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 2b8e15565dbb160eeaca94e3126fc3e8a079a982) @@ -58,14 +58,14 @@ #define VALVE_TRANSITION_TIMEOUT_MS 5000 //TODO test the timeout #define MAX_POSITION_DEVIATION_FROM_EDGES_IN_COUNTS 150U #define MAX_ALLOWED_FAILED_HOMINGS 3U -#define HOMING_EDGE_DETECTION_TARGET_COUNTER 5U +#define HOMING_EDGE_DETECTION_TARGET_COUNTER 2U #define VALVES_CURRENT_THRESHOLD_AMPS 1U #define OVER_CURRENT_COUNTER 100U #define OUT_OF_RANGE_COUNTER 100U #define NEXT_STEP_COMD_TIME_INTERVAL_COUNTER 10U -#define DEENERGIZED_EDGE_CHECK_COUNTER 250U +#define DEENERGIZED_EDGE_CHECK_COUNTER 100U // TODO changed the interval count to every 10 calls in priority task which is 100 ms #define VALVES_DATA_PUB_INTERVAL ( MS_PER_SECOND / ( 10 * TASK_PRIORITY_INTERVAL ) ) @@ -100,7 +100,7 @@ VALVE_POSITION_T commandedPosition; VALVE_POSITION_T currentPosition; S16 currentPositionInCounts; - S16 nextPositionInCounts; + S16 targetPositionInCounts; BOOL hasTransitionBeenRequested; VALVE_STATE_T execState; U32 transitionStartTime; @@ -109,14 +109,14 @@ U32 valveOverCurrentCounter; U32 valvePositionOutOfRangeCounter; U32 valveDataPublishCounter; - U32 valveNextStepCmdCounter; + U32 valveRestCounter; // Homing variables S16 previousPositionInCounts; U32 numberOfFailedHomings; U32 homingEdgeDetectionCounter; BOOL hasHomingBeenRequested; BOOL hasValveBeenHomed; - U32 deenergizedEdgeCheckKickoffCounter; + U32 deenergizedEdgeCheckStartCounter; } VALVE_STATUS_T; #pragma pack(pop) @@ -389,6 +389,11 @@ valvesSelfTestResult = SELF_TEST_STATUS_PASSED; state = VALVE_SELF_TEST_COMPLETE; } + else + { + valvesSelfTestResult = SELF_TEST_STATUS_FAILED; + state = VALVE_SELF_TEST_COMPLETE; + } return state; } @@ -546,16 +551,16 @@ && isDoorClosed ) { // Get ready for the energized state - valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].numberOfFailedHomings = 0; - valvesStatus[ valve ].valveNextStepCmdCounter = 0; - valvesStatus[ valve ].nextPositionInCounts = STEP_CHANGE_IN_COUNTS; - valvesStatus[ valve ].previousPositionInCounts = 0; - valvesStatus[ valve ].hasValveBeenHomed = FALSE; - valvesStatus[ valve ].deenergizedEdgeCheckKickoffCounter = 0; - valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - setFPGAValue( valve, valvesStatus[ valve ].nextPositionInCounts ); + valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].numberOfFailedHomings = 0; + valvesStatus[ valve ].valveRestCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = 0; + valvesStatus[ valve ].previousPositionInCounts = 0; + valvesStatus[ valve ].hasValveBeenHomed = FALSE; + valvesStatus[ valve ].deenergizedEdgeCheckStartCounter = 0; + valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; } @@ -575,39 +580,30 @@ { VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; - // Check if the valve is within accepted range of the target - if ( ++valvesStatus[ valve ].valveNextStepCmdCounter >= NEXT_STEP_COMD_TIME_INTERVAL_COUNTER ) + S16 prevPosition = valvesStatus[ valve ].previousPositionInCounts; + S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; + S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; + + if ( fabs( fabs(prevPosition) - fabs(currentPosition) ) <= MAX_POSITION_DEVIATION_FROM_EDGES_IN_COUNTS && + ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) { - // Read the energized position back - S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; - S16 previousPosition = valvesStatus[ valve ].previousPositionInCounts; + // Current position (positive or negative) will be stored in the Position B of the current valve + // Do not use the above variables because they absolute valves using fabs + valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition; + valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + } - // Check if the valve is within the range from the energized edge and it has been there for the defined time - if( fabs( fabs( currentPosition ) - fabs( previousPosition ) ) <= MAX_POSITION_DEVIATION_FROM_EDGES_IN_COUNTS && - ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) + else if ( fabs( fabs(currentPosition) - fabs(targetPosition) ) <= 100 ) //TODO Add a #define for the edge tolerance + { + U32 restStatus = valvesStatus[ valve ].valveRestCounter; + + if ( ++restStatus >= 0 ) //TODO add a #define for the rest time. Do we need the rest? { - // Current position (positive or negative) will be stored in the Position B of the current valve - // Do not use the above variables because they absolute valves using fabs - valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition; - // Reset the current time and transition to the next state - valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].valveNextStepCmdCounter = 0; - state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + valvesStatus[ valve ].targetPositionInCounts = currentPosition + STEP_CHANGE_IN_COUNTS; + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); } - else - { - // Everything is normal and the valve is in transition - // Reset the number of failed homing attempts in case there was lingering failures from before - //valvesStatus[ valve ].homingEdgeDetectionCounter = 0; //TODO we should not do this - - valvesStatus[ valve ].nextPositionInCounts = valvesStatus[ valve ].nextPositionInCounts + STEP_CHANGE_IN_COUNTS; - - setFPGAValue( valve, valvesStatus[ valve ].nextPositionInCounts ); - } - - valvesStatus[ valve ].valveNextStepCmdCounter = 0; - valvesStatus[ valve ].previousPositionInCounts = currentPosition; } // Check if the transition has timed out /*else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) @@ -621,6 +617,7 @@ state = VALVE_STATE_HOMING_NOT_STARTED; }*/ + valvesStatus[ valve ].previousPositionInCounts = currentPosition; // TODO check is this too fast? valvesStatus[ valve ].execState = state; } @@ -637,54 +634,44 @@ { VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; - // Read the energized position back - S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; - S16 previousPosition = valvesStatus[ valve ].previousPositionInCounts; + S16 prevPosition = valvesStatus[ valve ].previousPositionInCounts; + S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; + S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - // Check if the valve is within accepted range of the target - if ( ++valvesStatus[ valve ].deenergizedEdgeCheckKickoffCounter >= DEENERGIZED_EDGE_CHECK_COUNTER ) + if ( fabs( fabs(prevPosition) - fabs(currentPosition) ) <= MAX_POSITION_DEVIATION_FROM_EDGES_IN_COUNTS && + ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER && + ++valvesStatus[ valve ].deenergizedEdgeCheckStartCounter >= DEENERGIZED_EDGE_CHECK_COUNTER ) { - // Check if the valve is within the range from the energized edge and it has been there for the defined time - if( fabs( fabs( currentPosition ) - fabs( previousPosition ) ) <= MAX_POSITION_DEVIATION_FROM_EDGES_IN_COUNTS && - ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) - { - S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; - // Position A is the average of the Position B that was read last time and position C that was - // the target of this state - S16 positionA = ( currentPosition + positionB ) / 2; - valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ] = positionA; + 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; - // 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; + // 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 ].positions[ VALVE_POSITION_C_CLOSE ] = currentPosition - INITIAL_EDGE_OFFSET_READ_COUNT; - - // Homing successfully finished - valvesStatus[ valve ].hasValveBeenHomed = TRUE; - - // Set the current position to Position C and the commanded position to Position A - // Request a transition - valvesStatus[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; - valvesStatus[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; - valvesStatus[ valve ].hasTransitionBeenRequested = TRUE; - valvesStatus[ valve ].hasHomingBeenRequested = FALSE; - - // Idle will initiate a transition - state = VALVE_STATE_IDLE; - } - - valvesStatus[ valve ].previousPositionInCounts = currentPosition; + // Idle will initiate a transition + state = VALVE_STATE_IDLE; } - if ( ++valvesStatus[ valve ].valveNextStepCmdCounter >= NEXT_STEP_COMD_TIME_INTERVAL_COUNTER ) + else if ( fabs( fabs(currentPosition) - fabs(targetPosition) ) <= 100 ) //TODO Add a #define for the edge tolerance { - valvesStatus[ valve ].nextPositionInCounts = valvesStatus[ valve ].nextPositionInCounts - STEP_CHANGE_IN_COUNTS; + U32 restStatus = valvesStatus[ valve ].valveRestCounter; - setFPGAValue( valve, valvesStatus[ valve ].nextPositionInCounts ); - - valvesStatus[ valve ].valveNextStepCmdCounter = 0; - valvesStatus[ valve ].previousPositionInCounts = currentPosition; + if ( ++restStatus >= 0 ) //TODO add a #define for the rest time. Do we need the rest? + { + valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + } } // Check if the transition has timed out @@ -699,6 +686,7 @@ state = VALVE_STATE_HOMING_NOT_STARTED; }*/ + valvesStatus[ valve ].previousPositionInCounts = currentPosition; // TODO check is this too fast? valvesStatus[ valve ].execState = state; } @@ -714,6 +702,7 @@ static void execMonitorValves( void ) { // TODO check enable status of the valves here too + areValvesEnabled(); // Get the current position of the valves in counts and store them getAndMonitorValvesCurrentPosition(); @@ -825,7 +814,7 @@ case VBA: - setFPGAValveBloodVenousPosition( position ); + setFPGAValveBloodArterialPosition( position ); break; case VBV: @@ -834,6 +823,7 @@ break; default: + // TODO alarm break; } } @@ -977,7 +967,7 @@ valveData.currentPosID = (U32)valvesStatus[ valve ].currentPosition; valveData.currentPos = valvesStatus[ valve ].currentPositionInCounts; valveData.prevPos = valvesStatus[ valve ].previousPositionInCounts; - valveData.nextPos = valvesStatus[ valve ].nextPositionInCounts; + valveData.nextPos = valvesStatus[ valve ].targetPositionInCounts; valveData.current = valvesStatus[ valve ].valveCurrent; broadcastHDValves(); @@ -988,7 +978,7 @@ // TODO REMOVE Fast 10ms publish fastDataRemoveLater.currentPos = valvesStatus[ valve ].currentPositionInCounts; fastDataRemoveLater.current = valvesStatus[ valve ].valveCurrent; - fastDataRemoveLater.cmdPosition = valvesStatus[ valve ].nextPositionInCounts; + fastDataRemoveLater.cmdPosition = valvesStatus[ valve ].targetPositionInCounts; broadcastFastTempHDValves(); // TODO REMOVE }