Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r1c6d80a873aa1bd11e28a8955ff38866fc5717f4 -reef24091316f7c4b5717e72fef7ca06e58fc174a --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 1c6d80a873aa1bd11e28a8955ff38866fc5717f4) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision eef24091316f7c4b5717e72fef7ca06e58fc174a) @@ -43,7 +43,7 @@ #define VALVE_FORCE_HOME TRUE ///< Force valve to home even if already homed. #define ZERO_ENC_DEBOUNCE_THRESHOLD_CNT 5 ///< Valves zero encoder debounce threshold count. #define ZERO_ENC_DEBOUNCE_TIMEOUT_MS ( 0.25 * MS_PER_SECOND ) ///< Valves zero encoder debounce timeout in milliseconds. -#define VALVE_OFFEST_FROM_EDG_CNT 32 ///< Valves offset from the edge. +#define VALVE_OFFEST_FROM_EDG_CNT 200 ///< Valves offset from the edge. #define POS_C_FROM_ZERO_CNT VALVE_OFFEST_FROM_EDG_CNT ///< Position C from zero position in counts. #define POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT ( POS_C_FROM_ZERO_CNT + 32 ) ///< Position D partial close from zero position in counts. @@ -56,6 +56,7 @@ VALVE_STATE_ENABLE_VALVE, ///< 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. VALVE_STATE_IDLE, ///< Valve state idle. VALVE_STATE_IN_TRANSITION, ///< Valve state in transition. NUM_OF_VALVE_STATES, ///< Number of valve exec states. @@ -78,9 +79,9 @@ // Homing variables BOOL hasHomingBeenRequested; ///< Valve homing request flag. BOOL hasValveBeenHomed; ///< Valve homing completed flag. - BOOL hasHomingFailed; ///< Valve homing failed flag - S16 proposedEnergizedPos; ///< Valve homing proposed energized position. BOOL hasValveBeenReset; ///< Valve homing has valve been reset. + S16 currentMaxEncPositionCnt; ///< Valve homing current maximum encoder position counts. + S16 previousMaxEncPositionCnt; ///< Valve homing previous maximum encoder position counts. } VALVE_STATUS_T; /// Payload record structure for pinch valve set position request @@ -114,9 +115,12 @@ 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 ); static VALVE_STATE_T handleValvesIdleState( VALVE_T valve ); static VALVE_STATE_T handleValvesTransitionState( VALVE_T valve ); static void checkValveInRange( VALVE_T valve, SW_FAULT_ID_T SWFault ); +static BOOL hasEdgeBeenReached( VALVE_T valve ); +static S16 convertCountToDivision8( S16 count ); static void publishValvesData( void ); /*********************************************************************//** @@ -263,9 +267,12 @@ for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { - // update valve encoder positions + // Update valve encoder positions currentValveStates[ valve ].priorEncPosition = currentValveStates[ valve ].currentEncPosition; currentValveStates[ valve ].currentEncPosition = getValveEncoderPosition( valve ); + // Update valve max encoder positions + currentValveStates[ valve ].previousMaxEncPositionCnt = currentValveStates[ valve ].currentMaxEncPositionCnt; + currentValveStates[ valve ].currentMaxEncPositionCnt = getValveMaximumEncoderPosition( valve ); // execute valve state machine switch ( currentValveStates[ valve ].controlState ) @@ -294,6 +301,10 @@ currentValveStates[ valve ].controlState = handleValvesFindEnergizedEdgeState( valve ); break; + case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: + currentValveStates[ valve ].controlState = handleValvesFindDeenergizedEdgeState( valve ); + break; + case VALVE_STATE_IDLE: currentValveStates[ valve ].controlState = handleValvesIdleState( valve ); break; @@ -407,13 +418,7 @@ } else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, ZERO_ENC_DEBOUNCE_TIMEOUT_MS ) ) { - // Getting ready for homing enable the homing bit upon transitioning to homing not started - setValveHomingEnableDisable( valve, TRUE ); - // 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 ] = POS_C_FROM_ZERO_CNT; - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_D_PARTIAL_CLOSE ] = POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT; - nextState = VALVE_STATE_HOMING_NOT_STARTED; + nextState = VALVE_STATE_HOMING_NOT_STARTED; } return nextState; @@ -436,34 +441,27 @@ if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - currentValveStates[ valve ].proposedEnergizedPos = ROTARY_VALVE_INIT_FULL_SWING_COUNTS; + // Getting ready for homing enable the homing bit upon transitioning to homing not started + setValveHomingEnableDisable( valve, TRUE ); - // TODO remove this? What is the criteria for the homing failure? - if ( ( currentValveStates[ valve ].proposedEnergizedPos <= ROTARY_VALVE_ENERGIZED_EDGE_MIN_COUNTS ) || - ( currentValveStates[ valve ].proposedEnergizedPos >= ROTARY_VALVE_ENERGIZED_EDGE_MAX_COUNTS ) ) - { - currentValveStates[ valve ].hasHomingFailed = TRUE; - currentValveStates[ valve ].hasHomingBeenRequested = FALSE; - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_HOMING_FAILED, (U32)valve, (U32)currentValveStates[ valve ].proposedEnergizedPos ) - } - else - { - // Command valve to move to energized edge (end of travel) - currentValveStates[ valve ].valveOpsStartTime = 0; - currentValveStates[ valve ].hasValveBeenHomed = FALSE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; - setValveCmdChangePosition( valve, (U16)currentValveStates[ valve ].proposedEnergizedPos, MOTOR_DIR_FORWARD ); - } + // TODO figure out the new valve homing failure alarm + // SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_HOMING_FAILED, (U32)valve, (U32)currentValveStates[ valve ].proposedEnergizedPos ) + + // Command valve to move to energized edge (end of travel) + currentValveStates[ valve ].valveOpsStartTime = 0; + currentValveStates[ valve ].hasValveBeenHomed = FALSE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + setValveCmdChangePosition( valve, (U16)ROTARY_VALVE_INIT_ENERGIZED_COUNTS, MOTOR_DIR_FORWARD ); } return nextState; } /*********************************************************************//** * @brief - * The handleValvesFindEnergizedEdgeState function handles the Find Energized - * Edge state of the state machine for a given valve. + * The handleValvesFindEnergizedEdgeState function handles the find energized + * edge state of the state machine for a given valve. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[] * @param valve ID of valve for which to handle the Find Energized Edge state @@ -472,53 +470,79 @@ static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; - S16 currPos = currentValveStates[ valve ].currentEncPosition; - BOOL hasEdgeBeenReached = FALSE; checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_FIND_ENERGIZED_EDGE ); - // have we found forward edge of travel? - if ( abs( currPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) + // TODO remove + //S16 currPos = currentValveStates[ valve ].currentEncPosition; + //BOOL hasEdgeBeenReached = FALSE; + //// have we found forward edge of travel? + //if ( abs( currPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) + //{ + // if ( 0 == currentValveStates[ valve ].valveOpsStartTime ) + // { + // currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + // } + // else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) + // { + // hasEdgeBeenReached = TRUE; + // currPos -= VALVE_OFFEST_FROM_EDG_CNT; + // } + //} + //else if ( currPos <= 100 ) + //{ + // // Lost the shaft. Get the maximum value in the special register as the maximum. + // S16 maxEncPosition = getValveMaximumEncoderPosition( valve ); + // + // hasEdgeBeenReached = TRUE; + // currPos = maxEncPosition - VALVE_OFFEST_FROM_EDG_CNT; + //} + + if ( TRUE == hasEdgeBeenReached( valve ) ) { - if ( 0 == currentValveStates[ valve ].valveOpsStartTime ) - { - currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); - } - else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) - { - hasEdgeBeenReached = TRUE; - } + nextState = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } - else if ( currPos <= 0 ) - { - // Lost the shaft. Get the maximum value in the special register as the maximum. - S16 maxEncPosition = getValveMaximumEncoderPosition( valve ); - hasEdgeBeenReached = TRUE; - currPos = maxEncPosition - VALVE_OFFEST_FROM_EDG_CNT; - } + return nextState; +} - if ( TRUE == hasEdgeBeenReached ) - { - // Done with homing disable the homing bit - setValveHomingEnableDisable( valve, FALSE ); +/*********************************************************************//** + * @brief + * The handleValvesFindDeenergizedEdgeState function handles the find Deenergized + * edge state of the state machine for a given valve. + * @details \b Inputs: currentValveStates[] + * @details \b Outputs: currentValveStates[] + * @param valve ID of valve for which to handle the find deenergized edge state + * @return next state of the state machine for the given valve + *************************************************************************/ +static VALVE_STATE_T handleValvesFindDeenergizedEdgeState( VALVE_T valve ) +{ + VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; - S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - S16 posA = posC + ( ( currPos - posC ) / 2 ); + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_FIND_DEENERGIZED_EDGE ); - // 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; + // Done with homing disable the homing bit + setValveHomingEnableDisable( valve, FALSE ); - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = currPos; - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posA; - currentValveStates[ valve ].hasValveBeenHomed = TRUE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_B_OPEN; - currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; - currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; - currentValveStates[ valve ].hasHomingBeenRequested = FALSE; - nextState = VALVE_STATE_IDLE; - } + // TODO in the future this needs to be searched + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = VALVE_OFFEST_FROM_EDG_CNT; + // TODO in the future this needs to be searched + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_D_PARTIAL_CLOSE ] = POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT; + S16 posB = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ]; + S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + S16 posA = posC + ( ( posB - posC ) / 2 ); + + posA = convertCountToDivision8( posA ); + + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posA; + currentValveStates[ valve ].hasValveBeenHomed = TRUE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_B_OPEN; + currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; + currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; + currentValveStates[ valve ].hasHomingBeenRequested = FALSE; + nextState = VALVE_STATE_IDLE; + return nextState; } @@ -656,6 +680,70 @@ /*********************************************************************//** * @brief + * The hasEdgeBeenReached function checks whether the valve has reached to + * its edge or not. + * @details \b Inputs: currentValveStates[] + * @details \b Outputs: currentValveStates[] + * @param valve ID of valve for which to check its edge + * @return TRUE if the edge has been reached otherwise, FALSE + *************************************************************************/ +static BOOL hasEdgeBeenReached( VALVE_T valve ) +{ + BOOL status = FALSE; + + checkValveInRange( valve, SW_FAULT_ID_VALVES_INVALID_VALVE_FINDING_EDGE ); + + S16 previousMaxEnc = currentValveStates[ valve ].previousMaxEncPositionCnt; + S16 currentMaxEnc = currentValveStates[ valve ].currentMaxEncPositionCnt; + + if ( abs( currentMaxEnc - previousMaxEnc ) < VALVE_HOME_MIN_POS_CHG ) + { + if ( 0 == currentValveStates[ valve ].valveOpsStartTime ) + { + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + } + else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) + { + status = TRUE; + currentMaxEnc = convertCountToDivision8( currentMaxEnc ); + currentMaxEnc -= VALVE_OFFEST_FROM_EDG_CNT; + + currentValveStates[ valve ].valveOpsStartTime = 0; + + if ( VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE == currentValveStates[ valve ].controlState ) + { + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = currentMaxEnc; + } + // TODO uncomment the deenergized edge + //else if ( VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE == currentValveStates[ valve ].controlStat ) + //{ + // currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = currentMaxEnc + //} + } + } + + return status; +} + +/*********************************************************************//** + * @brief + * The convertCountToDivision8 function converts the provided data to be in + * division of 8. + * @details \b Inputs: none + * @details \b Outputs: none + * @param count the value to be converted to the division of 8 + * @return the converted value + *************************************************************************/ +static S16 convertCountToDivision8( S16 count ) +{ + // If position A was calculated to not be a division of 8 it is rounded down to be a division of 8 + S16 convCount = ( count % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( count >> 3 ) << 3 : count; + + return convCount; +} + +/*********************************************************************//** + * @brief * The publishValvesData function constructs and sends the valves data * broadcast message. * @details \b Message \b Sent: MSG_ID_TD_VALVES_DATA @@ -682,6 +770,7 @@ 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 ]; + data.maxHomingPos = currentValveStates[ valve ].currentMaxEncPositionCnt; broadcastData( MSG_ID_TD_VALVES_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&data, sizeof( TD_VALVE_DATA_T ) ); } Index: firmware/App/Controllers/Valves.h =================================================================== diff -u -rbe1f6ba8f58abfe098865c85ebca070eb0dde6ce -reef24091316f7c4b5717e72fef7ca06e58fc174a --- firmware/App/Controllers/Valves.h (.../Valves.h) (revision be1f6ba8f58abfe098865c85ebca070eb0dde6ce) +++ firmware/App/Controllers/Valves.h (.../Valves.h) (revision eef24091316f7c4b5717e72fef7ca06e58fc174a) @@ -57,6 +57,7 @@ S16 posB; ///< Position B (count) S16 posC; ///< Position C (count) S16 posD; ///< Position D (count) + S16 maxHomingPos; ///< Maximum homing position } TD_VALVE_DATA_T; #pragma pack(pop) Index: firmware/App/Drivers/EjectorMotor.c =================================================================== diff -u -rfaf64f3fb669c2a86a9d96bd51669cfbd731f6ba -reef24091316f7c4b5717e72fef7ca06e58fc174a --- firmware/App/Drivers/EjectorMotor.c (.../EjectorMotor.c) (revision faf64f3fb669c2a86a9d96bd51669cfbd731f6ba) +++ firmware/App/Drivers/EjectorMotor.c (.../EjectorMotor.c) (revision eef24091316f7c4b5717e72fef7ca06e58fc174a) @@ -66,7 +66,6 @@ EJECTOR_MOTOR_CONTROL_32TH_STEP; /// Control bits to disable ejector motor -// Set all bits to 0 for disabling static const U08 EJECTOR_MOTOR_CONTROL_DISABLED = 0; /// Payload record structure for ejector motor set request Index: firmware/App/Drivers/RotaryValve.c =================================================================== diff -u -r1500c2a584e9e9ce0ffcd32727c0b9e7cdd141b6 -reef24091316f7c4b5717e72fef7ca06e58fc174a --- firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision 1500c2a584e9e9ce0ffcd32727c0b9e7cdd141b6) +++ firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision eef24091316f7c4b5717e72fef7ca06e58fc174a) @@ -133,6 +133,7 @@ { if ( valve < NUM_OF_VALVES ) { + // TODO this line is not needed because the positions are calculated in the homing U16 mag8 = ( ( mag % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( mag >> 3 ) << 3 : ( ( mag >> 3 ) << 3 ) + ROTARY_VALVE_MICROSTEP_FRACTION ); // round to multiple of 8 to maximize holding torque S16 chgSteps = (S16)mag8 * ( MOTOR_DIR_REVERSE == dir ? -1 : 1 ); // convert to signed change in position - negative value indicates reverse direction Index: firmware/App/Drivers/RotaryValve.h =================================================================== diff -u -r5eda216f634a74512a5bc11b0984676567512d80 -reef24091316f7c4b5717e72fef7ca06e58fc174a --- firmware/App/Drivers/RotaryValve.h (.../RotaryValve.h) (revision 5eda216f634a74512a5bc11b0984676567512d80) +++ firmware/App/Drivers/RotaryValve.h (.../RotaryValve.h) (revision eef24091316f7c4b5717e72fef7ca06e58fc174a) @@ -35,9 +35,8 @@ #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_INIT_FULL_SWING_COUNTS 1880 ///< Rotary valve swing arm travel range is appx. micro steps. -#define ROTARY_VALVE_ENERGIZED_EDGE_MAX_COUNTS 1900 ///< Rotary valve energized edge maximum allowed counts. -#define ROTARY_VALVE_ENERGIZED_EDGE_MIN_COUNTS 1700 ///< Rotary valve energized edge minimum allowed counts. +#define ROTARY_VALVE_INIT_ENERGIZED_COUNTS 2000 ///< Rotary valve target energized edge in counts. +#define ROTARY_VALVE_INIT_DEENERGIZED_COUNTS -200 ///< Rotary valve target de-energized edge in counts. /// rotary pinch valve names typedef enum valveNames Index: firmware/App/Services/AlarmMgmtSWFaults.h =================================================================== diff -u -r5eda216f634a74512a5bc11b0984676567512d80 -reef24091316f7c4b5717e72fef7ca06e58fc174a --- firmware/App/Services/AlarmMgmtSWFaults.h (.../AlarmMgmtSWFaults.h) (revision 5eda216f634a74512a5bc11b0984676567512d80) +++ firmware/App/Services/AlarmMgmtSWFaults.h (.../AlarmMgmtSWFaults.h) (revision eef24091316f7c4b5717e72fef7ca06e58fc174a) @@ -179,6 +179,8 @@ SW_FAULT_ID_TD_EJECTOR_OPTICAL_SENSOR_INVALID_REQ = 148, SW_FAULT_ID_VALVES_INVALID_VALVE_HOMING_SET = 149, SW_FAULT_ID_VALVES_INVALID_VALVE_MAX_ENC_POS_REQ = 150, + SW_FAULT_ID_VALVES_INVALID_VALVE_FINDING_EDGE = 151, + SW_FAULT_ID_TD_VALVES_INVALID_FIND_DEENERGIZED_EDGE = 152, NUM_OF_SW_FAULT_IDS } SW_FAULT_ID_T;