Index: firmware/App/Controllers/Ejector.c =================================================================== diff -u -rfdb7ee915da2741dc200fef1b624b7f383f4db85 -r9a8e1fc597eb4f681527e691fb6cb24e550d8b92 --- firmware/App/Controllers/Ejector.c (.../Ejector.c) (revision fdb7ee915da2741dc200fef1b624b7f383f4db85) +++ firmware/App/Controllers/Ejector.c (.../Ejector.c) (revision 9a8e1fc597eb4f681527e691fb6cb24e550d8b92) @@ -31,13 +31,13 @@ // ********** private definitions ********** #define EJECTOR_RETRACT_OP_TIME ( ( MS_PER_SECOND * 5 ) / TASK_GENERAL_INTERVAL ) ///< Ejector retract operation interval. -#define EJECTOR_EXTEND_OP_TIME ( ( MS_PER_SECOND * 4 ) / TASK_GENERAL_INTERVAL ) ///< Ejector extend operation interval. +#define EJECTOR_EXTEND_OP_TIME ( ( MS_PER_SECOND * 10 ) / TASK_GENERAL_INTERVAL ) ///< Ejector extend operation interval. #define EJECTOR_BACKOFF_OP_TIME ( 50 / TASK_GENERAL_INTERVAL ) ///< Ejector back-off operation interval. #define EJECTOR_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Ejector data publish interval. #define DATA_PUBLISH_COUNTER_START_COUNT 13 ///< Ejector data publish start counter. #define EJECTOR_RETRACT_MOTOR_SPEED_RPM -350.0F ///< Ejector motor retract speed (in RPM). -#define EJECTOR_EXTEND_MOTOR_SPEED_RPM 350.0F ///< Ejector motor extend speed (in RPM). +#define EJECTOR_EXTEND_MOTOR_SPEED_RPM 162.5F ///< Ejector motor extend speed (in RPM). #define EJECTOR_OFF_MOTOR_SPEED_RPM 0.0F ///< Ejector motor extend speed (in RPM). /// Enumeration of ejector states. @@ -60,17 +60,20 @@ static BOOL ejectorHomeRequested; ///< Flag indicating that ejector home operation is requested. static BOOL ejectorRetractRequested; ///< Flag indicating that ejector retract operation is requested. static BOOL ejectorExtendRequested; ///< Flag indicating that ejector extend operation is requested. +static BOOL ejectorAbortRequested; ///< Flag indicating that ejector abort operation in requested. // ********** private function prototypes ********** static EJECTOR_STATE_T handleEjectorStartState( void ); static EJECTOR_STATE_T handleEjectorHomingState( void ); -static EJECTOR_STATE_T handleEjectorRetractedState ( void ); -static EJECTOR_STATE_T handleEjectorExtendedState ( void ); -static EJECTOR_STATE_T handleEjectorRetractingState ( void ); -static EJECTOR_STATE_T handleEjectorExtendingState ( void ); +static EJECTOR_STATE_T handleEjectorRetractedState( void ); +static EJECTOR_STATE_T handleEjectorExtendedState( void ); +static EJECTOR_STATE_T handleEjectorRetractingState( void ); +static EJECTOR_STATE_T handleEjectorExtendingState( void ); static EJECTOR_STATE_T handleEjectorRetractBackoffState( void ); static EJECTOR_STATE_T handleEjectorDirChangeStopState( void ); +static EJECTOR_STATE_T handleEjectorAbortState( void ); +static void setEjectorSpeed( F32 speed ); static void resetEjectorFlags( void ); static void publishEjectorData( void ); @@ -103,18 +106,34 @@ * The resetEjectorFlags function resets the ejector flags to FALSE. * @details \b Inputs: none * @details \b Outputs: ejectorHomeRequested, ejectorRetractRequested, - * ejectorExtendRequested + * ejectorExtendRequested, ejectorAbortRequested * @return none *************************************************************************/ static void resetEjectorFlags( void ) { ejectorHomeRequested = FALSE; ejectorRetractRequested = FALSE; ejectorExtendRequested = FALSE; + ejectorAbortRequested = FALSE; } /*********************************************************************//** * @brief + * The setEjectorSpeed function records the requested ejector speed and + * sets the ejector motor speed to that speed. + * @details \b Inputs: none + * @details \b Outputs: currentEjectorSetSpeed + * @param speed Target speed for ejector motor. + * @return none + *************************************************************************/ +static void setEjectorSpeed( F32 speed ) +{ + currentEjectorSetSpeed = speed; + setEjectorMotorSpeed( currentEjectorSetSpeed ); +} + +/*********************************************************************//** + * @brief * The homeEjector function requests an ejector home operation. * @details \b Inputs: currentEjectorState * @details \b Outputs: ejectorHomeRequested @@ -129,6 +148,10 @@ ejectorHomeRequested = TRUE; result = TRUE; } + else + { + result = retractEjector(); + } return result; } @@ -175,6 +198,31 @@ /*********************************************************************//** * @brief + * The abortEjectorOperation function requests an ejector abort operation. + * @details \b Inputs: currentEjectorState + * @details \b Outputs: ejectorAbortRequested + * @return none + *************************************************************************/ +void abortEjectorOperation( void ) +{ + switch ( currentEjectorState ) + { + case EJECTOR_STATE_HOMING: + case EJECTOR_STATE_RETRACTING: + case EJECTOR_STATE_EXTENDING: + setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); + // Set the flag to TRUE regardless of where the ejector is at + ejectorAbortRequested = TRUE; + break; + + default: + // Do nothing on the rest of the states. Only initiate the abort sequence if an operation is in progress. + break; + } +} + +/*********************************************************************//** + * @brief * The getEjectorState function returns the current ejector control state machine. * @details \b Inputs: currentEjectorState * @details \b Outputs: none @@ -195,6 +243,12 @@ *************************************************************************/ void execEjectorController( void ) { + if ( TRUE == ejectorAbortRequested ) + { + // If abort has been called, set it to abort state no matter where we are at. + currentEjectorState = EJECTOR_STATE_ABORT; + } + switch( currentEjectorState ) { case EJECTOR_STATE_INIT: @@ -229,6 +283,10 @@ currentEjectorState = handleEjectorDirChangeStopState(); break; + case EJECTOR_STATE_ABORT: + currentEjectorState = handleEjectorAbortState(); + break; + default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_EJECTOR_INVALID_STATE, (U32)currentEjectorState ) break; @@ -255,9 +313,13 @@ if ( TRUE == ejectorHomeRequested ) { ejectorOperationTimerCounter = 0; - setEjectorMotorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM ); + setEjectorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_HOMING; } + else + { + disableEjectorMotor(); + } return state; } @@ -277,7 +339,7 @@ if ( ++ejectorOperationTimerCounter >= EJECTOR_RETRACT_OP_TIME ) { ejectorOperationTimerCounter = 0; - setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); + setEjectorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_DIR_CHANGE_STOP; } @@ -299,9 +361,13 @@ if ( TRUE == ejectorExtendRequested ) { ejectorOperationTimerCounter = 0; - setEjectorMotorSpeed( EJECTOR_EXTEND_MOTOR_SPEED_RPM ); + setEjectorSpeed( EJECTOR_EXTEND_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_EXTENDING; } + else + { + disableEjectorMotor(); + } return state; } @@ -321,9 +387,13 @@ if ( TRUE == ejectorRetractRequested ) { ejectorOperationTimerCounter = 0; - setEjectorMotorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM ); + setEjectorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_RETRACTING; } + else + { + disableEjectorMotor(); + } return state; } @@ -343,7 +413,7 @@ if ( ++ejectorOperationTimerCounter >= EJECTOR_RETRACT_OP_TIME ) { ejectorOperationTimerCounter = 0; - setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); + setEjectorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_DIR_CHANGE_STOP; } @@ -365,7 +435,7 @@ if ( ++ejectorOperationTimerCounter >= EJECTOR_BACKOFF_OP_TIME ) { ejectorOperationTimerCounter = 0; - setEjectorMotorSpeed( EJECTOR_EXTEND_MOTOR_SPEED_RPM ); + setEjectorSpeed( EJECTOR_EXTEND_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_RETRACT_BACKOFF; } @@ -387,7 +457,7 @@ if ( ++ejectorOperationTimerCounter >= EJECTOR_BACKOFF_OP_TIME ) { ejectorOperationTimerCounter = 0; - setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); + setEjectorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_RETRACTED; } @@ -406,10 +476,10 @@ { EJECTOR_STATE_T state = EJECTOR_STATE_EXTENDING; - if ( ++ejectorOperationTimerCounter >= EJECTOR_RETRACT_OP_TIME ) + if ( ++ejectorOperationTimerCounter >= EJECTOR_EXTEND_OP_TIME ) { ejectorOperationTimerCounter = 0; - setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); + setEjectorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_EXTENDED; } @@ -418,6 +488,28 @@ /*********************************************************************//** * @brief + * The handleEjectorAbortState function handles the abort state of the + * ejector control state machine. + * @details \b Inputs: ejectorOperationTimerCounter, ejectorHomeRequested + * @details \b Outputs: ejectorOperationTimerCounter + * @return next state of the ejector control state machine + *************************************************************************/ +static EJECTOR_STATE_T handleEjectorAbortState( void ) +{ + EJECTOR_STATE_T state = EJECTOR_STATE_ABORT; + + if ( TRUE == ejectorHomeRequested ) + { + ejectorOperationTimerCounter = 0; + setEjectorMotorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM ); + state = EJECTOR_STATE_HOMING; + } + + return state; +} + +/*********************************************************************//** + * @brief * The publishEjectorData function constructs and sends the air pump data * broadcast message. * @details \b Message \b Sent: MSG_ID_TD_EJECTOR_DATA