diff --git a/.devcontainer/Dockerfile_Focal b/.devcontainer/Dockerfile_Focal index 35b82732..f9c65091 100644 --- a/.devcontainer/Dockerfile_Focal +++ b/.devcontainer/Dockerfile_Focal @@ -16,6 +16,7 @@ ARG ZED_MAJOR="4" ARG ZED_MINOR="0" ARG CMAKE_VERSION="3.24.3" ARG OPENCV_VERSION="4.8.0" +ARG GEOLIB_VERSION="v2.3" ARG QUILL_VERSION="v3.3.1" ARG GTEST_VERSION="v1.14.0" ARG TZ="America/Chicago" @@ -113,11 +114,23 @@ RUN git clone --depth 1 --branch ${OPENCV_VERSION} https://github.com/opencv/ope cd ../.. && \ rm -rf opencv_contrib && rm -rf opencv +# Install GeographicLib from Source +RUN git clone --depth 1 --branch ${GEOLIB_VERSION} https://github.com/geographiclib/geographiclib.git && \ + mkdir geographiclib/build && \ + cd geographiclib/build && \ + cmake \ + -D CMAKE_BUILD_TYPE=Release .. && \ + make && \ + make install && \ + cd ../.. && \ + rm -rf geographiclib + # Install Quill from Source RUN git clone --depth 1 --branch ${QUILL_VERSION} http://github.com/odygrd/quill.git && \ mkdir quill/build && \ cd quill/build && \ - cmake .. && \ + cmake \ + -D CMAKE_BUILD_TYPE=Release .. && \ make && \ make install && \ cd ../.. && \ diff --git a/.devcontainer/Dockerfile_Jammy b/.devcontainer/Dockerfile_Jammy index edbd2911..aec47377 100644 --- a/.devcontainer/Dockerfile_Jammy +++ b/.devcontainer/Dockerfile_Jammy @@ -16,6 +16,7 @@ ARG ZED_MAJOR="4" ARG ZED_MINOR="0" ARG CMAKE_VERSION="3.24.3" ARG OPENCV_VERSION="4.8.0" +ARG GEOLIB_VERSION="v2.3" ARG QUILL_VERSION="v3.3.1" ARG GTEST_VERSION="v1.14.0" ARG TZ="America/Chicago" @@ -116,6 +117,17 @@ RUN git clone --depth 1 --branch ${OPENCV_VERSION} https://github.com/opencv/ope cd ../.. && \ rm -rf opencv_contrib && rm -rf opencv +# Install GeographicLib from Source +RUN git clone --depth 1 --branch ${GEOLIB_VERSION} https://github.com/geographiclib/geographiclib.git && \ + mkdir geographiclib/build && \ + cd geographiclib/build && \ + cmake \ + -D CMAKE_BUILD_TYPE=RelWithDebInfo .. && \ + make && \ + make install && \ + cd ../.. && \ + rm -rf geographiclib + # Install Quill from Source RUN git clone --depth 1 --branch ${QUILL_VERSION} http://github.com/odygrd/quill.git && \ mkdir quill/build && \ diff --git a/.devcontainer/Dockerfile_JetPack b/.devcontainer/Dockerfile_JetPack index dd495526..b62d5f77 100644 --- a/.devcontainer/Dockerfile_JetPack +++ b/.devcontainer/Dockerfile_JetPack @@ -16,6 +16,7 @@ ARG ZED_MAJOR="4" ARG ZED_MINOR="0" ARG CMAKE_VERSION="3.24.3" ARG OPENCV_VERSION="4.8.0" +ARG GEOLIB_VERSION="v2.3" ARG QUILL_VERSION="v3.3.1" ARG GTEST_VERSION="v1.14.0" ARG TZ="America/Chicago" @@ -115,6 +116,17 @@ RUN git clone --depth 1 --branch ${OPENCV_VERSION} https://github.com/opencv/ope cd ../.. && \ rm -rf opencv_contrib && rm -rf opencv +# Install GeographicLib from Source +RUN git clone --depth 1 --branch ${GEOLIB_VERSION} https://github.com/geographiclib/geographiclib.git && \ + mkdir geographiclib/build && \ + cd geographiclib/build && \ + cmake \ + -D CMAKE_BUILD_TYPE=RelWithDebInfo .. && \ + make && \ + make install && \ + cd ../.. && \ + rm -rf geographiclib + # Install Quill from Source RUN git clone --depth 1 --branch ${QUILL_VERSION} http://github.com/odygrd/quill.git && \ mkdir quill/build && \ diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index f46cc421..59c1296f 100755 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -32,7 +32,7 @@ // "image": "ghcr.io/missourimrdt/autonomy-jetpack:latest", // "build": { // // "dockerfile": "Dockerfile_Jammy" - // "dockerfile": "Dockerfile_Focal" + // // "dockerfile": "Dockerfile_Focal" // // "dockerfile": "Dockerfile_JetPack" // }, // Features to add to the dev container. More info: https://containers.dev/features. @@ -95,7 +95,9 @@ "/usr/local/include/gmock/", "/usr/local/include/gmock/**", "/usr/local/include/gtest/", - "/usr/local/include/gtest/**" + "/usr/local/include/gtest/**", + "/usr/local/include/GeographicLib/", + "/usr/local/include/GeographicLib/**" ], // CMAKE extension settings. "cmake.configureOnOpen": true, diff --git a/CMakeLists.txt b/CMakeLists.txt index c298a6dc..7545f280 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,6 +74,10 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE) find_package(OpenCV REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) +## Find Geographic Lib. +find_package(GeographicLib REQUIRED) +include_directories(${GeographicLib_INCLUDE_DIRS}) + ## Find CUDA. find_package(CUDA 11.4 REQUIRED) message(STATUS "Found CUDA ${CUDA_VERSION_STRING} at ${CUDA_TOOLKIT_ROOT_DIR}") @@ -133,12 +137,13 @@ target_link_libraries(${PROJECT_NAME} PRIVATE Threads::Threads Eigen3::Eigen quill::quill ${OpenCV_LIBS} + ${GeographicLib_LIBRARIES} ${ZED_LIBS} RoveComm_CPP ) ## Package Executable -install(TARGETS ${PROJECT_NAME} RUNTIME_DEPENDENCIES DIRECTORIES ${OpenCV_LIBS} ${ZED_LIBS} quill::quill RoveComm_CPP RUNTIME DESTINATION bin) +install(TARGETS ${PROJECT_NAME} RUNTIME_DEPENDENCIES DIRECTORIES ${OpenCV_LIBS} ${GeographicLib_LIBRARIES} ${ZED_LIBS} quill::quill RoveComm_CPP RUNTIME DESTINATION bin) ## Unit/Integration Tests file(GLOB_RECURSE UnitTests_SRC CONFIGURE_DEPENDS "tests/Unit/*.cc") @@ -150,7 +155,7 @@ list(LENGTH IntegrationTests_SRC IntegrationTests_LEN) if (UnitTests_LEN GREATER 0) add_executable(${PROJECT_NAME}_UnitTests ${UnitTests_SRC} ${Logging_SRC}) - target_link_libraries(${PROJECT_NAME}_UnitTests GTest::gtest GTest::gtest_main ${OpenCV_LIBS} quill::quill) + target_link_libraries(${PROJECT_NAME}_UnitTests GTest::gtest GTest::gtest_main quill::quill ${OpenCV_LIBS} ${GeographicLib_LIBRARIES}) add_test(Unit_Tests ${PROJECT_NAME}_UnitTests) else() message("No Unit Tests!") @@ -158,7 +163,7 @@ endif() if (IntegrationTests_LEN GREATER 0) add_executable(${PROJECT_NAME}_IntegrationTests ${IntegrationTests_SRC} ${Logging_SRC}) - target_link_libraries(${PROJECT_NAME}_IntegrationTests GTest::gtest GTest::gtest_main quill::quill) + target_link_libraries(${PROJECT_NAME}_IntegrationTests GTest::gtest GTest::gtest_main quill::quill ${OpenCV_LIBS} ${GeographicLib_LIBRARIES}) add_test(Integration_Tests ${PROJECT_NAME}_IntegrationTests) else() message("No Integration Tests!") diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 4c4f4c66..2b127398 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -103,6 +103,7 @@ Examples: `int g_nExampleGlobalInteger` or `int m_nExampleMemberInteger` - Boost: - State Chart > `sc` > Example: `sc::simple_state` - Quill > `q` > Example: `quill::Config qConfig` +- GeographicLib > `ge` > Example: `GeographicLib::Geodesic geExampleGeographicType` ### Macro Names Macros should be in all caps using underscores to separate words. They should be detailed enough to not require an additional comment. diff --git a/data/Custom_Dictionaries/Autonomy-Dictionary.txt b/data/Custom_Dictionaries/Autonomy-Dictionary.txt index 952d5c22..715ca78d 100644 --- a/data/Custom_Dictionaries/Autonomy-Dictionary.txt +++ b/data/Custom_Dictionaries/Autonomy-Dictionary.txt @@ -4,28 +4,42 @@ alloc allocs APRILTAG Aruco +autofetch arucotag AUTONOMYTHREAD BASICCAM CALIB clayjay codeql +codezombiech Coeffs +Colour +cpptools +cschlosser CUDA deadband deque Desaturate devcontainer diffdrive +doxdocgen Doxygen driveboard +Extention +feedforward flightsticks geoops Geospatial +gmock +GPSMDRS +GPSSDELC +gruntfuggly +gtest hyperthreaded imgops imread imshow +jbockle inferencing jetpack jspencerpittman @@ -37,6 +51,7 @@ mainpage MAPRANGE Memcheck microcontrollers +missourimrdt MJPEG MRDT MULTIMEDIABOARD @@ -55,9 +70,12 @@ reprojection RESOLUTIONX RESOLUTIONY RGBE +Rolla RIGHTCAM rovecomm searchbar +setpoint +setpoints SIMCAM statechart Struct @@ -67,13 +85,18 @@ Tele tensorflowtag threadpool tparam +twxs UART +UTMMDRS +UTMUPS UNVALIDATED valgrind Watchpoints +wslg XYZBGRA XYZRGBA YUVJ YUYV +Zain ZEDCAM ZEDSDK diff --git a/src/AutonomyConstants.h b/src/AutonomyConstants.h index 44c333c4..c3e36aa9 100644 --- a/src/AutonomyConstants.h +++ b/src/AutonomyConstants.h @@ -37,7 +37,16 @@ namespace constants const float DRIVE_MIN_EFFORT = -0.5; // Control constants. - const bool DRIVE_SQUARE_CONTROL_INPUTS = false; // This is used by the DifferentialDrive algorithms. True makes fine inputs smoother, but less responsive. + const double DRIVE_PID_PROPORTIONAL = 0.1; // The proportional gain for the controller used to point the rover at a goal heading during navigation. + const double DRIVE_PID_INTEGRAL = 0.01; // The integral gain for the controller used to point the rover at a goal heading during navigation. + const double DRIVE_PID_DERIVATIVE = 0.0; // The derivative gain for the controller used to point the rover at a goal heading during navigation. + const double DRIVE_PID_MAX_ERROR_PER_ITER = 100; // The max allowable error the controller will see per iteration. This is on degrees from setpoint. + const double DRIVE_PID_MAX_INTEGRAL_TERM = 0.3; // The max effort the I term is allowed to contribute. + const double DRIVE_PID_MAX_OUTPUT_EFFORT = 0.5; // The max effort the entire PID controller is allowed to output. Range is within DRIVE_MAX/MIN_POWER. + const double DRIVE_PID_MAX_RAMP_RATE = 0.4; // The max ramp rate of the output of the PID controller. + const double DRIVE_PID_OUTPUT_FILTER = 0.0; // Larger values will filter out large spikes or oscillations. 0.1 is a good starting point. + const bool DRIVE_PID_OUTPUT_REVERSED = false; // Negates the output of the PID controller. + const bool DRIVE_SQUARE_CONTROL_INPUTS = false; // This is used by the DifferentialDrive algorithms. True makes fine inputs smoother, but less responsive. const bool DRIVE_CURVATURE_KINEMATICS_ALLOW_TURN_WHILE_STOPPED = true; // This enabled turning in-place when using curvature drive control. /////////////////////////////////////////////////////////////////////////// diff --git a/src/algorithms/DifferentialDrive.hpp b/src/algorithms/DifferentialDrive.hpp index d7f76a85..71172dd3 100644 --- a/src/algorithms/DifferentialDrive.hpp +++ b/src/algorithms/DifferentialDrive.hpp @@ -12,13 +12,13 @@ #ifndef DIFFERENTIAL_DRIVE_HPP #define DIFFERENTIAL_DRIVE_HPP +#include "../AutonomyConstants.h" #include "../util/NumberOperations.hpp" +#include "controllers/PIDController.h" #include #include -// TODO: Write Unit Tests - /****************************************************************************** * @brief Namespace containing algorithms related to calculating drive powers, * odometry, trajectories, kinematics, etc of differential drive (tank drive) robots. @@ -45,18 +45,48 @@ ******************************************************************************/ namespace diffdrive { + ///////////////////////////////////////// + // Declare public enums that are specific to and used within this namespace. + ///////////////////////////////////////// + + // Enum for choosing the differential drive method for certain functions. + // Enumerator used to specify what method of drive control to use. + enum DifferentialControlMethod + { + eTankDrive, // Simple controller. Left and right input is directly assigned to each side of the drivetrain. + eArcadeDrive, // Typical drive control method for flightsticks. Uses speed and turn input to determine drive powers. + eCurvatureDrive // Similar to arcade drive with flightsticks, but the current turning speed of the robot is dampened when moving fast. + }; + + /****************************************************************************** + * @brief This struct is used to store the left and right drive powers for the robot. + * Storing these values in a struct allows for easy handling and access to said + * variables. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-13 + ******************************************************************************/ + struct DrivePowers + { + public: + // Define public struct attributes. + double dLeftDrivePower; + double dRightDrivePower; + }; + /****************************************************************************** * @brief Tank drive inverse kinematics for differential drive robots. * * @param dLeftSpeed - The left drive power input for the drive. (-1.0 - 1.0) * @param dRightSpeed - The right drive power input for the drive. (-1.0 - 1.0) * @param bSquareInputs - Decreases the input sensitivity at low input speeds. - * @return std::array - The result drive powers. [left, right] + * @return DrivePowers - The result drive powers. [left, right] * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-22 ******************************************************************************/ - std::array CalculateTankDrive(double dLeftSpeed, double dRightSpeed, bool bSquareInputs = false) + inline DrivePowers CalculateTankDrive(double dLeftSpeed, double dRightSpeed, bool bSquareInputs = false) { // Limit the input powers. dLeftSpeed = std::clamp(dLeftSpeed, -1.0, 1.0); @@ -80,12 +110,12 @@ namespace diffdrive * @param dSpeed - Speed at which the robot should drive forward/backward. Forward is positive. (-1.0 - 1.0) * @param dRotation - The rotation rate of the robot. Clockwise is positive. (-1.0 - 1.0) * @param bSquareInputs - Decreases the input sensitivity at low input speeds. - * @return std::array - The result drive powers. [left, right] + * @return DrivePowers - The result drive powers. [left, right] * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-22 ******************************************************************************/ - std::array CalculateArcadeDrive(double dSpeed, double dRotation, const bool bSquareInputs = true) + inline DrivePowers CalculateArcadeDrive(double dSpeed, double dRotation, const bool bSquareInputs = false) { // Limit the input powers. dSpeed = std::clamp(dSpeed, -1.0, 1.0); @@ -99,19 +129,19 @@ namespace diffdrive dRotation = std::copysign(dRotation * dRotation, dRotation); } - // Differential drive inverse kinematics for arcade drive. - double dLeftSpeed = dSpeed - dRotation; - double dRightSpeed = dSpeed + dRotation; // Find the maximum possible value of throttle and turn along the vector that the speed and rotation is pointing. double dGreaterInput = std::max(std::abs(dSpeed), std::abs(dRotation)); double dLesserInput = std::min(std::abs(dSpeed), std::abs(dRotation)); // If the biggest input is zero, then the wheel speeds should be zero. - if (dGreaterInput) + if (dGreaterInput == 0.0) { // Return zero drive power output. return {0.0, 0.0}; } + // Differential drive inverse kinematics for arcade drive. + double dLeftSpeed = dSpeed + dRotation; + double dRightSpeed = dSpeed - dRotation; // Desaturate the input. Normalize to (-1.0, 1.0). double dSaturatedInput = (dGreaterInput + dLesserInput) / dGreaterInput; dLeftSpeed /= dSaturatedInput; @@ -130,12 +160,12 @@ namespace diffdrive * @param dRotation - The normalized curvature of the robot. Clockwise is positive. (-1.0, 1.0) * @param bAllowTurnInPlace - Whether or not forward input is required to turn. True makes this control exactly like a car. * @param bSquareInputs - Decreases the input sensitivity at low input speeds. - * @return std::array - The result drive powers. [left, right] + * @return DrivePowers - The result drive powers. [left, right] * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-22 ******************************************************************************/ - std::array CalculateCurvatureDrive(double dSpeed, double dRotation, const bool bAllowTurnInPlace, const bool bSquareInputs = false) + inline DrivePowers CalculateCurvatureDrive(double dSpeed, double dRotation, const bool bAllowTurnInPlace, const bool bSquareInputs = false) { // Create instance variables. double dLeftSpeed = 0.0; @@ -154,17 +184,17 @@ namespace diffdrive } // Check if turn-in-place is allowed. - if (bAllowTurnInPlace) + if (bAllowTurnInPlace && dSpeed == 0.0) { // Differential drive inverse kinematics for curvature drive with turn while stopped. - dLeftSpeed = dSpeed - dRotation; - dRightSpeed = dSpeed + dRotation; + dLeftSpeed = dSpeed + dRotation; + dRightSpeed = dSpeed - dRotation; } else { // Differential drive inverse kinematics for curvature drive. - dLeftSpeed = dSpeed - std::abs(dSpeed) * dRotation; - dRightSpeed = dSpeed + std::abs(dSpeed) * dRotation; + dLeftSpeed = dSpeed + std::abs(dSpeed) * dRotation; + dRightSpeed = dSpeed - std::abs(dSpeed) * dRotation; } // Desaturate the input. Normalize to (-1.0, 1.0). @@ -178,5 +208,55 @@ namespace diffdrive // Return result drive powers. return {dLeftSpeed, dRightSpeed}; } + + /****************************************************************************** + * @brief This function will calculate the drive powers for a given speed and + * absolute heading. The method used to get the drive powers is determined by + * the given enumerator (must be arcade or curvature). The turning rate or + * curvature is determined by the given PID controller which must be properly + * configured. + * + * @param dGoalSpeed - The goal speed for the robot. + * @param dGoalHeading - The goal absolute heading for the robot. (0-360 degrees, CW positive.) + * @param dActualHeading - The actual current heading of the robot. + * @param eDriveMethod - The differential drive method to use for navigation. + * @param PID - A reference to the PID controller to use for hitting the heading setpoint. + * @return DrivePowers - The resultant drive powers. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ + inline DrivePowers CalculateMotorPowerFromHeading(double dGoalSpeed, + double dGoalHeading, + double dActualHeading, + DifferentialControlMethod eDriveMethod, + PIDController& PID) + { + // Create instance variables. + DrivePowers stOutputPowers; + + // Get control output from PID controller. + double dTurnOutput = PID.Calculate(dActualHeading, dGoalHeading); + // Calculate drive powers from inverse kinematics of goal speed and turning adjustment. + switch (eDriveMethod) + { + case eArcadeDrive: stOutputPowers = CalculateArcadeDrive(dGoalSpeed, dTurnOutput, constants::DRIVE_SQUARE_CONTROL_INPUTS); break; + case eCurvatureDrive: + stOutputPowers = CalculateCurvatureDrive(dGoalSpeed, + dTurnOutput, + constants::DRIVE_CURVATURE_KINEMATICS_ALLOW_TURN_WHILE_STOPPED, + constants::DRIVE_SQUARE_CONTROL_INPUTS); + break; + default: + stOutputPowers = CalculateCurvatureDrive(dGoalSpeed, + dGoalHeading, + constants::DRIVE_CURVATURE_KINEMATICS_ALLOW_TURN_WHILE_STOPPED, + constants::DRIVE_SQUARE_CONTROL_INPUTS); + break; + } + + // Return result powers. + return stOutputPowers; + } } // namespace diffdrive #endif diff --git a/src/algorithms/controllers/PIDController.cpp b/src/algorithms/controllers/PIDController.cpp new file mode 100644 index 00000000..13bf29d6 --- /dev/null +++ b/src/algorithms/controllers/PIDController.cpp @@ -0,0 +1,736 @@ +/****************************************************************************** + * @brief Implements the PIDController class. + * Design is based off of https://github.com/tekdemo/MiniPID and + * https://github.com/wpilibsuite/allwpilib/blob/b3eb64b0f774130833e6b5f8c18cd61403f420f3/wpimath/src/main/native/cpp/controller/PIDController.cpp + * + * @file PIDController.cpp + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + * + * @copyright Copyright Mars Rover Design Team 2023 - All Rights Reserved + ******************************************************************************/ + +#include "PIDController.h" +#include "../../util/NumberOperations.hpp" + +/****************************************************************************** + * @brief Construct a new PIDController::PIDController object. + * + * @param dKp - The proportional gain (Kp) component adjusts the control output in proportion to the current error. + * @param dKi - The integral gain (Ki) component accumulates past errors to eliminate steady-state error. + * @param dKd - The derivative gain (Kd) component anticipates future error by considering the rate of change of the error. + * @param dKff - The feedforward gain (Kff) allows for predictive control, improving response time and minimizing overshoot. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +PIDController::PIDController(const double dKp, const double dKi, const double dKd, const double dKff) +{ + // Initialize member variable. + m_dKp = dKp; + m_dKi = dKi; + m_dKd = dKd; + m_dKff = dKff; + m_dSetpoint = 0.0; + m_dErrorSum = 0.0; + m_dMaxError = 0.0; + m_dMaxIEffort = 0.0; + m_dMinEffort = 0.0; + m_dMaxEffort = 0.0; + m_dLastActual = 0.0; + m_dOutputRampRate = 0.0; + m_dLastControlOutput = 0.0; + m_dOutputFilter = 0.0; + m_dMaxSetpointDifference = 0.0; + m_bFirstCalculation = true; + m_bReversed = false; +} + +/****************************************************************************** + * @brief Calculate the next control output given the current actual and a setpoint. + * + * @param dActual - The current actual position of the system. + * @param dSetpoint - The desired setpoint of the controller. + * @return double - The resultant PID controller output. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +double PIDController::Calculate(const double dActual, const double dSetpoint) +{ + // Create instance variables. + double dPTermOutput; + double dITermOutput; + double dDTermOutput; + double dFFTermOutput; + double dOutput; + + // Update setpoint member variable. + m_dSetpoint = dSetpoint; + + // Apply the sliding setpoint range adjust if necessary. + if (m_dMaxSetpointDifference != 0) + { + // Clamp current setpoint value within a certain range of the current actual. + m_dSetpoint = numops::Clamp(m_dSetpoint, dActual - m_dMaxSetpointDifference, dActual + m_dMaxSetpointDifference); + } + + // Determine error from setpoint. + double dError = dSetpoint - dActual; + + // TODO: Ensure this wraparound login is correct. + // Check if the input, and therefor controller is continuous. + if (m_bControllerIsContinuous) + { + // Calculate error bounds. + double dErrorBound = (m_dMaximumContinuousInput - m_dMinimumContinuousInput) / 2.0; + // Wrap heading. + dError = numops::InputModulus(dError, -dErrorBound, dErrorBound); + } + + // Calculate feedforward term. + dFFTermOutput = m_dKff * dSetpoint; + // Calculate proportional term. + dPTermOutput = m_dKp * dError; + + // Check if this is the first time doing a calculation. + if (m_bFirstCalculation) + { + // Assume the process variable hold same as previous. + m_dLastActual = dActual; + // Assume the last output is the current time-independent output. + m_dLastControlOutput = dPTermOutput + dFFTermOutput; + + // Set first calculation toggle to false. + m_bFirstCalculation = false; + } + + // Calculate derivative term. + // Note, derivative is actually negative and "slows" the system if it's doing + // the correct thing. Small gain values help prevent output spikes and overshoot. + dDTermOutput = -m_dKd * (dActual - m_dLastActual); + m_dLastActual = dActual; + + // Calculate integral term. + // The integral term is more complex. There's several things to factor in to make it easier to deal with. + // 1. m_dMaxIEffort restricts the amount of output contributed by the integral term. + // 2. prevent windup by not increasing errorSum if we're already running against our max Ioutput + // 3. prevent windup by not increasing errorSum if output is output=maxOutput + dITermOutput = m_dKi * m_dErrorSum; + // Apply integral term limits. + if (m_dMaxIEffort != 0) + { + // Clamp current integral term output. + dITermOutput = numops::Clamp(dITermOutput, -m_dMaxIEffort, m_dMaxIEffort); + } + + // Here's PID! + dOutput = dFFTermOutput + dPTermOutput + dITermOutput + dDTermOutput; + // Check if error is at not at limits. + if (m_dMinEffort != m_dMaxEffort && !numops::Bounded(dOutput, m_dMinEffort, m_dMaxEffort)) + { + // Reset the error to a sane level. Settings to the current error ensures a smooth transition when the P term + // decreases enough for the I term to start acting upon the controller properly. + m_dErrorSum = dError; + } + // Check if the error is within the allowed ramp rate compared to the last output. + else if (m_dOutputRampRate != 0 && !numops::Bounded(dOutput, m_dLastControlOutput - m_dOutputRampRate, m_dLastControlOutput + m_dOutputRampRate)) + { + // Reset the error to a sane level. Settings to the current error ensures a smooth transition when the P term + // decreases enough for the I term to start acting upon the controller properly. + m_dErrorSum = dError; + } + // Check if integral term output should be limited. + else if (m_dMaxIEffort != 0) + { + // In addition to output limiting directly, we also want to prevent integral term wind-up. Restrict directly. + m_dErrorSum = numops::Clamp(m_dErrorSum + dError, -m_dMaxError, m_dMaxError); + } + else + { + // Accumulate error. + m_dErrorSum += dError; + } + + // Restrict output to our specified ramp limits. + if (m_dOutputRampRate != 0) + { + // Clamp output to ramp rate. + dOutput = numops::Clamp(dOutput, m_dLastControlOutput - m_dOutputRampRate, m_dLastControlOutput + m_dOutputRampRate); + } + // Restrict output to our specified effort limits. + if (m_dMinEffort != m_dMaxEffort) + { + // Clamp output to effort limits. + dOutput = numops::Clamp(dOutput, m_dMinEffort, m_dMaxEffort); + } + // Reduce jitters and output spikes with an exponential rolling sum filter. + if (m_dOutputFilter != 0) + { + // Filter output effort. + dOutput = (m_dLastControlOutput * m_dOutputFilter) + (dOutput * (1 - m_dOutputFilter)); + } + + // Update last output member variable. + m_dLastControlOutput = dOutput; + + // Return the PID controller calculated output effort. + return dOutput; +} + +/****************************************************************************** + * @brief Calculate the next control output given the current actual and using the + * previously set setpoint. + * + * @param dActual - The current actual position of the system. + * @return double - The resultant PID controller output. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +double PIDController::Calculate(const double dActual) +{ + // Calculate and return the output from the PIDController using the same setpoint. + return this->Calculate(dActual, m_dSetpoint); +} + +/****************************************************************************** + * @brief Calculates the control output using the last provided setpoint and actual. + * + * @return double - The resultant PID controller output. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +double PIDController::Calculate() +{ + // Calculate and return the output from the PIDController using the previous actual and setpoint. + return this->Calculate(m_dLastActual, m_dSetpoint); +} + +/****************************************************************************** + * @brief Enables continuous wraparound of PID controller input. Use for setpoints + * that exists on a circle. For example, if using degrees for setpoint and actual + * calling this method with 0, 360 will treat 0 and 360 as the same number. This + * prevents the controller from going the long way around the circle when the setpoint + * and actual are near the wraparound point. + * + * @param dMinimumInput - Minimum input before wraparound. + * @param dMaximumInput - Maximum input before wraparound. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +void PIDController::EnableContinuousInput(const double dMinimumInput, const double dMaximumInput) +{ + // Assign member variables. + m_dMinimumContinuousInput = dMinimumInput; + m_dMaximumContinuousInput = dMaximumInput; + + // Enable continuous input. + m_bControllerIsContinuous = true; +} + +/****************************************************************************** + * @brief Disable continuous input wraparound of the PID controller. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +void PIDController::DisableContinuousInput() +{ + // Disable continuous input. + m_bControllerIsContinuous = false; +} + +/****************************************************************************** + * @brief Resets the controller. This erases the integral term buildup, and removes + * derivative term on the next iteration. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::Reset() +{ + // Reset necessary values to reset the controller. + m_bFirstCalculation = true; + m_dErrorSum = 0.0; +} + +/****************************************************************************** + * @brief Configure the proportional gain parameter. This quickly responds to changes in setpoint, + * and provides most of the initial driving force to make corrections. + * Some systems can be used with only a P gain, and many can be operated with only PI. + * For position based controllers, Proportional gain the first parameter to tune, with integral gain second. + * For rate controlled systems, Proportional is often the second after feedforward. + * + * @param dKp - The proportional gain for the controller to use for calculation. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetProportional(const double dKp) +{ + // Assign member variables. + m_dKp = dKp; + + // Check signs of gains. + this->CheckGainSigns(); +} + +/****************************************************************************** + * @brief Configure the integral gain parameter. This is used for overcoming disturbances, and ensuring that + * the controller always minimizes error, when the control actual is close to the setpoint. + * Typically tuned second for "Position" based modes, and third for "Rate" or continuous based modes. + * + * @param dKi - The integral gain for the controller to use for calculation. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetIntegral(const double dKi) +{ + // Check if the current I gain is not zero and we need to scale our current error. + if (m_dKi != 0) + { + // Scale the current error sum, to match the new integral gain. + m_dErrorSum = m_dErrorSum * (m_dKi / dKi); + } + // Check if max integral term effort is enabled. + if (m_dMaxIEffort != 0) + { + // Update max error from new integral and max effort. + m_dMaxError = m_dMaxIEffort / dKi; + } + + // Assign integral gain member variable. + m_dKi = dKi; + + // Check the signs of the PID gains. + this->CheckGainSigns(); +} + +/****************************************************************************** + * @brief Configure the derivative gain parameter. This acts as a brake or dampener on the control effort. + * The more the controller tries to change the value, the more it counteracts the effort. Typically this + * Parameter is tuned last and only used to reduce overshoot in systems. When using derivative gain, you + * can usually get a more responsive controller by over-tuning proportional gain so that small oscillations + * are visible and then bringing derivative gain up until the oscillations disappear. + * + * @param dKd - The derivative gain for the controller to use for calculation. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetDerivative(const double dKd) +{ + // Assign member variables. + m_dKd = dKd; + + // Check signs of gains. + this->CheckGainSigns(); +} + +/****************************************************************************** + * @brief Configure the feedforward gain parameter. This is excellent for velocity, rate, + * and other continuous control modes where you can expect a rough output value based + * solely on the setpoint. This should not be used in "position" based control modes. + * + * @param dKff - The feedforward gain for the controller to use for calculation. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetFeedforward(const double dKff) +{ + // Assign member variables. + m_dKff = dKff; + + // Check signs of gains. + this->CheckGainSigns(); +} + +/****************************************************************************** + * @brief Mutator for PID gains of the controller. + * + * @param dKp - The proportional gain for the controller to use for calculation. + * @param dKi - The integral gain for the controller to use for calculation. + * @param dKd - The derivative gain for the controller to use for calculation. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetPID(const double dKp, const double dKi, const double dKd) +{ + // Update member variables. + m_dKp = dKp; + m_dKd = dKd; + + // Call SetIntegral since is scales the accumulated error and checks signs. + this->SetIntegral(dKi); +} + +/****************************************************************************** + * @brief Mutator for PID and Feedforward gains of the controller. + * + * @param dKp - The proportional gain for the controller to use for calculation. + * @param dKi - The integral gain for the controller to use for calculation. + * @param dKd - The derivative gain for the controller to use for calculation. + * @param dKff - The feedforward gain for the controller to use for calculation. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetPID(const double dKp, const double dKi, const double dKd, const double dKff) +{ + // Update member variables. + m_dKp = dKp; + m_dKd = dKd; + m_dKff = dKff; + + // Call SetIntegral since is scales the accumulated error and checks signs. + this->SetIntegral(dKi); +} + +/****************************************************************************** + * @brief Mutator for the setpoint for the PID controller. + * + * @param dSetpoint - The setpoint that the controller should aim to get the actual to. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetSetpoint(const double dSetpoint) +{ + // Assign member variable. + m_dSetpoint = dSetpoint; +} + +/****************************************************************************** + * @brief Mutator for the maximum allowable distance of the setpoint from the actual value. + * This doesn't limit the setable value of the setpoint. It only limits the value that the + * controller is given specify the error from the setpoint. + * + * @param dMaxSetpointDifference - The allowable distance of the setpoint from the actual. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetMaxSetpointDifference(const double dMaxSetpointDifference) +{ + // Assign member variables. + m_dMaxSetpointDifference = dMaxSetpointDifference; +} + +/****************************************************************************** + * @brief Mutator for the max value of the integral term during PID calculation. + * This is useful for preventing integral wind-up runaways. + * + * @param dMaxIEffort - The max allowable value of the integral term. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetMaxIntegralEffort(const double dMaxIEffort) +{ + // Assign member variable. + m_dMaxIEffort = dMaxIEffort; +} + +/****************************************************************************** + * @brief Mutator for setting the minimum and maximum allowable values of the + * control output from the controller. + * + * @param dMinEffort - The minimum allowable output from the PID controller. + * @param dMaxEffort - The maximum allowable output from the PID controller. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetOutputLimits(const double dMinEffort, const double dMaxEffort) +{ + // Check if the maximum is greater than the minimum. + if (dMaxEffort > dMinEffort) + { + // Assign member variables. + m_dMinEffort = dMinEffort; + m_dMaxEffort = dMaxEffort; + + // Ensure the bounds of the integral term are within the bounds of the allowable output range. + if (m_dMaxIEffort == 0 || m_dMaxIEffort > (dMaxEffort - dMinEffort)) + { + // Set new integral max output. + this->SetMaxIntegralEffort(dMaxEffort - dMinEffort); + } + } +} + +/****************************************************************************** + * @brief Mutator for setting the minimum and maximum allowable values of the + * control output from the controller. + * + * @param dMinMax - The overall output range of the controller. Must be positive. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetOutputLimits(const double dMinMax) +{ + // Set output limits with the same max and min. + this->SetOutputLimits(-dMinMax, dMinMax); +} + +/****************************************************************************** + * @brief Mutator for the maximum rate that the output can change per iteration. + * + * @param dOutputRampRate - The maximum ramp rate of the output for the controller. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::SetOutputRampRate(const double dOutputRampRate) +{ + // Assign member variable. + m_dOutputRampRate = dOutputRampRate; +} + +/****************************************************************************** + * @brief Set a filter on the output to reduce sharp oscillations. 0.1 is likely a good + * starting point. + * + * @param dStrength - The output filtering strength. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +void PIDController::SetOutputFilter(const double dStrength) +{ + // Check if the strength is valid. + if (dStrength == 0 || numops::Bounded(dStrength, 0.0, 1.0)) + { + // Assign member variable. + m_dOutputFilter = dStrength; + } +} + +/****************************************************************************** + * @brief Set the operating direction of the PID controller. Set true to reverse PID + * output. + * + * @param bReversed - Whether or not eh PID output should be negated. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +void PIDController::SetDirection(const bool bReversed) +{ + // Assign member variable. + m_bReversed = bReversed; +} + +/****************************************************************************** + * @brief Accessor for proportional gain of the PID controller. + * + * @return double - dKp proportional gain of the controller. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetProportional() const +{ + // Return the member variable. + return m_dKp; +} + +/****************************************************************************** + * @brief Accessor for integral gain of the PID controller. + * + * @return double - dKi integral gain of the controller. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetIntegral() const +{ + // Return the member variable. + return m_dKi; +} + +/****************************************************************************** + * @brief Accessor for derivative gain of the PID controller. + * + * @return double - dKd derivative gain of the controller. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetDerivative() const +{ + // Return the member variable. + return m_dKd; +} + +/****************************************************************************** + * @brief Accessor for feedforward gain of the PID controller. + * + * @return double - dKff feedforward gain of the controller. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetFeedforward() const +{ + // Return the member variable. + return m_dKff; +} + +/****************************************************************************** + * @brief Accessor for the current setpoint that the controller is trying to reach. + * + * @return double - The current setpoint. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetSetpoint() const +{ + // Return the member variable. + return m_dSetpoint; +} + +/****************************************************************************** + * @brief Accessor for the maximum allowable difference the setpoint can be + * from the actual. Doesn't limit the setpoint, just the max error. + * 0 = disabled. + * + * @return double - The maximum setpoint difference from the actual. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetMaxSetpointDifference() const +{ + // Return the member variable. + return m_dMaxSetpointDifference; +} + +/****************************************************************************** + * @brief Accessor for the maximum contribution of the integral term within the + * controller. 0 = disabled. + * + * @return double - The maximum allowed integral value. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetMaxIntegralEffort() const +{ + // Return the member variable. + return m_dMaxIEffort; +} + +/****************************************************************************** + * @brief Accessor the the maximum allowed output ramp rate of the PID controller. + * + * @return double - The max ramp rate of the controller output. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetOutputRampRate() const +{ + // Return the member variable. + return m_dOutputRampRate; +} + +/****************************************************************************** + * @brief Accessor for the exponential rolling sum filter strength. Used to + * reduce sharp output oscillations or jumps. + * + * @return double - Strength of the output filtering. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +double PIDController::GetOutputFilter() const +{ + // Return the member variable. + return m_dOutputFilter; +} + +/****************************************************************************** + * @brief Accessor for if the controller outputs are set to be negated or reversed. + * + * @return true - The output is negated. + * @return false - The output is not negated. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +bool PIDController::GetReversed() const +{ + // Return the member variable. + return m_bReversed; +} + +/****************************************************************************** + * @brief To operate correctly, all PID parameters require the same sign. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-17 + ******************************************************************************/ +void PIDController::CheckGainSigns() +{ + // Check if the gains are supposed to be reversed. + if (m_bReversed) + { + // All values should be below zero. + if (m_dKp > 0) + { + // Flip sign sign for proportional gain. + m_dKi *= -1; + } + if (m_dKi > 0) + { + // Flip sign for integral gain. + m_dKi *= -1; + } + if (m_dKd > 0) + { + // Flip sign for derivative gain. + m_dKd *= -1; + } + if (m_dKff > 0) + { + // Flip sign for feedforward gain. + m_dKff *= -1; + } + } + else + { + // All values should be above zero. + if (m_dKp < 0) + { + // Flip sign sign for proportional gain. + m_dKi *= -1; + } + if (m_dKi < 0) + { + // Flip sign for integral gain. + m_dKi *= -1; + } + if (m_dKd < 0) + { + // Flip sign for derivative gain. + m_dKd *= -1; + } + if (m_dKff < 0) + { + // Flip sign for feedforward gain. + m_dKff *= -1; + } + } +} diff --git a/src/algorithms/controllers/PIDController.h b/src/algorithms/controllers/PIDController.h new file mode 100644 index 00000000..506406a1 --- /dev/null +++ b/src/algorithms/controllers/PIDController.h @@ -0,0 +1,123 @@ +/****************************************************************************** + * @brief Defines the PIDController class. + * + * @file PIDController.h + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-16 + * + * @copyright Copyright Mars Rover Design Team 2023 - All Rights Reserved + ******************************************************************************/ + +#ifndef PID_CONTROLLER_H +#define PID_CONTROLLER_H + +/****************************************************************************** + * @brief This class encapsulates the fundamental principles of Proportional-Integral-Derivative (PID) control with an additional feedforward component. It provides a + * versatile control mechanism for various systems, enabling precise control of processes by adjusting an output in response to the error between a desired setpoint + * and a measured process variable. + * + * The key features of this class include: + * - Proportional Control: The proportional gain (Kp) component adjusts the control output in proportion to the current error. + * - Integral Control: The integral gain (Ki) component accumulates past errors to eliminate steady-state error. + * - Derivative Control: The derivative gain (Kd) component anticipates future error by considering the rate of change of the error. + * - Feedforward Control: The feedforward gain (Kff) allows for predictive control, improving response time and minimizing overshoot. + * See https://blog.opticontrols.com/archives/297 for a better explanation on feedforward controller. + * + * Additionally, this PID controller offers an optional wraparound feature. When enabled, it can be used for systems with non-linear, circular, or cyclic setpoints, + * effectively managing control in such scenarios. + * + * This class is a valuable tool for control engineering, robotics, and automation, allowing you to fine-tune the control behavior of various processes and systems. + * + * For specific details on class attributes and methods, please refer to the class documentation. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-16 + ******************************************************************************/ +class PIDController +{ + public: + ///////////////////////////////////////// + // Declare public enums that are specific to and used withing this class. + ///////////////////////////////////////// + + ///////////////////////////////////////// + // Declare public methods and member variables. + ///////////////////////////////////////// + + PIDController(const double dKp, const double dKi, const double dKd, const double dKff = 0.0); + double Calculate(const double dActual, const double dSetpoint); + double Calculate(const double dActual); + double Calculate(); + void EnableContinuousInput(const double dMinimumInput, const double dMaximumInput); + void DisableContinuousInput(); + void Reset(); + + ///////////////////////////////////////// + // Setters + ///////////////////////////////////////// + + void SetProportional(const double dKp); + void SetIntegral(const double dKi); + void SetDerivative(const double dKd); + void SetFeedforward(const double dKff); + void SetPID(const double dKp, const double dKi, const double dKd); + void SetPID(const double dKp, const double dKi, const double dKd, const double dKff); + void SetSetpoint(const double dSetpoint); + void SetMaxSetpointDifference(const double dMaxSetpointDifference); + void SetMaxIntegralEffort(const double dMaxIEffort); + void SetOutputLimits(const double dMinEffort, const double dMaxEffort); + void SetOutputLimits(const double dMaxMin); + void SetOutputRampRate(const double dOutputRampRate); + void SetOutputFilter(const double dStrength); + void SetDirection(const bool bReversed = false); + + ///////////////////////////////////////// + // Getters + ///////////////////////////////////////// + + double GetProportional() const; + double GetIntegral() const; + double GetDerivative() const; + double GetFeedforward() const; + double GetSetpoint() const; + double GetMaxSetpointDifference() const; + double GetMaxIntegralEffort() const; + double GetOutputRampRate() const; + double GetOutputFilter() const; + double GetContinuousInputEnabled() const; + bool GetReversed() const; + + private: + ///////////////////////////////////////// + // Declare public methods. + ///////////////////////////////////////// + + void CheckGainSigns(); + + ///////////////////////////////////////// + // Declare private member variables. + ///////////////////////////////////////// + + double m_dKp; // Proportional gain. + double m_dKi; // Integral gain. + double m_dKd; // Derivative gain. + double m_dKff; // Feedforward gain. + double m_dSetpoint; // Current control setpoint. + double m_dErrorSum; // Error accumulation. + double m_dMaxError; // Max allowed error. + double m_dMaxIEffort; // Max integral calculated term effort. + double m_dMinEffort; // Min output of the PID controller. + double m_dMaxEffort; // Max output of the PID controller. + double m_dLastActual; // The previous process variable input. + double m_dOutputRampRate; // The max rate of change of the controller output. + double m_dLastControlOutput; // The previous control output of the controller. + double m_dOutputFilter; // Strength of an exponential rolling sum filter. Used to reduce sharp oscillations. + double m_dMaxSetpointDifference; // Limit on how far the setpoint can be from the current position. + double m_dMinimumContinuousInput; // The minimum wraparound value of the input for the controller. + double m_dMaximumContinuousInput; // The maximum wraparound value of the input for the controller. + bool m_bControllerIsContinuous; // Whether of not the controller is working with a circular position range. Good for setpoints that use degrees. + bool m_bFirstCalculation; // Whether or not this is the first iteration of the control loop. + bool m_bReversed; // Operating direction of the controller. +}; +#endif diff --git a/src/algorithms/controllers/README.md b/src/algorithms/controllers/README.md new file mode 100755 index 00000000..1eb8cb4d --- /dev/null +++ b/src/algorithms/controllers/README.md @@ -0,0 +1,31 @@ +\dir src/algorithms/controllers + +# Controllers Directory + +The **Controllers** directory is dedicated to storing C++ files that define and implement advanced machine and motion controllers. Specifically, it contains files related to PID (Proportional-Integral-Derivative), LQR (Linear Quadratic Regulator), MPC (Model Predictive Controller), and the Stanley Controller. + +## Purpose + +The **Controllers** directory focuses on the logic and computational aspects of the project, providing implementations for a range of control algorithms and data structures. + +## Guidelines + +To maintain a well-organized directory, please adhere to the following guidelines: + +1. Each C++ file should contain one or more implementations of control algorithms or data structures. +2. Use clear and descriptive names for the files, reflecting the purpose and algorithm implemented. +3. Ensure that the files are properly documented with comments explaining the algorithm's logic, input/output specifications, and any relevant details. +4. Include a README file in any subdirectories within the **Controllers** directory to provide additional information if necessary. + +## Usage + +In the **Controllers** directory, you can expect to find files related to the following categories: + +1. **PID Controllers**: Files implementing the Proportional-Integral-Derivative controller, used for control loops in various applications. +2. **LQR Controllers**: Files that contain Linear Quadratic Regulator implementations, suitable for control system design and optimization. +3. **MPC Controllers**: Implementations of Model Predictive Controllers for advanced control in dynamic systems. +4. **Stanley Controller**: Files dedicated to the Stanley Controller for path tracking in autonomous vehicles. + +Each controller should have a `.h` and a `.cpp` file, not `.hpp`. + +Happy coding and control system development! diff --git a/src/drivers/DriveBoard.cpp b/src/drivers/DriveBoard.cpp index 3876e253..65c3f699 100755 --- a/src/drivers/DriveBoard.cpp +++ b/src/drivers/DriveBoard.cpp @@ -13,7 +13,6 @@ #include "../AutonomyConstants.h" #include "../AutonomyLogging.h" -#include "../algorithms/DifferentialDrive.hpp" #include "../util/NumberOperations.hpp" /****************************************************************************** @@ -26,8 +25,17 @@ DriveBoard::DriveBoard() { // Initialize member variables. - m_fTargetSpeedLeft = 0.0; - m_fTargetSpeedRight = 0.0; + m_stDrivePowers.dLeftDrivePower = 0.0; + m_stDrivePowers.dRightDrivePower = 0.0; + + // Configure PID controller for heading hold function. + m_pPID = new PIDController(constants::DRIVE_PID_PROPORTIONAL, constants::DRIVE_PID_INTEGRAL, constants::DRIVE_PID_DERIVATIVE); + m_pPID->SetMaxSetpointDifference(constants::DRIVE_PID_MAX_ERROR_PER_ITER); + m_pPID->SetMaxIntegralEffort(constants::DRIVE_PID_MAX_INTEGRAL_TERM); + m_pPID->SetOutputLimits(constants::DRIVE_PID_MAX_OUTPUT_EFFORT); + m_pPID->SetOutputRampRate(constants::DRIVE_PID_MAX_RAMP_RATE); + m_pPID->SetOutputFilter(constants::DRIVE_PID_OUTPUT_FILTER); + m_pPID->SetDirection(constants::DRIVE_PID_OUTPUT_REVERSED); } /****************************************************************************** @@ -41,72 +49,67 @@ DriveBoard::~DriveBoard() { // Stop drivetrain. this->SendStop(); + + // Delete dynamically allocated memory. + delete m_pPID; + + // Set dangling pointers to null. + m_pPID = nullptr; } /****************************************************************************** * @brief This method determines drive powers to make the Rover drive towards a * given heading at a given speed * - * @param fSpeed - The speed to drive at (-1 to 1) - * @param fAngle - The angle to drive towards. + * @param dGoalSpeed - The speed to drive at (-1 to 1) + * @param dGoalHeading - The angle to drive towards. (0 - 360) 0 is North. + * @param dActualHeading - * @param eKinematicsMethod - The kinematics model to use for differential drive control. Enum within DifferentialDrive.hpp - * @return std::array - 1D array of length 2 containing two values. (left power, right power) + * @return diffdrive::DrivePowers - A struct containing two values. (left power, right power) * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-21 ******************************************************************************/ -std::array DriveBoard::CalculateMove(const float fSpeed, const float fAngle, const DifferentialControlMethod eKinematicsMethod) +diffdrive::DrivePowers DriveBoard::CalculateMove(const double dGoalSpeed, + const double dGoalHeading, + const double dActualHeading, + const diffdrive::DifferentialControlMethod eKinematicsMethod) { - // Create instance variables. - std::array aDrivePowers; - - // Check what kinematics model we should use. - switch (eKinematicsMethod) - { - case eArcadeDrive: aDrivePowers = diffdrive::CalculateArcadeDrive(double(fSpeed), double(fAngle), constants::DRIVE_MIN_POWER); break; - case eCurvatureDrive: - aDrivePowers = diffdrive::CalculateCurvatureDrive(double(fSpeed), - double(fAngle), - constants::DRIVE_CURVATURE_KINEMATICS_ALLOW_TURN_WHILE_STOPPED, - constants::DRIVE_SQUARE_CONTROL_INPUTS); - break; - } - - // Update member variables with new targets speeds. Adjust to match power range. - m_fTargetSpeedLeft = double(aDrivePowers[0]); - m_fTargetSpeedRight = double(aDrivePowers[1]); + // Calculate the drive powers from the current heading, goal heading, and goal speed. + m_stDrivePowers = diffdrive::CalculateMotorPowerFromHeading(dGoalSpeed, dGoalHeading, dActualHeading, eKinematicsMethod, *m_pPID); // Submit logger message. - LOG_DEBUG(logging::g_qSharedLogger, "Driving at: ({}, {})", m_fTargetSpeedLeft, m_fTargetSpeedRight); + LOG_DEBUG(logging::g_qSharedLogger, "Driving at: ({}, {})", m_stDrivePowers.dLeftDrivePower, m_stDrivePowers.dRightDrivePower); - return {m_fTargetSpeedLeft, m_fTargetSpeedRight}; + return m_stDrivePowers; } /****************************************************************************** * @brief Sets the left and right drive powers of the drive board. * - * @param fLeftSpeed - Left drive speed (-1 to 1) - * @param fRightSpeed - Right drive speed (-1 to 1) + * @param dLeftSpeed - Left drive speed (-1 to 1) + * @param dRightSpeed - Right drive speed (-1 to 1) * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-21 ******************************************************************************/ -void DriveBoard::SendDrive(float fLeftSpeed, float fRightSpeed) +void DriveBoard::SendDrive(double dLeftSpeed, double dRightSpeed) { // Limit input values. - fLeftSpeed = std::clamp(fLeftSpeed, -1.0f, 1.0f); - fRightSpeed = std::clamp(fRightSpeed, -1.0f, 1.0f); + dLeftSpeed = std::clamp(dLeftSpeed, -1.0, 1.0); + dRightSpeed = std::clamp(dRightSpeed, -1.0, 1.0); // Update member variables with new target speeds. - m_fTargetSpeedLeft = fLeftSpeed; - m_fTargetSpeedRight = fRightSpeed; + m_stDrivePowers.dLeftDrivePower = dLeftSpeed; + m_stDrivePowers.dRightDrivePower = dRightSpeed; - // Remap -1.0 - 1.0 range to drive power range defined in constants. This is so that the driveboard/rovecomm can understand our input. - m_fTargetSpeedLeft = numops::MapRange(m_fTargetSpeedLeft, -1.0f, 1.0f, constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER); - m_fTargetSpeedRight = numops::MapRange(m_fTargetSpeedRight, -1.0f, 1.0f, constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER); - // Limit the power to max and min effort defined in constants. - m_fTargetSpeedLeft = std::clamp(m_fTargetSpeedLeft, constants::DRIVE_MIN_EFFORT, constants::DRIVE_MAX_EFFORT); - m_fTargetSpeedRight = std::clamp(m_fTargetSpeedRight, constants::DRIVE_MIN_EFFORT, constants::DRIVE_MAX_EFFORT); + // TODO: Uncomment once RoveComm is implemented. This is commented to gid rid of unused variable warnings. + // // Remap -1.0 - 1.0 range to drive power range defined in constants. This is so that the driveboard/rovecomm can understand our input. + // float fDriveBoardLeftPower = numops::MapRange(float(dLeftSpeed), -1.0f, 1.0f, constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER); + // float fDriveBoardRightPower = numops::MapRange(float(dRightSpeed), -1.0f, 1.0f, constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER); + // // Limit the power to max and min effort defined in constants. + // fDriveBoardLeftPower = std::clamp(float(dLeftSpeed), constants::DRIVE_MIN_EFFORT, constants::DRIVE_MAX_EFFORT); + // fDriveBoardRightPower = std::clamp(float(dRightSpeed), constants::DRIVE_MIN_EFFORT, constants::DRIVE_MAX_EFFORT); // Send drive command over RoveComm to drive board. // TODO: Add RoveComm sendpacket. @@ -122,9 +125,23 @@ void DriveBoard::SendDrive(float fLeftSpeed, float fRightSpeed) void DriveBoard::SendStop() { // Update member variables with new target speeds. - m_fTargetSpeedLeft = 0.0; - m_fTargetSpeedRight = 0.0; + m_stDrivePowers.dLeftDrivePower = 0.0; + m_stDrivePowers.dRightDrivePower = 0.0; // Send drive command over RoveComm to drive board. // TODO: Add RoveComm sendpacket. } + +/****************************************************************************** + * @brief Accessor for the current drive powers of the robot. + * + * @return diffdrive::DrivePowers - A struct containing the left and right drive power of the drivetrain. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-20 + ******************************************************************************/ +diffdrive::DrivePowers DriveBoard::GetDrivePowers() const +{ + // Return the current drive powers. + return m_stDrivePowers; +} diff --git a/src/drivers/DriveBoard.h b/src/drivers/DriveBoard.h index 79fa6cb8..0f603a9b 100755 --- a/src/drivers/DriveBoard.h +++ b/src/drivers/DriveBoard.h @@ -12,6 +12,9 @@ #ifndef DRIVEBOARD_H #define DRIVEBOARD_H +#include "../algorithms/DifferentialDrive.hpp" +#include "../algorithms/controllers/PIDController.h" + #include /****************************************************************************** @@ -29,41 +32,35 @@ class DriveBoard // Declare private member variables. ///////////////////////////////////////// - float m_fTargetSpeedLeft; - float m_fTargetSpeedRight; + diffdrive::DrivePowers m_stDrivePowers; // Struct used to store the left and right drive powers of the robot. + PIDController* m_pPID; // The PID controller used for drive towards a heading. public: ///////////////////////////////////////// - // Declare public enums that are specific to and used withing this class. + // Declare public enums that are specific to and used within this class. ///////////////////////////////////////// - // Enumerator used to specify what method of drive control to use. - enum DifferentialControlMethod - { - eArcadeDrive, // Typical drive control method for flightsticks. Uses speed and turn input to determine drive powers. - eCurvatureDrive // Similar to arcade drive with flightsticks, but the current turning speed of the robot is dampened when moving fast. - }; - ///////////////////////////////////////// // Declare public methods and member variables. ///////////////////////////////////////// DriveBoard(); ~DriveBoard(); - std::array CalculateMove(const float fSpeed, const float fAngle, const DifferentialControlMethod eKinematicsMethod); - void SendDrive(float fLeftSpeed, float fRightSpeed); + diffdrive::DrivePowers CalculateMove(const double dGoalSpeed, + const double dGoalHeading, + const double dActualHeading, + const diffdrive::DifferentialControlMethod eKinematicsMethod); + void SendDrive(double dLeftSpeed, double dRightSpeed); void SendStop(); ///////////////////////////////////////// // Setters ///////////////////////////////////////// - void SetMaxDrivePower(); - ///////////////////////////////////////// // Getters ///////////////////////////////////////// - float GetMaxDrivePower() const; + diffdrive::DrivePowers GetDrivePowers() const; }; #endif diff --git a/src/drivers/NavigationBoard.cpp b/src/drivers/NavigationBoard.cpp index 31da3a70..731d2fd5 100755 --- a/src/drivers/NavigationBoard.cpp +++ b/src/drivers/NavigationBoard.cpp @@ -39,6 +39,8 @@ NavigationBoard::~NavigationBoard() {} ******************************************************************************/ void NavigationBoard::ProcessIMUData(geoops::IMUData stPacket) { + // Acquire write lock for writing to IMU struct. + std::unique_lock lkIMUProcessLock(m_muOrientationMutex); // Update member variables attributes. m_stOrientation.dPitch = stPacket.dPitch; m_stOrientation.dRoll = stPacket.dRoll; @@ -58,6 +60,8 @@ void NavigationBoard::ProcessIMUData(geoops::IMUData stPacket) ******************************************************************************/ void NavigationBoard::ProcessGPSData(geoops::GPSCoordinate stPacket) { + // Acquire write lock for writing to IMU struct. + std::unique_lock lkGPSProcessLock(m_muLocationMutex); // Submit logger message. LOG_DEBUG(logging::g_qSharedLogger, "Incoming GPS Data: ({} lat, {} lon, {} alt, {} acc)", @@ -65,6 +69,9 @@ void NavigationBoard::ProcessGPSData(geoops::GPSCoordinate stPacket) stPacket.dLongitude, stPacket.dAltitude, stPacket.d2DAccuracy); + + // Convert to UTM coord and store in member variable. + m_stLocation = geoops::ConvertGPSToUTM(stPacket); } /****************************************************************************** @@ -75,8 +82,10 @@ void NavigationBoard::ProcessGPSData(geoops::GPSCoordinate stPacket) * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ -geoops::IMUData NavigationBoard::GetIMUData() const +geoops::IMUData NavigationBoard::GetIMUData() { + // Acquire read lock for getting IMU struct. + std::shared_lock lkIMUProcessLock(m_muOrientationMutex); // Return the orientation struct member variable. return m_stOrientation; } @@ -89,10 +98,12 @@ geoops::IMUData NavigationBoard::GetIMUData() const * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ -geoops::GPSCoordinate NavigationBoard::GetGPSData() const +geoops::GPSCoordinate NavigationBoard::GetGPSData() { - // TODO: Return empty coordinate for now. - return geoops::GPSCoordinate(); + // Acquire read lock for getting UTM struct. + std::shared_lock lkIMUProcessLock(m_muLocationMutex); + // Convert the currently stored UTM coord to GPS and return. + return geoops::ConvertUTMToGPS(m_stLocation); } /****************************************************************************** @@ -104,8 +115,9 @@ geoops::GPSCoordinate NavigationBoard::GetGPSData() const * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ -geoops::UTMCoordinate NavigationBoard::GetUTMData() const +geoops::UTMCoordinate NavigationBoard::GetUTMData() { - // TODO: Return empty coordinate for now. - return geoops::UTMCoordinate(); + // Acquire read lock for getting UTM struct. + std::shared_lock lkIMUProcessLock(m_muLocationMutex); + return m_stLocation; } diff --git a/src/drivers/NavigationBoard.h b/src/drivers/NavigationBoard.h index e7f3b266..60bd6ddb 100755 --- a/src/drivers/NavigationBoard.h +++ b/src/drivers/NavigationBoard.h @@ -13,6 +13,8 @@ #include "../util/GeospatialOperations.hpp" +#include + /****************************************************************************** * @brief This class handles communication with the navigation board on the rover * by sending RoveComm packets over the network. @@ -31,10 +33,9 @@ class NavigationBoard ///////////////////////////////////////// // Declare public methods and member variables. ///////////////////////////////////////// + NavigationBoard(); ~NavigationBoard(); - void ProcessIMUData(geoops::IMUData stPacket); - void ProcessGPSData(geoops::GPSCoordinate stPacket); ///////////////////////////////////////// // Setters @@ -43,18 +44,29 @@ class NavigationBoard ///////////////////////////////////////// // Getters ///////////////////////////////////////// - geoops::IMUData GetIMUData() const; - geoops::GPSCoordinate GetGPSData() const; - geoops::UTMCoordinate GetUTMData() const; + + geoops::IMUData GetIMUData(); + geoops::GPSCoordinate GetGPSData(); + geoops::UTMCoordinate GetUTMData(); private: + ///////////////////////////////////////// + // Declare private methods. + ///////////////////////////////////////// + + void ProcessIMUData(geoops::IMUData stPacket); + void ProcessGPSData(geoops::GPSCoordinate stPacket); + ///////////////////////////////////////// // Declare private member variables. ///////////////////////////////////////// - geoops::UTMCoordinate m_stLocation; - geoops::IMUData m_stOrientation; - // TODO: RoveComm Node + geoops::UTMCoordinate m_stLocation; // Store current global position in UTM format. + geoops::IMUData m_stOrientation; // Store current IMU orientation. + std::shared_mutex m_muLocationMutex; // Mutex for acquiring read and write lock on location member variable. + std::shared_mutex m_muOrientationMutex; // Mutex for acquiring read and write lock on orientation member variable. + + // TODO: RoveComm Node // TODO: RoveComm Callback -> IMU Data // TODO: RoveComm Callback -> GPS Data diff --git a/src/util/GeospatialOperations.hpp b/src/util/GeospatialOperations.hpp index 690eeb8a..70f5b922 100644 --- a/src/util/GeospatialOperations.hpp +++ b/src/util/GeospatialOperations.hpp @@ -12,6 +12,9 @@ #ifndef GEOSPATIAL_OPERATIONS_HPP #define GEOSPATIAL_OPERATIONS_HPP +#include +#include + /****************************************************************************** * @brief Namespace containing functions related to operations on global position number * systems and other datatypes. @@ -22,6 +25,29 @@ ******************************************************************************/ namespace geoops { + ///////////////////////////////////////// + // Declare public variables. + ///////////////////////////////////////// + // Constants retrieved from: https://nssdc.gsfc.nasa.gov/planetary/factsheet/earthfact.html + const double dEarthAverageRadius = 6371000.0; // Earth's radius in meters. + const double dEarthEllipsoidFlattening = 0.003353; // The flattening factor of the earth due to its spin. + + /****************************************************************************** + * @brief This struct is used to store the distance and arc length for a calculated + * geographic distance. Storing these values in a struct allows for easy + * handling and access to said variables. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-13 + ******************************************************************************/ + struct GeoDistance + { + public: + // Define public struct attributes. + double dDistanceMeters; + double dArcLengthDegrees; + }; /****************************************************************************** * @brief This struct stores/contains information about orientation. @@ -83,96 +109,47 @@ namespace geoops { public: // Declare struct public attributes - double dLatitude; - double dLongitude; - double dAltitude; - double d2DAccuracy; - double d3DAccuracy; + double dLatitude; // The geographic latitude in degrees, typically within the range [-90, 90]. + double dLongitude; // The geographic longitude in degrees, typically within the range [-180, 180]. + double dAltitude; // The elevation above sea level in meters. + double d2DAccuracy; // The horizontal accuracy of the GPS coordinates in meters. + double d3DAccuracy; // The three-dimensional accuracy, including altitude, in meters. + double dMeridianConvergence; // The angle between true north and grid north at the given location in degrees. Positive in eastern direction. + double dScale; // The scale factor applied to the UTM coordinates for map projection. ///////////////////////////////////////// // Declare public methods. ///////////////////////////////////////// - - /****************************************************************************** - * @brief Construct a new GPSCoordinate object. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ - GPSCoordinate() - { - // Initialize member variables to default values. - this->dLatitude = 0.0; - this->dLongitude = 0.0; - this->dAltitude = 0.0; - this->d2DAccuracy = -1.0; - this->d3DAccuracy = -1.0; - } - - /****************************************************************************** - * @brief Construct a new GPSCoordinate object. - * - * @param dLatitude - The latitude of the GPS coordinate. - * @param dLongitude - The longitude of the GPS coordinate. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ - GPSCoordinate(double dLatitude, double dLongitude) - { - // Initialize member variables with given values. - this->dLatitude = dLatitude; - this->dLongitude = dLongitude; - - // Use default values for everything else. - dAltitude = 0.0; - d2DAccuracy = -1.0; - d3DAccuracy = -1.0; - } - /****************************************************************************** * @brief Construct a new GPSCoordinate object. * - * @param dLatitude - The latitude of the GPS coordinate. - * @param dLongitude - The longitude of the GPS coordinate. - * @param Altitude - The altitude of the GPS coordinate. + * @param dLatitude - The geographic latitude in degrees, typically within the range [-90, 90]. + * @param dLongitude - The geographic longitude in degrees, typically within the range [-180, 180]. + * @param dAltitude - The elevation above sea level in meters. + * @param d2DAccuracy - The horizontal accuracy of the GPS coordinates in meters. + * @param d3DAccuracy - The three-dimensional accuracy, including altitude, in meters. + * @param dMeridianConvergence - The angle between true north and grid north at the given location in degrees. Positive in eastern direction. + * @param dScale - The scale factor applied to the UTM coordinates for map projection. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ - GPSCoordinate(double dLatitude, double dLongitude, double dAltitude) + GPSCoordinate(double dLatitude = 0.0, + double dLongitude = 0.0, + double dAltitude = 0.0, + double d2DAccuracy = -1.0, + double d3DAccuracy = -1.0, + double dMeridianConvergence = -1.0, + double dScale = 0.0) { // Initialize member variables with given values. - this->dLatitude = dLatitude; - this->dLongitude = dLongitude; - this->dAltitude = dAltitude; - - // Use default values for everything else. - d2DAccuracy = -1.0; - d2DAccuracy = -1.0; - } - - /****************************************************************************** - * @brief Construct a new GPSCoordinate object. - * - * @param dLatitude - The latitude of the GPS coordinate. - * @param dLongitude - The longitude of the GPS coordinate. - * @param Altitude - The altitude of the GPS coordinate. - * @param d2DAccuracy - The accuracy of the lat/lon - * @param d3DAccuracy - The accuracy of the lat/lon/alt - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ - GPSCoordinate(double dLatitude, double dLongitude, double dAltitude, double d2DAccuracy, double d3DAccuracy) - { - // Initialize member variables with given values. - this->dLatitude = dLatitude; - this->dLongitude = dLongitude; - this->dAltitude = dAltitude; - this->d2DAccuracy = d2DAccuracy; - this->d3DAccuracy = d3DAccuracy; + this->dLatitude = dLatitude; + this->dLongitude = dLongitude; + this->dAltitude = dAltitude; + this->d2DAccuracy = d2DAccuracy; + this->d3DAccuracy = d3DAccuracy; + this->dMeridianConvergence = dMeridianConvergence; + this->dScale = dScale; } }; @@ -187,102 +164,168 @@ namespace geoops { public: // Declare struct public attributes. - double dEasting; - double dNorthing; - double dAltitude; - int nZone; - double d2DAccuracy; - double d3DAccuracy; + double dEasting; // The eastward displacement from the UTM zone's central meridian in meters. + double dNorthing; // The northward displacement from the equator in meters. + int nZone; // The UTM zone number identifying the region on the Earth's surface. + bool bWithinNorthernHemisphere; // Indicates whether the coordinate is located in the northern hemisphere. + double dAltitude; // The elevation above sea level in meters. + double d2DAccuracy; // The horizontal accuracy of the UTM coordinates in meters. + double d3DAccuracy; // The three-dimensional accuracy, including altitude, in meters. + double dMeridianConvergence; // The angle between true north and grid north at the given location in degrees. Positive in eastern direction. + double dScale; // The scale factor applied to the UTM coordinates for map projection. ///////////////////////////////////////// // Declare public methods. ///////////////////////////////////////// + /****************************************************************************** - * @brief Construct a new UTMCoordinate object. + * @brief Default Construct a new UTMCoordinate object. * * * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 + * @date 2023-10-12 ******************************************************************************/ UTMCoordinate() { - // Initialize member variables to default values. - this->dEasting = 0.0; - this->dNorthing = 0.0; - this->dAltitude = 0.0; - this->nZone = 0; - this->d2DAccuracy = -1.0; - this->d3DAccuracy = -1.0; + // Initialize member variables. + this->dEasting = 0.0; + this->dNorthing = 0.0; + this->nZone = 0; + this->bWithinNorthernHemisphere = true; + this->dAltitude = 0.0; + this->d2DAccuracy = -1.0; + this->d3DAccuracy = -1.0; + this->dMeridianConvergence = 0.0; + this->dScale = 0.0; } /****************************************************************************** * @brief Construct a new UTMCoordinate object. * - * @param dEasting - The Easting of the UTM coordinate. - * @param dNorthing - The Northing of the UTM coordinate. + * @param dEasting - The eastward displacement from the UTM zone's central meridian in meters. + * @param dNorthing - The northward displacement from the equator in meters. + * @param nZone - The UTM zone number identifying the region on the Earth's surface. + * @param bWithinNorthernHemisphere - Indicates whether the coordinate is located in the northern hemisphere. + * @param dAltitude - The elevation above sea level in meters. + * @param d2DAccuracy - The horizontal accuracy of the UTM coordinates in meters. + * @param d3DAccuracy - The three-dimensional accuracy, including altitude, in meters. + * @param dMeridianConvergence - The angle between true north and grid north at the given location in degrees. Positive in eastern direction. + * @param dScale - The scale factor applied to the UTM coordinates for map projection. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ - UTMCoordinate(double dEasting, double dNorthing) + UTMCoordinate(double dEasting, + double dNorthing, + int nZone, + bool bWithinNorthernHemisphere, + double dAltitude = 0.0, + double d2DAccuracy = -1.0, + double d3DAccuracy = -1.0, + double dMeridianConvergence = -1.0, + double dScale = 0.0) { // Initialize member variables with given values. - this->dEasting = dEasting; - this->dNorthing = dNorthing; - - // Use default values for everything else. - dAltitude = 0.0; - nZone = 0; - d2DAccuracy = -1.0; - d3DAccuracy = -1.0; + this->dEasting = dEasting; + this->dNorthing = dNorthing; + this->nZone = nZone; + this->bWithinNorthernHemisphere = bWithinNorthernHemisphere; + this->dAltitude = dAltitude; + this->d2DAccuracy = d2DAccuracy; + this->d3DAccuracy = d3DAccuracy; + this->dMeridianConvergence = dMeridianConvergence; + this->dScale = dScale; } + }; - /****************************************************************************** - * @brief Construct a new UTMCoordinate object. - * - * @param dEasting - The Easting of the UTM coordinate. - * @param dNorthing - The Northing of the UTM coordinate. - * @param dAltitude - The Altitude of the UTM coordinate. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ - UTMCoordinate(double dEasting, double dNorthing, double dAltitude) - { - // Initialize member variables with given values. - this->dEasting = dEasting; - this->dNorthing = dNorthing; - this->dAltitude = dAltitude; + /****************************************************************************** + * @brief Given a GPS coordinate, convert to UTM and create a new UTMCoordinate object. + * + * @param stGPSCoord - The struct containing the GPS coordinate data. + * @return UTMCoordinate - The UTM coordinate corresponding to the given GPS coordinate. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + ******************************************************************************/ + inline UTMCoordinate ConvertGPSToUTM(GPSCoordinate stGPSCoord) + { + // Create instance variables. + UTMCoordinate stConvertCoord; - // Use default values for everything else. - nZone = 0; - d2DAccuracy = -1.0; - d2DAccuracy = -1.0; - } + // Get data out of the GPS coord and repackage it into the UTM struct. + stConvertCoord.d2DAccuracy = stGPSCoord.d2DAccuracy; + stConvertCoord.d3DAccuracy = stGPSCoord.d3DAccuracy; + stConvertCoord.dAltitude = stGPSCoord.dAltitude; + GeographicLib::UTMUPS::Forward(stGPSCoord.dLatitude, + stGPSCoord.dLongitude, + stConvertCoord.nZone, + stConvertCoord.bWithinNorthernHemisphere, + stConvertCoord.dEasting, + stConvertCoord.dNorthing, + stConvertCoord.dMeridianConvergence, + stConvertCoord.dScale); - /****************************************************************************** - * @brief Construct a new UTMCoordinate object. - * - * @param dEasting - The Easting of the UTM coordinate. - * @param dNorthing - The Northing of the UTM coordinate. - * @param dAltitude - The Altitude of the UTM coordinate. - * @param nZone - The zone of the UTM coordinate. - * @param d2DAccuracy - The accuracy of the east / north - * @param d3DAccuracy - The accuracy of the east / north / alt - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ - UTMCoordinate(double dEasting, double dNorthing, double dAltitude, int nZone, double d2DAccuracy, double d3DAccuracy) - { - // Initialize member variables with given values. - this->dEasting = dEasting; - this->dNorthing = dNorthing; - this->dAltitude = dAltitude; - this->nZone = nZone; - this->d2DAccuracy = d2DAccuracy; - this->d3DAccuracy = d3DAccuracy; - } - }; + // Return the converted UTM coordinate. + return stConvertCoord; + } + + /****************************************************************************** + * @brief Given a UTM coordinate, convert to GPS and create a new GPSCoordinate object. + * + * @param stUTMCoord - The struct containing the UTM coordinate data. + * @return GPSCoordinate - The GPS coordinate corresponding to the given UTM coordinate. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + ******************************************************************************/ + inline GPSCoordinate ConvertUTMToGPS(UTMCoordinate stUTMCoord) + { + // Create instance variables. + GPSCoordinate stConvertCoord; + + // Get data out of the UTM coord and repackage it into the GPS struct. + stConvertCoord.d2DAccuracy = stUTMCoord.d2DAccuracy; + stConvertCoord.d3DAccuracy = stUTMCoord.d3DAccuracy; + stConvertCoord.dAltitude = stUTMCoord.dAltitude; + GeographicLib::UTMUPS::Reverse(stUTMCoord.nZone, + stUTMCoord.bWithinNorthernHemisphere, + stUTMCoord.dEasting, + stUTMCoord.dNorthing, + stConvertCoord.dLatitude, + stConvertCoord.dLongitude, + stConvertCoord.dMeridianConvergence, + stConvertCoord.dScale); + + // Return the converted UTM coordinate. + return stConvertCoord; + } + + /****************************************************************************** + * @brief The shortest path between two points on an ellipsoid at (lat1, lon1) and (lat2, lon2) is called the geodesic. + * Given those two points create an ellipsoid with earth's characteristics and find the distance between them. + * + * @param stCoord1 - The first GPS coordinate. + * @param stCoord2 - The second GPS coordinate. + * @return GeoDistance - Struct containing the distance in meters and arc length degrees. + * + * @see https://geographiclib.sourceforge.io/C++/doc/classGeographicLib_1_1Geodesic.html#ae66c9cecfcbbcb1da52cb408e69f65de + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-13 + ******************************************************************************/ + inline GeoDistance CalculateGeoDistance(const GPSCoordinate stCoord1, const GPSCoordinate stCoord2) + { + // Create instance variables. + GeoDistance stDistances; + + // Construct a geodesic with earth characteristics. + GeographicLib::Geodesic geGeodesic(dEarthAverageRadius, dEarthEllipsoidFlattening); + + // Solve the inverse geodesic. + stDistances.dArcLengthDegrees = geGeodesic.Inverse(stCoord1.dLatitude, stCoord1.dLongitude, stCoord2.dLatitude, stCoord2.dLongitude, stDistances.dDistanceMeters); + + // Return result distance. + return stDistances; + } } // namespace geoops #endif diff --git a/src/util/NumberOperations.hpp b/src/util/NumberOperations.hpp index 12be446b..516428ea 100755 --- a/src/util/NumberOperations.hpp +++ b/src/util/NumberOperations.hpp @@ -13,6 +13,7 @@ #define NUMBER_OPERATIONS_HPP #include +#include /****************************************************************************** * @brief Namespace containing functions related to operations on numbers and @@ -24,6 +25,53 @@ ******************************************************************************/ namespace numops { + /****************************************************************************** + * @brief Clamps a given value from going above or below a given threshold. + * + * @tparam T - Template argument for given value type. + * @param tValue - The value to clamp. + * @param tMin - Minimum value quantity. + * @param tMax - Maximum value quantity. + * @return T - The clamped value. + * + * @author Eli Byrd (edbgkk@mst.edu), ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-06-20 + ******************************************************************************/ + template + inline T Clamp(T tValue, T tMin, T tMax) + { + return std::max(std::min(tMax, tValue), tMin); + } + + /****************************************************************************** + * @brief Checks if a given value is between the given maximum and minimum ranges. + * + * @tparam T - Template argument for given value type. + * @param tValue - The value to check. + * @param tMin - The minimum bound for the value to be valid. + * @param tMax - The maximum bound for the value to be valid. + * @return true - The value is within the bounds. + * @return false - The value is not within the bounds. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-16 + ******************************************************************************/ + template + inline bool Bounded(T tValue, T tMin, T tMax, const bool bInclusive = true) + { + // Check if value is inclusive or not. + if (bInclusive) + { + // Return true if the given value is valid. + return (tValue >= tMin) && (tValue <= tMax); + } + else + { + // Return true if the given value is valid. + return (tValue > tMin) && (tValue < tMax); + } + } + /****************************************************************************** * @brief Maps a value to a new range given the old range. * @@ -60,5 +108,33 @@ namespace numops return tNewMinimum + tScaledValue * tNewValueRange; } + /****************************************************************************** + * @brief Calculates the modulus of an input. + * + * @tparam T - Template value specifying the type of the number to find modulus of. + * @param tValue - Input value to wrap. + * @param tMinValue - The minimum value expected from the input. + * @param tMaxValue - The maximum value expected from the input. + * @return constexpr T - The wrapped value. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ + template + inline constexpr T InputModulus(T tValue, T tMinValue, T tMaxValue) + { + // Determine the correct modulus number. + T tModulus = tMaxValue - tMinValue; + + // Wrap input if it's above the maximum input. + int nNumMax = (tValue - tMinValue) / tModulus; + tValue -= nNumMax * tModulus; + // Wrap input if it's below the minimum input. + int nNumMin = (tValue - tMaxValue) / tModulus; + tValue -= nNumMin * tModulus; + + // Return wrapped number. + return tValue; + } } // namespace numops #endif diff --git a/tests/Unit/src/algorithms/DifferentialDrive.cc b/tests/Unit/src/algorithms/DifferentialDrive.cc new file mode 100644 index 00000000..404bc4e2 --- /dev/null +++ b/tests/Unit/src/algorithms/DifferentialDrive.cc @@ -0,0 +1,100 @@ +/****************************************************************************** + * @brief Unit test for DifferentialDrive algorithm class. + * + * @file DifferentialDrive.cc + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + * + * @copyright Copyright MRDT 2023 - All Rights Reserved + ******************************************************************************/ + +#include +#include +#include + +#include "../../../../src/algorithms/DifferentialDrive.hpp" + +/****************************************************************************** + * @brief Test DifferentialDrive TankDrive functionality. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + ******************************************************************************/ +TEST(DifferentialDriveTest, TankDrive) +{ + // Create array for storing input and expect output values. + const int nTestValuesLength = 11; + const double aLeftSpeedInput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, 1.0, 0.5, 0.0, -0.5, -1.0, 1.5}; + const double aRightSpeedInput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, -1.0, -0.5, 0.0, 0.5, 1.0, 1.5}; + const double aLeftSpeedOutput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, 1.0, 0.5, 0.0, -0.5, -1.0, 1.0}; + const double aRightSpeedOutput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, -1.0, -0.5, 0.0, 0.5, 1.0, 1.0}; + + // Loop through each value and compare inputs and outputs. + for (int nIter = 0; nIter < nTestValuesLength; ++nIter) + { + // Calculate drive powers. + auto [dLeftDrivePower, dRightDrivePower] = diffdrive::CalculateTankDrive(aLeftSpeedInput[nIter], aRightSpeedInput[nIter]); + + // Check that the expected output values were calculated. + EXPECT_NEAR(aLeftSpeedOutput[nIter], dLeftDrivePower, 0.01); // Left output check. + EXPECT_NEAR(aRightSpeedOutput[nIter], dRightDrivePower, 0.01); // Right output check. + } +} + +/****************************************************************************** + * @brief Test DifferentialDrive ArcadeDrive functionality. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + ******************************************************************************/ +TEST(DifferentialDriveTest, ArcadeDrive) +{ + // Create array for storing input and expect output values. + const int nTestValuesLength = 11; + const double aSpeedInput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.3, 1.5}; + const double aRotationInput[nTestValuesLength] = {0.0, 0.0, 0.0, 0.0, 0.0, -1.0, -0.5, 0.5, 1.0, 1.0, 1.5}; + const double aLeftSpeedOutput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, -1.0, -0.5, 0.5, 1.0, 1.0, 1.0}; + const double aRightSpeedOutput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, 1.0, 0.5, -0.5, -1.0, -0.53, 0.0}; + + // Loop through each value and compare inputs and outputs. + for (int nIter = 0; nIter < nTestValuesLength; ++nIter) + { + // Calculate drive powers. + auto [dLeftDrivePower, dRightDrivePower] = diffdrive::CalculateArcadeDrive(aSpeedInput[nIter], aRotationInput[nIter]); + + // Check that the expected output values were calculated. + EXPECT_NEAR(aLeftSpeedOutput[nIter], dLeftDrivePower, 0.02); // Left output check. + EXPECT_NEAR(aRightSpeedOutput[nIter], dRightDrivePower, 0.02); // Right output check. + } +} + +/****************************************************************************** + * @brief Test DifferentialDrive CurvatureDrive functionality. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-13 + ******************************************************************************/ +TEST(DifferentialDriveTest, CurvatureDrive) +{ + // Create array for storing input and expect output values. + const int nTestValuesLength = 11; + const double aSpeedInput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.3, 1.5}; + const double aRotationInput[nTestValuesLength] = {0.0, 0.0, 0.0, 0.0, 0.0, -1.0, -0.5, 0.5, 1.0, 1.0, 1.5}; + const bool aAllowTurnInPlaceInput[nTestValuesLength] = {true, true, true, true, true, true, true, true, true, true, true}; + const double aLeftSpeedOutput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, -1.0, -0.5, 0.5, 1.0, 0.59, 1.0}; + const double aRightSpeedOutput[nTestValuesLength] = {-1.0, -0.5, 0.0, 0.5, 1.0, 1.0, 0.5, -0.5, -1.0, 0.0, 0.0}; + + // Loop through each value and compare inputs and outputs. + for (int nIter = 0; nIter < nTestValuesLength; ++nIter) + { + // Calculate drive powers. + auto [dLeftDrivePower, dRightDrivePower] = diffdrive::CalculateCurvatureDrive(aSpeedInput[nIter], aRotationInput[nIter], aAllowTurnInPlaceInput[nIter]); + + // Check that the expected output values were calculated. + EXPECT_NEAR(aLeftSpeedOutput[nIter], dLeftDrivePower, 0.02); // Left output check. + EXPECT_NEAR(aRightSpeedOutput[nIter], dRightDrivePower, 0.02); // Right output check. + } +} diff --git a/tests/Unit/src/util/GeospatialOperations.cc b/tests/Unit/src/util/GeospatialOperations.cc new file mode 100644 index 00000000..3f4ed38f --- /dev/null +++ b/tests/Unit/src/util/GeospatialOperations.cc @@ -0,0 +1,147 @@ +/****************************************************************************** + * @brief Unit test for GeospatialOperations utility class. + * + * @file GeospatialOperations.cc + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + * + * @copyright Copyright MRDT 2023 - All Rights Reserved + ******************************************************************************/ + +#include + +#include "../../../../src/util/GeospatialOperations.hpp" + +/****************************************************************************** + * @brief Test the functionality of the ConvertGPStoUTM function. Also tests functionality + * of GPS and UTM Coordinate structs. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + ******************************************************************************/ +TEST(GeoOpsTest, ConvertGPStoUTM) +{ + // Initialize coordinates. + geoops::GPSCoordinate stGPSRollaCoordinate(37.951766, -91.778187); + geoops::GPSCoordinate stGPSMDRSCoordinate(38.406267, -110.791997); + geoops::GPSCoordinate stGPSAustraliaCoordinate(-23.249790, 141.016173); + geoops::GPSCoordinate stGPSYucatanCoordinate(20.402472, -89.286368); + + // Convert GPS coordinates to UTM coordinate. + geoops::UTMCoordinate stUTMRollaCoordinate = geoops::ConvertGPSToUTM(stGPSRollaCoordinate); + geoops::UTMCoordinate stUTMMDRSCoordinate = geoops::ConvertGPSToUTM(stGPSMDRSCoordinate); + geoops::UTMCoordinate stUTMAustraliaCoordinate = geoops::ConvertGPSToUTM(stGPSAustraliaCoordinate); + geoops::UTMCoordinate stUTMYucatanCoordinate = geoops::ConvertGPSToUTM(stGPSYucatanCoordinate); + + // Check Rolla conversion. + EXPECT_NEAR(stUTMRollaCoordinate.dEasting, 607344.14, 0.01); + EXPECT_NEAR(stUTMRollaCoordinate.dNorthing, 4201167.33, 0.01); + EXPECT_EQ(stUTMRollaCoordinate.nZone, 15); + EXPECT_NEAR(stUTMRollaCoordinate.dMeridianConvergence, 0.7, 0.1); + EXPECT_TRUE(stUTMRollaCoordinate.bWithinNorthernHemisphere); + + // Check Mars Desert Research Station conversion. + EXPECT_NEAR(stUTMMDRSCoordinate.dEasting, 518160.91, 0.01); + EXPECT_NEAR(stUTMMDRSCoordinate.dNorthing, 4250913.23, 0.01); + EXPECT_EQ(stUTMMDRSCoordinate.nZone, 12); + EXPECT_NEAR(stUTMMDRSCoordinate.dMeridianConvergence, 0.1, 0.1); + EXPECT_TRUE(stUTMMDRSCoordinate.bWithinNorthernHemisphere); + + // Check Australia conversion. + EXPECT_NEAR(stUTMAustraliaCoordinate.dEasting, 501654.37, 0.01); + EXPECT_NEAR(stUTMAustraliaCoordinate.dNorthing, 7428828.03, 0.01); + EXPECT_EQ(stUTMAustraliaCoordinate.nZone, 54); + EXPECT_NEAR(stUTMAustraliaCoordinate.dMeridianConvergence, 0.0, 0.1); + EXPECT_FALSE(stUTMAustraliaCoordinate.bWithinNorthernHemisphere); + + // Check Yucatan conversion. + EXPECT_NEAR(stUTMYucatanCoordinate.dEasting, 261399.43, 0.01); + EXPECT_NEAR(stUTMYucatanCoordinate.dNorthing, 2257680.11, 0.01); + EXPECT_EQ(stUTMYucatanCoordinate.nZone, 16); + EXPECT_NEAR(stUTMYucatanCoordinate.dMeridianConvergence, -0.7, 0.1); + EXPECT_TRUE(stUTMYucatanCoordinate.bWithinNorthernHemisphere); +} + +/****************************************************************************** + * @brief Test the functionality of the ConvertUTMtoGPS function. Also tests functionality + * of GPS and UTM Coordinate structs. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + ******************************************************************************/ +TEST(GeoOpsTest, ConvertUTMtoGPS) +{ + // Initialize coordinates. + geoops::UTMCoordinate stUTMRollaCoordinate(607344.14, 4201167.33, 15, true); + geoops::UTMCoordinate stUTMMDRSCoordinate(518160.91, 4250913.23, 12, true); + geoops::UTMCoordinate stUTMAustraliaCoordinate(501654.37, 7428828.03, 54, false); + geoops::UTMCoordinate stUTMYucatanCoordinate(261399.43, 2257680.11, 16, true); + + // Convert GPS coordinates to UTM coordinate. + geoops::GPSCoordinate stGPSRollaCoordinate = geoops::ConvertUTMToGPS(stUTMRollaCoordinate); + geoops::GPSCoordinate stGPSMDRSCoordinate = geoops::ConvertUTMToGPS(stUTMMDRSCoordinate); + geoops::GPSCoordinate stGPSAustraliaCoordinate = geoops::ConvertUTMToGPS(stUTMAustraliaCoordinate); + geoops::GPSCoordinate stGPSYucatanCoordinate = geoops::ConvertUTMToGPS(stUTMYucatanCoordinate); + + // Check Rolla conversion. + EXPECT_NEAR(stGPSRollaCoordinate.dLatitude, 37.951766, 0.02); + EXPECT_NEAR(stGPSRollaCoordinate.dLongitude, -91.778187, 0.02); + EXPECT_NEAR(stGPSRollaCoordinate.dMeridianConvergence, 0.7, 0.1); + + // Check Mars Desert Research Station conversion. + EXPECT_NEAR(stGPSMDRSCoordinate.dLatitude, 38.406267, 0.02); + EXPECT_NEAR(stGPSMDRSCoordinate.dLongitude, -110.791997, 0.02); + EXPECT_NEAR(stGPSMDRSCoordinate.dMeridianConvergence, 0.1, 0.1); + + // Check Australia conversion. + EXPECT_NEAR(stGPSAustraliaCoordinate.dLatitude, -23.249790, 0.02); + EXPECT_NEAR(stGPSAustraliaCoordinate.dLongitude, 141.016173, 0.02); + EXPECT_NEAR(stGPSAustraliaCoordinate.dMeridianConvergence, 0.0, 0.1); + + // Check Yucatan conversion. + EXPECT_NEAR(stGPSYucatanCoordinate.dLatitude, 20.402472, 0.02); + EXPECT_NEAR(stGPSYucatanCoordinate.dLongitude, -89.286368, 0.02); + EXPECT_NEAR(stGPSYucatanCoordinate.dMeridianConvergence, -0.7, 0.1); +} + +/****************************************************************************** + * @brief Test the functionality of the CalculateGeoDistance function. Also tests functionality + * of GPS and UTM Coordinate structs. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-13 + ******************************************************************************/ +TEST(GeoOpsTest, CalculateGeoDistance) +{ + // Initialize coordinates. + geoops::UTMCoordinate stUTMRollaCoordinate(607344.14, 4201167.33, 15, true); + geoops::UTMCoordinate stUTMMDRSCoordinate(519116.71, 3807223.16, 12, true); + geoops::GPSCoordinate stGPSSDELC1(37.951856, -91.778340); + geoops::GPSCoordinate stGPSSDELC2(37.951670, -91.778537); + geoops::GPSCoordinate stGPSSDELC3(37.950635, -91.782465); + + // Convert GPS coordinates to UTM coordinate. + geoops::GPSCoordinate stGPSRollaCoordinate = geoops::ConvertUTMToGPS(stUTMRollaCoordinate); + geoops::GPSCoordinate stGPSMDRSCoordinate = geoops::ConvertUTMToGPS(stUTMMDRSCoordinate); + + // Calculate meter distance between the first two GPS points. + auto [dDistance1, dDegrees1] = geoops::CalculateGeoDistance(stGPSRollaCoordinate, stGPSMDRSCoordinate); + // Check distance calculation. + EXPECT_NEAR(dDistance1, 1749794.40, 0.02); + EXPECT_NEAR(dDegrees1, 15.77, 0.02); + + // Calculate meter distance between the second two GPS points. + auto [dDistance2, dDegrees2] = geoops::CalculateGeoDistance(stGPSSDELC1, stGPSSDELC2); + // Check distance calculation. + EXPECT_NEAR(dDistance2, 26.91, 0.02); + EXPECT_NEAR(dDegrees2, 0.00024, 0.00002); + + // Calculate meter distance between the second two GPS points. + auto [dDistance3, dDegrees3] = geoops::CalculateGeoDistance(stGPSSDELC1, stGPSSDELC3); + // Check distance calculation. + EXPECT_NEAR(dDistance3, 386.61, 0.02); + EXPECT_NEAR(dDegrees3, 0.003, 0.002); +} diff --git a/tests/Unit/src/util/NumberOperations.cc b/tests/Unit/src/util/NumberOperations.cc index ec51e617..2b1bbe79 100644 --- a/tests/Unit/src/util/NumberOperations.cc +++ b/tests/Unit/src/util/NumberOperations.cc @@ -14,6 +14,60 @@ #include "../../../../src/util/NumberOperations.hpp" +/****************************************************************************** + * @brief Test the functionality of the Clamp function. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-16 + ******************************************************************************/ +TEST(NumOpsTest, Clamp) +{ + // Create array for storing input and expect output values. + const int nTestValuesLength = 6; + const double aValues[nTestValuesLength] = {1.0, 0.0, 567.0, 0.05, -1.0, -89.3}; + const double aMinimums[nTestValuesLength] = {0.0, 0.0, -5.0, 0.05, -3.0, 50.0}; + const double aMaximums[nTestValuesLength] = {2.0, 1.0, 0.0, 0.04, 0.0, -100.0}; + const double aOutput[nTestValuesLength] = {1.0, 0.0, 0.0, 0.05, -1.0, 50.0}; + + // Loop through each value and compare inputs and outputs. + for (int nIter = 0; nIter < nTestValuesLength; ++nIter) + { + // Calculate clamp values. + double dResult = numops::Clamp(aValues[nIter], aMinimums[nIter], aMaximums[nIter]); + + // Check that the expected output values were calculated. + EXPECT_EQ(dResult, aOutput[nIter]); // output check. + } +} + +/****************************************************************************** + * @brief Test the functionality of the Bounded function. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-16 + ******************************************************************************/ +TEST(NumOpsTest, Bounded) +{ + // Create array for storing input and expect output values. + const int nTestValuesLength = 6; + const double aValues[nTestValuesLength] = {1.0, 0.0, 567.0, 0.05, -1.0, -89.3}; + const double aMinimums[nTestValuesLength] = {0.0, 0.0, -5.0, 0.05, -3.0, 50.0}; + const double aMaximums[nTestValuesLength] = {2.0, 1.0, 0.0, 0.04, 0.0, -100.0}; + const bool aOutput[nTestValuesLength] = {true, true, false, false, true, false}; + + // Loop through each value and compare inputs and outputs. + for (int nIter = 0; nIter < nTestValuesLength; ++nIter) + { + // Calculate valid bounds. + bool bResult = numops::Bounded(aValues[nIter], aMinimums[nIter], aMaximums[nIter]); + + // Check that the expected output values were calculated. + EXPECT_EQ(bResult, aOutput[nIter]); // output check. + } +} + /****************************************************************************** * @brief Test the functionality of the MapRange function. * @@ -53,3 +107,30 @@ TEST(NumOpsTest, MapRange) // Make sure original value is returned. EXPECT_EQ(dActualValue, dOldValue); } + +/****************************************************************************** + * @brief Test the functionality of the InputModulus function. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-10-19 + ******************************************************************************/ +TEST(NumOpsTest, InputModulus) +{ + // Create array for storing input and expect output values. + const int nTestValuesLength = 6; + const double aValues[nTestValuesLength] = {1.0, -1.0, 4.0, 360, 350, 170}; + const double aMinimums[nTestValuesLength] = {0.0, 0.0, -2.0, -180, -180, -180}; + const double aMaximums[nTestValuesLength] = {2.0, 2.0, 2.0, 180, 180, 180}; + const double aOutput[nTestValuesLength] = {1.0, 1.0, 0.0, 0, -10, 170}; + + // Loop through each value and compare inputs and outputs. + for (int nIter = 0; nIter < nTestValuesLength; ++nIter) + { + // Calculate valid bounds. + double dResult = numops::InputModulus(aValues[nIter], aMinimums[nIter], aMaximums[nIter]); + + // Check that the expected output values were calculated. + EXPECT_EQ(dResult, aOutput[nIter]); + } +}