Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r5aa897d2ca22435bf7ec90dfdf656facb740276a -rbe1f6ba8f58abfe098865c85ebca070eb0dde6ce --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 5aa897d2ca22435bf7ec90dfdf656facb740276a) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision be1f6ba8f58abfe098865c85ebca070eb0dde6ce) @@ -273,56 +273,49 @@ for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { - if ( valve < NUM_OF_VALVES ) + // update valve encoder positions + currentValveStates[ valve ].priorEncPosition = currentValveStates[ valve ].currentEncPosition; + currentValveStates[ valve ].currentEncPosition = getValveEncoderPosition( valve ); + + // execute valve state machine + switch ( currentValveStates[ valve ].controlState ) { - // update valve encoder positions - currentValveStates[ valve ].priorEncPosition = currentValveStates[ valve ].currentEncPosition; - currentValveStates[ valve ].currentEncPosition = getValveEncoderPosition( valve ); + case VALVE_STATE_WAIT_FOR_POST: + currentValveStates[ valve ].controlState = handleValvesWait4PostState( valve ); + break; - // execute valve state machine - switch ( currentValveStates[ valve ].controlState ) - { - case VALVE_STATE_WAIT_FOR_POST: - currentValveStates[ valve ].controlState = handleValvesWait4PostState( valve ); - break; + case VALVE_STATE_RESET_VALVE: + currentValveStates[ valve ].controlState = handleValvesResetValve( 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_RESET_ENCODER: - currentValveStates[ valve ].controlState = handleValvesResetEncoder( valve ); - break; + case VALVE_STATE_ENABLE_VALVE: + currentValveStates[ valve ].controlState = handleValvesEnableValve( 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; - case VALVE_STATE_HOMING_NOT_STARTED: - currentValveStates[ valve ].controlState = handleValvesNotHomedState( valve ); - break; + case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: + currentValveStates[ valve ].controlState = handleValvesFindEnergizedEdgeState( valve ); + break; - case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: - currentValveStates[ valve ].controlState = handleValvesFindEnergizedEdgeState( valve ); - break; + case VALVE_STATE_IDLE: + currentValveStates[ valve ].controlState = handleValvesIdleState( valve ); + break; - case VALVE_STATE_IDLE: - currentValveStates[ valve ].controlState = handleValvesIdleState( valve ); - break; + case VALVE_STATE_IN_TRANSITION: + currentValveStates[ valve ].controlState = handleValvesTransitionState( valve ); + break; - case VALVE_STATE_IN_TRANSITION: - currentValveStates[ valve ].controlState = handleValvesTransitionState( valve ); - break; - - default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_STATE, (U32)currentValveStates[ valve ].controlState ) - break; - } + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_STATE, (U32)currentValveStates[ valve ].controlState ) + break; } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) - } } publishValvesData(); @@ -487,7 +480,7 @@ // 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; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_D_PARTIAL_CLOSE ] = POS_D_VENOUS_OFFSET_FROM_ZERO_CNT; nextState = VALVE_STATE_HOMING_NOT_STARTED; #endif @@ -611,7 +604,7 @@ else { S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - S16 posA = posC + ( curPos / 2 ); + S16 posA = posC + ( ( curPos - posC ) / 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 ) << 3 : posA; @@ -802,6 +795,7 @@ data.posA = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ]; data.posB = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ]; data.posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + data.posD = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_D_PARTIAL_CLOSE ]; broadcastData( MSG_ID_TD_VALVES_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&data, sizeof( TD_VALVE_DATA_T ) ); }