statemachine::ReversingState Class Reference

The ReversingState class implements the Reversing state for the Autonomy State Machine. More...

#include <ReversingState.h>

Inheritance diagram for statemachine::ReversingState:

Collaboration diagram for statemachine::ReversingState:

Public Member Functions
	ReversingState ()
	Construct a new State object.
void	Run () override
	Run the state machine. Returns the next state.
States	TriggerEvent (Event eEvent) override
	Trigger an event in the state machine. Returns the next state.
Public Member Functions inherited from statemachine::State
	State (States eState)
	Construct a new State object.
virtual	~State ()=default
	Destroy the State object.
States	GetState () const
	Accessor for the State private member.
virtual std::string	ToString () const
	Accessor for the State private member. Returns the state as a string.
virtual bool	operator== (const State &other) const
	Checks to see if the current state is equal to the passed state.
virtual bool	operator!= (const State &other) const
	Checks to see if the current state is not equal to the passed state.

Protected Member Functions
void	Start () override
	This method is called when the state is first started. It is used to initialize the state.
void	Exit () override
	This method is called when the state is exited. It is used to clean up the state.

Private Attributes
geoops::RoverPose	m_stStartRoverPose
std::chrono::system_clock::time_point	m_tmStartReversingTime
std::chrono::system_clock::time_point	m_tmTimeSinceLastMeter
bool	m_bInitialized

Detailed Description

The ReversingState class implements the Reversing state for the Autonomy State Machine.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Constructor & Destructor Documentation

ReversingState()

statemachine::ReversingState::ReversingState

(

)

Construct a new State object.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

                                   : State(States::eReversing)
    {
        // Submit logger message.
        LOG_INFO(logging::g_qConsoleLogger, "Entering State: {}", ToString());
 
        m_bInitialized = false;
 
        if (!m_bInitialized)
        {
            Start();
            m_bInitialized = true;
        }
    }

Here is the call graph for this function:

Member Function Documentation

Start()

void statemachine::ReversingState::Start ( )

overrideprotectedvirtual

This method is called when the state is first started. It is used to initialize the state.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Reimplemented from statemachine::State.

    {
        // Schedule the next run of the state's logic
        LOG_INFO(logging::g_qSharedLogger, "ReversingState: Scheduling next run of state logic.");
 
        // Store the starting position and heading of rover when it entered this state.
        m_stStartRoverPose = globals::g_pWaypointHandler->SmartRetrieveRoverPose();
 
        // Store state start time.
        m_tmStartReversingTime = std::chrono::high_resolution_clock::now();
        m_tmTimeSinceLastMeter = m_tmStartReversingTime;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Exit()

void statemachine::ReversingState::Exit ( )

overrideprotectedvirtual

This method is called when the state is exited. It is used to clean up the state.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Reimplemented from statemachine::State.

    {
        // Clean up the state before exiting
        LOG_INFO(logging::g_qSharedLogger, "ReversingState: Exiting state.");
    }

Here is the caller graph for this function:

Run()

void statemachine::ReversingState::Run ( )

overridevirtual

Run the state machine. Returns the next state.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Implements statemachine::State.

    {
        // Submit logger message.
        LOG_DEBUG(logging::g_qSharedLogger, "ReversingState: Running state-specific behavior.");
 
        // Create instance variables.
        static bool bTimeSinceLastMeterAlreadySet = false;
 
        // Get current position and heading.
        geoops::RoverPose stCurrentRoverPose = globals::g_pWaypointHandler->SmartRetrieveRoverPose();
        // Get the current time.
        std::chrono::system_clock::time_point tmCurrentTime = std::chrono::high_resolution_clock::now();
        // Calculate current distance from start point.
        geoops::GeoMeasurement stMeasurement = geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetGPSCoordinate(), m_stStartRoverPose.GetGPSCoordinate());
 
        // Calculate time elapsed.
        double dTotalTimeElapsed          = std::chrono::duration_cast<std::chrono::seconds>(tmCurrentTime - m_tmStartReversingTime).count();
        double dTimeElapsedSinceLastMeter = std::chrono::duration_cast<std::chrono::seconds>(tmCurrentTime - m_tmTimeSinceLastMeter).count();
        // Check if rover has reversed the desired distance or timeout has been reached.
        if (stMeasurement.dDistanceMeters >= constants::REVERSE_DISTANCE)
        {
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger, "ReversingState: Successfully reversed {} meters in {} seconds.", stMeasurement.dDistanceMeters, dTotalTimeElapsed);
            // Stop reversing.
            globals::g_pDriveBoard->SendStop();
            // Handle reversing complete event.
            globals::g_pStateMachineHandler->HandleEvent(Event::eReverseComplete);
            // Exit method without running other code below.
            return;
        }
        // Check if we aren't covering enough distance within the timeout limit.
        else if (dTimeElapsedSinceLastMeter >= constants::REVERSE_TIMEOUT_PER_METER)
        {
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger,
                       "ReversingState: Reversed {} meters in {} seconds before timeout was reached. Goal was {} meters, so rover must be running into something...",
                       stMeasurement.dDistanceMeters,
                       dTotalTimeElapsed,
                       constants::REVERSE_DISTANCE);
            // Stop reversing.
            globals::g_pDriveBoard->SendStop();
            // Handle reversing complete event.
            globals::g_pStateMachineHandler->HandleEvent(Event::eStuck);
            // Exit method without running other code below.
            return;
        }
        // Reset the timestamp since last meter every other meter reversed.
        else if (int(stMeasurement.dDistanceMeters) % 2 == 0 && !bTimeSinceLastMeterAlreadySet)
        {
            // Update timestamp.
            m_tmTimeSinceLastMeter = std::chrono::high_resolution_clock::now();
            // Set toggle.
            bTimeSinceLastMeterAlreadySet = true;
        }
        else if (int(stMeasurement.dDistanceMeters) % 2 != 0 && bTimeSinceLastMeterAlreadySet)
        {
            // Reset toggle.
            bTimeSinceLastMeterAlreadySet = false;
        }
 
        // Check if we should try to maintain heading while reversing.
        if (constants::REVERSE_MAINTAIN_HEADING)
        {
            // Reverse straight backwards.
            diffdrive::DrivePowers stReverse = globals::g_pDriveBoard->CalculateMove(-std::fabs(constants::REVERSE_MOTOR_POWER),
                                                                                     m_stStartRoverPose.GetCompassHeading(),
                                                                                     stCurrentRoverPose.GetCompassHeading(),
                                                                                     diffdrive::DifferentialControlMethod::eArcadeDrive);
            // Send drive powers.
            globals::g_pDriveBoard->SendDrive(stReverse);
        }
        else
        {
            // Just set reverse drive powers manually.
            diffdrive::DrivePowers stMotorPowers{-std::fabs(constants::REVERSE_MOTOR_POWER), -std::fabs(constants::REVERSE_MOTOR_POWER)};
            // Send drive powers.
            globals::g_pDriveBoard->SendDrive(stMotorPowers);
        }
    }

Here is the call graph for this function:

TriggerEvent()

States statemachine::ReversingState::TriggerEvent ( Event  eEvent )

overridevirtual

Trigger an event in the state machine. Returns the next state.

Parameters

eEvent

- The event to trigger.

Returns

std::shared_ptr<State> - The next state.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-01-17

Implements statemachine::State.

    {
        // Initialize member variables.
        States eNextState       = States::eReversing;
        bool bCompleteStateExit = true;
 
        switch (eEvent)
        {
            case Event::eStart:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ReversingState: Handling Start event.");
                break;
            }
            case Event::eAbort:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ReversingState: Handling Abort event.");
                // Change states.
                eNextState = States::eIdle;
                break;
            }
            case Event::eReverseComplete:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ReversingState: Handling ReverseComplete event.");
                // Transition back to state that triggered reversing.
                eNextState = globals::g_pStateMachineHandler->GetPreviousState();
                break;
            }
            case Event::eStuck:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ReversingState: Handling Stuck event.");
                // Change states.
                eNextState = States::eStuck;
                break;
            }
            default:
            {
                // Submit logger message.
                LOG_INFO(logging::g_qSharedLogger, "ReversingState: Handling unknown event.");
                // Change states.
                eNextState = States::eIdle;
                break;
            }
        }
 
        if (eNextState != States::eReversing)
        {
            LOG_INFO(logging::g_qSharedLogger, "ReversingState: Transitioning to {} State.", StateToString(eNextState));
 
            // Exit the current state
            if (bCompleteStateExit)
            {
                Exit();
            }
        }
 
        return eNextState;
    }

Here is the call graph for this function:

---

The documentation for this class was generated from the following files:

• src/states/ReversingState.h
• src/states/ReversingState.cpp