Skip to content

Commit

Permalink
Merge pull request #272 from MissouriMRDT/topic/post-competition-cleanup
Browse files Browse the repository at this point in the history 
Cleanup and fix compile warning.
  • Loading branch information
@Byrdman32
Byrdman32 authored Aug 15, 2024
2 parents 1c71468 + bc26cad commit 5fd4f26
Show file tree
Hide file tree
Showing 10 changed files with 105 additions and 89 deletions.
27 changes: 22 additions & 5 deletions src/drivers/MultimediaBoard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@ void MultimediaBoard::SendLightingState(MultimediaBoardLightingState eState)
switch (eState)
{
case eOff:
{
// Construct a RoveComm packet with the lighting data.
stPacket.unDataId = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_ID;
stPacket.unDataCount = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_COUNT;
Expand All @@ -73,40 +74,55 @@ void MultimediaBoard::SendLightingState(MultimediaBoardLightingState eState)
stPacket.vData.emplace_back(0);
stPacket.vData.emplace_back(0);
break;

}
case eCustom:
{
// Use RoveComm to send old custom values previously set.
this->SendRGB(m_stCustomRGBValues);
break;

}
case eTeleOp:
{
// Construct a RoveComm packet with the lighting data.
stPacket.unDataId = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_ID;
stPacket.unDataCount = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_COUNT;
stPacket.eDataType = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_TYPE;
// Use RoveComm to send BLUE color state value.
stPacket.vData.emplace_back(static_cast<uint8_t>(manifest::Core::DISPLAYSTATE::TELEOP));
break;

}
case eAutonomy:
{
// Construct a RoveComm packet with the lighting data.
stPacket.unDataId = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_ID;
stPacket.unDataCount = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_COUNT;
stPacket.eDataType = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_TYPE;
// Use RoveComm to send RED color state value.
stPacket.vData.emplace_back(static_cast<uint8_t>(manifest::Core::DISPLAYSTATE::AUTONOMY));
break;

}
case eReachedGoal:
{
// Send Reached Goal state over RoveComm.
// Construct a RoveComm packet.
rovecomm::RoveCommPacket<uint8_t> stPacket;
stPacket.unDataId = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_ID;
stPacket.unDataCount = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_COUNT;
stPacket.eDataType = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_TYPE;
stPacket.vData.emplace_back(1);
// Send telemetry over RoveComm to all subscribers.
network::g_pRoveCommUDPNode->SendUDPPacket(stPacket, "0.0.0.0", constants::ROVECOMM_OUTGOING_UDP_PORT);

// Construct a RoveComm packet with the lighting data.
stPacket.unDataId = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_ID;
stPacket.unDataCount = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_COUNT;
stPacket.eDataType = manifest::Core::COMMANDS.find("STATEDISPLAY")->second.DATA_TYPE;
// Use RoveComm to send flashing GREEN color state value.
stPacket.vData.emplace_back(static_cast<uint8_t>(manifest::Core::DISPLAYSTATE::REACHED_GOAL));
break;

}
default:
{
// Construct a RoveComm packet with the lighting data.
stPacket.unDataId = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_ID;
stPacket.unDataCount = manifest::Core::COMMANDS.find("LEDRGB")->second.DATA_COUNT;
Expand All @@ -116,6 +132,7 @@ void MultimediaBoard::SendLightingState(MultimediaBoardLightingState eState)
stPacket.vData.emplace_back(0);
stPacket.vData.emplace_back(0);
break;
}
}

// Check if we should send packets to the SIM or board.
Expand Down
27 changes: 24 additions & 3 deletions src/drivers/NavigationBoard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,13 +126,17 @@ geoops::UTMCoordinate NavigationBoard::GetUTMData()
LOG_WARNING(logging::g_qSharedLogger, "Current GPS data is out of date! GPS timestamp is {} seconds old!", nGPSDataAge);
// Set toggle.
bAlreadyPrintedWarning = true;
// Set Out of Date.
m_bNavBoardOutOfDate = true;
}
else if (nGPSDataAge < constants::NAVBOARD_MAX_GPS_DATA_AGE && bAlreadyPrintedWarning)
{
// Submit logger message.
LOG_WARNING(logging::g_qSharedLogger, "GPS data recovered!");
// Reset toggle.
bAlreadyPrintedWarning = false;
// Reset Out of Date.
m_bNavBoardOutOfDate = false;
}

// Convert the currently stored GPS coord to UTM and return.
Expand Down Expand Up @@ -163,13 +167,17 @@ double NavigationBoard::GetHeading()
LOG_WARNING(logging::g_qSharedLogger, "Current Compass data is out of date! Compass timestamp is {} seconds old!", nCompassDataAge);
// Set toggle.
bAlreadyPrintedWarning = true;
// Set Out of Date.
m_bNavBoardOutOfDate = true;
}
else if (nCompassDataAge < constants::NAVBOARD_MAX_COMPASS_DATA_AGE && bAlreadyPrintedWarning)
{
// Submit logger message.
LOG_WARNING(logging::g_qSharedLogger, "Compass data recovered!");
// Reset toggle.
bAlreadyPrintedWarning = false;
// Reset Out of Date.
m_bNavBoardOutOfDate = false;
}

// Return current Compass data.
Expand Down Expand Up @@ -200,13 +208,17 @@ double NavigationBoard::GetHeadingAccuracy()
LOG_WARNING(logging::g_qSharedLogger, "Current Compass data is out of date! Compass timestamp is {} seconds old!", nCompassDataAge);
// Set toggle.
bAlreadyPrintedWarning = true;
// Set Out of Date.
m_bNavBoardOutOfDate = true;
}
else if (nCompassDataAge < constants::NAVBOARD_MAX_COMPASS_DATA_AGE && bAlreadyPrintedWarning)
{
// Submit logger message.
LOG_WARNING(logging::g_qSharedLogger, "Compass data recovered!");
// Reset toggle.
bAlreadyPrintedWarning = false;
// Reset Out of Date.
m_bNavBoardOutOfDate = false;
}

// Return current Compass data.
Expand Down Expand Up @@ -238,13 +250,17 @@ double NavigationBoard::GetVelocity()
LOG_WARNING(logging::g_qSharedLogger, "Current Velocity data is out of date! GPS timestamp is {} seconds old!", nGPSDataAge);
// Set toggle.
bAlreadyPrintedWarning = true;
// Set Out of Date.
m_bNavBoardOutOfDate = true;
}
else if (nGPSDataAge < constants::NAVBOARD_MAX_GPS_DATA_AGE && bAlreadyPrintedWarning)
{
// Submit logger message.
LOG_WARNING(logging::g_qSharedLogger, "GPS data recovered!");
// Reset toggle.
bAlreadyPrintedWarning = false;
// Reset Out of Date.
m_bNavBoardOutOfDate = false;
}

// Return current velocity.
Expand Down Expand Up @@ -276,13 +292,17 @@ double NavigationBoard::GetAngularVelocity()
LOG_WARNING(logging::g_qSharedLogger, "Current Angular Velocity data is out of date! Compass timestamp is {} seconds old!", nCompassDataAge);
// Set toggle.
bAlreadyPrintedWarning = true;
// Set Out of Date.
m_bNavBoardOutOfDate = true;
}
else if (nCompassDataAge < constants::NAVBOARD_MAX_COMPASS_DATA_AGE && bAlreadyPrintedWarning)
{
// Submit logger message.
LOG_WARNING(logging::g_qSharedLogger, "Compass data recovered!");
// Reset toggle.
bAlreadyPrintedWarning = false;
// Reset Out of Date.
m_bNavBoardOutOfDate = false;
}

// Return angular velocity.
Expand Down Expand Up @@ -328,10 +348,11 @@ std::chrono::system_clock::duration NavigationBoard::GetCompassLastUpdateTime()
}

/******************************************************************************
* @brief
* @brief Checks if any of the navboard data is out of date. Expired data depends
* on constants::NAVBOARD_MAX_COMPASS_DATA_AGE.
*
* @return true -
* @return false -
* @return true - The data is out of date.
* @return false - The data is new.
*
* @author Eli Byrd ([email protected])
* @date 2024-05-24
Expand Down
6 changes: 1 addition & 5 deletions src/handlers/StateMachineHandler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,7 @@ StateMachineHandler::StateMachineHandler()
// Set RoveComm Node callbacks.
network::g_pRoveCommUDPNode->AddUDPCallback<uint8_t>(AutonomyStartCallback, manifest::Autonomy::COMMANDS.find("STARTAUTONOMY")->second.DATA_ID);
network::g_pRoveCommUDPNode->AddUDPCallback<uint8_t>(AutonomyStopCallback, manifest::Autonomy::COMMANDS.find("DISABLEAUTONOMY")->second.DATA_ID);

if (constants::BATTERY_CHECKS_ENABLED)
{
network::g_pRoveCommUDPNode->AddUDPCallback<float>(PMSCellVoltageCallback, manifest::PMS::TELEMETRY.find("CELLVOLTAGE")->second.DATA_ID);
}
network::g_pRoveCommUDPNode->AddUDPCallback<float>(PMSCellVoltageCallback, manifest::PMS::TELEMETRY.find("CELLVOLTAGE")->second.DATA_ID);

// Initialize member variables.
m_pMainCam = globals::g_pCameraHandler->GetZED(CameraHandler::eHeadMainCam);
Expand Down
4 changes: 2 additions & 2 deletions src/handlers/StateMachineHandler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,8 +147,8 @@ class StateMachineHandler : private AutonomyThread<void>
LOG_DEBUG(logging::g_qSharedLogger, "Incoming Packet: PMS Cell Voltages. Average voltage is: {}", dAverageCellVoltage);

// Check if voltage is above the safe minimum for lithium ion batteries.
if (dAverageCellVoltage < constants::BATTERY_MINIMUM_CELL_VOLTAGE && this->GetCurrentState() != statemachine::States::eIdle &&
constants::BATTERY_CHECKS_ENABLED)
if (constants::BATTERY_CHECKS_ENABLED && dAverageCellVoltage < constants::BATTERY_MINIMUM_CELL_VOLTAGE &&
this->GetCurrentState() != statemachine::States::eIdle)
{
// Submit logger message.
LOG_CRITICAL(logging::g_qSharedLogger,
Expand Down
36 changes: 19 additions & 17 deletions src/states/ApproachingMarkerState.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,11 +196,23 @@ namespace statemachine
m_dLastTargetHeading = dTargetHeading;
m_dLastTargetDistance = dTargetDistance;

// TODO: Change to a Debug Statement after we confirm it works.
LOG_INFO(logging::g_qSharedLogger,
"ApproachingMarkerState: Rover is {} meters from the marker. Minimum Distance is {}.",
dTargetDistance,
constants::APPROACH_MARKER_PROXIMITY_THRESHOLD);
// Only print out every so often.
static bool bAlreadyPrinted = false;
if ((std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count() % 5) == 0 && !bAlreadyPrinted)
{
// Submit logger message.
LOG_INFO(logging::g_qSharedLogger,
"ApproachingMarkerState: Rover is {} meters from the marker. Minimum Distance is {}.",
dTargetDistance,
constants::APPROACH_MARKER_PROXIMITY_THRESHOLD);
// Set toggle.
bAlreadyPrinted = true;
}
else if (bAlreadyPrinted)
{
// Reset toggle.
bAlreadyPrinted = false;
}

// If we are close enough to the target inform the state machine we have reached the marker.
if (dTargetDistance < constants::APPROACH_MARKER_PROXIMITY_THRESHOLD)
Expand Down Expand Up @@ -238,21 +250,11 @@ namespace statemachine
{
case Event::eReachedMarker:
{
// Submit logger message.
LOG_INFO(logging::g_qSharedLogger, "ApproachingMarkerState: Handling ReachedMarker event.");

// Send multimedia command to update state display.
globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eReachedGoal);

// Send Reached Goal state over RoveComm.
// Construct a RoveComm packet.
rovecomm::RoveCommPacket<uint8_t> stPacket;
stPacket.unDataId = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_ID;
stPacket.unDataCount = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_COUNT;
stPacket.eDataType = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_TYPE;
stPacket.vData.emplace_back(1);
// Send telemetry over RoveComm to all subscribers.
network::g_pRoveCommUDPNode->SendUDPPacket(stPacket, "0.0.0.0", constants::ROVECOMM_OUTGOING_UDP_PORT);

// Change states.
eNextState = States::eIdle;
break;
}
Expand Down
8 changes: 0 additions & 8 deletions src/states/IdleState.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,14 +97,6 @@ namespace statemachine
// Store the Rover's position.
m_vRoverPosition.push_back(std::make_tuple(stCurrentRoverPose.GetUTMCoordinate().dEasting, stCurrentRoverPose.GetUTMCoordinate().dNorthing));

geoops::GPSCoordinate stCurrentGPSPosition = globals::g_pNavigationBoard->GetGPSData();

geoops::GeoMeasurement stErrorMeasurement = geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetGPSCoordinate(), stCurrentGPSPosition);
LOG_INFO(logging::g_qSharedLogger,
"Distance from Rover: {} and Bearing to Rover: {}",
stErrorMeasurement.dDistanceMeters,
stErrorMeasurement.dStartRelativeBearing);

// Calculate distance from current position to position when idle state was entered.
geoops::GeoMeasurement stMeasurement = geoops::CalculateGeoMeasurement(m_stStartRoverPose.GetGPSCoordinate(), stCurrentRoverPose.GetGPSCoordinate());
// Check if the rover is still moving.
Expand Down
42 changes: 28 additions & 14 deletions src/states/NavigatingState.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,24 +108,38 @@ namespace statemachine
// Check if we are at the goal waypoint. (only if we aren't waiting for a goal waypoint)
if (!m_bFetchNewWaypoint)
{
geoops::GPSCoordinate stCurrentGPSPosition = globals::g_pNavigationBoard->GetGPSData();

// Get Current rover pose.
geoops::RoverPose stCurrentRoverPose = globals::g_pWaypointHandler->SmartRetrieveRoverPose();

geoops::GeoMeasurement stErrorMeasurement = geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetGPSCoordinate(), stCurrentGPSPosition);

geoops::RoverPose stCurrentRoverPose = globals::g_pWaypointHandler->SmartRetrieveRoverPose();
// Calculate distance and bearing from goal waypoint.
geoops::GeoMeasurement stGoalWaypointMeasurement =
geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetUTMCoordinate(), m_stGoalWaypoint.GetUTMCoordinate());
LOG_INFO(logging::g_qSharedLogger,
"Distance from target: {} and Bearing to target: {}",
stGoalWaypointMeasurement.dDistanceMeters,
stGoalWaypointMeasurement.dStartRelativeBearing);
LOG_INFO(logging::g_qSharedLogger,
"Distance from Rover: {} and Bearing to Rover: {}",
stErrorMeasurement.dDistanceMeters,
stErrorMeasurement.dStartRelativeBearing);

// Only print out every so often.
static bool bAlreadyPrinted = false;
if ((std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count() % 5) == 0 && !bAlreadyPrinted)
{
// Get raw Navboard GPS position.
geoops::GPSCoordinate stCurrentGPSPosition = globals::g_pNavigationBoard->GetGPSData();
// Calculate error between pose and GPS.
geoops::GeoMeasurement stErrorMeasurement = geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetGPSCoordinate(), stCurrentGPSPosition);

LOG_INFO(logging::g_qSharedLogger,
"Distance from target: {} and Bearing to target: {}",
stGoalWaypointMeasurement.dDistanceMeters,
stGoalWaypointMeasurement.dStartRelativeBearing);
LOG_INFO(logging::g_qSharedLogger,
"Distance from Rover: {} and Bearing to Rover: {}",
stErrorMeasurement.dDistanceMeters,
stErrorMeasurement.dStartRelativeBearing);

// Set toggle.
bAlreadyPrinted = true;
}
else if (bAlreadyPrinted)
{
// Reset toggle.
bAlreadyPrinted = false;
}

// Check if we are at the goal waypoint.
if (stGoalWaypointMeasurement.dDistanceMeters > constants::NAVIGATING_REACHED_GOAL_RADIUS)
Expand Down
15 changes: 4 additions & 11 deletions src/states/VerifyingMarkerState.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,18 +113,11 @@ namespace statemachine
}
case Event::eVerifyingComplete:
{
// Submit logger message.
LOG_INFO(logging::g_qSharedLogger, "VerifyingMarkerState: Handling Verifying Complete event.");

// Send Reached Goal state over RoveComm.
// Construct a RoveComm packet.
rovecomm::RoveCommPacket<uint8_t> stPacket;
stPacket.unDataId = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_ID;
stPacket.unDataCount = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_COUNT;
stPacket.eDataType = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_TYPE;
stPacket.vData.emplace_back(1);
// Send telemetry over RoveComm to all subscribers.
network::g_pRoveCommUDPNode->SendUDPPacket(stPacket, "0.0.0.0", constants::ROVECOMM_OUTGOING_UDP_PORT);

// Send multimedia command to update state display.
globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eReachedGoal);
// Change state.
eNextState = States::eIdle;
break;
}
Expand Down
15 changes: 4 additions & 11 deletions src/states/VerifyingObjectState.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,18 +114,11 @@ namespace statemachine
}
case Event::eVerifyingComplete:
{
// Submit logger message.
LOG_INFO(logging::g_qSharedLogger, "VerifyingObjectState: Handling Verifying Complete event.");

// Send Reached Goal state over RoveComm.
// Construct a RoveComm packet.
rovecomm::RoveCommPacket<uint8_t> stPacket;
stPacket.unDataId = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_ID;
stPacket.unDataCount = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_COUNT;
stPacket.eDataType = manifest::Autonomy::TELEMETRY.find("REACHEDGOAL")->second.DATA_TYPE;
stPacket.vData.emplace_back(1);
// Send telemetry over RoveComm to all subscribers.
network::g_pRoveCommUDPNode->SendUDPPacket(stPacket, "0.0.0.0", constants::ROVECOMM_OUTGOING_UDP_PORT);

// Send multimedia command to update state display.
globals::g_pMultimediaBoard->SendLightingState(MultimediaBoard::MultimediaBoardLightingState::eReachedGoal);
// Change state.
eNextState = States::eIdle;
break;
}
Expand Down
Loading

0 comments on commit 5fd4f26

Please sign in to comment.