From d3b3635115e20af76adb4fe0a54d721aa87a2cbb Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 6 Oct 2023 02:00:46 +0000 Subject: [PATCH 01/19] Add geographiclib. --- .devcontainer/Dockerfile_Focal | 15 ++++++++++++++- .devcontainer/devcontainer.json | 18 ++++++++++-------- CMakeLists.txt | 7 ++++++- 3 files changed, 30 insertions(+), 10 deletions(-) diff --git a/.devcontainer/Dockerfile_Focal b/.devcontainer/Dockerfile_Focal index 35b82732..e629db95 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=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 && \ cd quill/build && \ - cmake .. && \ + cmake \ + -D CMAKE_BUILD_TYPE=Release .. && \ make && \ make install && \ cd ../.. && \ diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index b6039088..7fae8144 100755 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -28,13 +28,13 @@ "source=${localWorkspaceFolder}/data/models/zed,target=/usr/local/zed/resources,type=bind,consistency=delegated" ], // "image": "ghcr.io/missourimrdt/autonomy-jammy:latest", - "image": "ghcr.io/missourimrdt/autonomy-focal:latest", + // "image": "ghcr.io/missourimrdt/autonomy-focal:latest", // "image": "ghcr.io/missourimrdt/autonomy-jetpack:latest", - // "build": { - // // "dockerfile": "Dockerfile_Jammy" - // "dockerfile": "Dockerfile_Focal" - // // "dockerfile": "Dockerfile_JetPack" - // }, + "build": { + // "dockerfile": "Dockerfile_Jammy" + "dockerfile": "Dockerfile_Focal" + // "dockerfile": "Dockerfile_JetPack" + }, // Features to add to the dev container. More info: https://containers.dev/features. // "features": {}, // Use 'forwardPorts' to make a list of ports inside the container available locally. @@ -94,7 +94,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, @@ -197,4 +199,4 @@ } } } -} +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 1aa0efe6..2050c532 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_LIBS} ${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_LIBS} ${ZED_LIBS} quill::quill RoveComm_CPP RUNTIME DESTINATION bin) ## Unit/Integration Tests file(GLOB_RECURSE UnitTests_SRC CONFIGURE_DEPENDS "tests/Unit/*.cc") From 3eb7b6c0159e1092a20a593d4058a896706836d8 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 6 Oct 2023 03:01:13 +0000 Subject: [PATCH 02/19] Add words to dictionary. --- data/Custom_Dictionaries/Autonomy-Dictionary.txt | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/data/Custom_Dictionaries/Autonomy-Dictionary.txt b/data/Custom_Dictionaries/Autonomy-Dictionary.txt index b0a7b641..b43b819f 100644 --- a/data/Custom_Dictionaries/Autonomy-Dictionary.txt +++ b/data/Custom_Dictionaries/Autonomy-Dictionary.txt @@ -3,26 +3,37 @@ ABGR alloc allocs Aruco +autofetch AUTONOMYTHREAD BASICCAM CALIB clayjay codeql +codezombiech Coeffs +Colour +cpptools +cschlosser CUDA deadband deque Desaturate devcontainer diffdrive +doxdocgen Doxygen driveboard +Extention flightsticks geoops Geospatial +gmock +gruntfuggly +gtest hyperthreaded imgops imshow +jbockle jetpack jthread LEFTCAM @@ -32,6 +43,7 @@ mainpage MAPRANGE Memcheck microcontrollers +missourimrdt MJPEG MRDT MULTIMEDIABOARD @@ -59,12 +71,15 @@ Styleguides Tele threadpool tparam +twxs UART valgrind Watchpoints +wslg XYZBGRA XYZRGBA YUVJ YUYV +Zain ZEDCAM ZEDSDK From b522af4fb23336414cf813e97d58870ffdc45b05 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Sun, 8 Oct 2023 18:37:12 +0000 Subject: [PATCH 03/19] Add geographiclib to other Dockerfiles. --- .devcontainer/Dockerfile_Jammy | 12 ++++++++++++ .devcontainer/Dockerfile_JetPack | 12 ++++++++++++ 2 files changed, 24 insertions(+) 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 && \ From 53415a455ca0200071e93669eeff47b3f7ba916c Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 12 Oct 2023 00:51:29 +0000 Subject: [PATCH 04/19] Added GPS/UTM conversions. --- .../Autonomy-Dictionary.txt | 1 + src/util/GeospatialOperations.hpp | 282 ++++++++---------- 2 files changed, 126 insertions(+), 157 deletions(-) diff --git a/data/Custom_Dictionaries/Autonomy-Dictionary.txt b/data/Custom_Dictionaries/Autonomy-Dictionary.txt index b43b819f..b8ed2838 100644 --- a/data/Custom_Dictionaries/Autonomy-Dictionary.txt +++ b/data/Custom_Dictionaries/Autonomy-Dictionary.txt @@ -73,6 +73,7 @@ threadpool tparam twxs UART +UTMUPS valgrind Watchpoints wslg diff --git a/src/util/GeospatialOperations.hpp b/src/util/GeospatialOperations.hpp index 690eeb8a..ebbfaa08 100644 --- a/src/util/GeospatialOperations.hpp +++ b/src/util/GeospatialOperations.hpp @@ -12,6 +12,8 @@ #ifndef GEOSPATIAL_OPERATIONS_HPP #define GEOSPATIAL_OPERATIONS_HPP +#include + /****************************************************************************** * @brief Namespace containing functions related to operations on global position number * systems and other datatypes. @@ -22,7 +24,6 @@ ******************************************************************************/ namespace geoops { - /****************************************************************************** * @brief This struct stores/contains information about orientation. * @@ -83,96 +84,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. + * @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) + 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; - - // 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. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ - GPSCoordinate(double dLatitude, double dLongitude, double dAltitude) - { - // 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,12 +139,15 @@ 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. + double dAltitude; // The elevation above sea level in meters. + int nZone; // The UTM zone number identifying the region on the Earth's surface. + 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. + bool bWithinNorthernHemisphere; // Indicates whether the coordinate is located in the northern hemisphere. ///////////////////////////////////////// // Declare public methods. @@ -200,89 +155,102 @@ namespace geoops /****************************************************************************** * @brief Construct a new UTMCoordinate object. * + * @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 dAltitude - The elevation above sea level in meters. + * @param nZone - The UTM zone number identifying the region on the Earth's surface. + * @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. + * @param bWithinNorthernHemisphere - Indicates whether the coordinate is located in the northern hemisphere. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ - UTMCoordinate() + UTMCoordinate(double dEasting = 0.0, + double dNorthing = 0.0, + double dAltitude = 0.0, + int nZone = 0, + double d2DAccuracy = -1.0, + double d3DAccuracy = -1.0, + double dMeridianConvergence = -1.0, + double dScale = 0.0, + bool bWithinNorthernHemisphere = true) { - // 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 with given values. + this->dEasting = dEasting; + this->dNorthing = dNorthing; + this->dAltitude = dAltitude; + this->nZone = nZone; + this->d2DAccuracy = d2DAccuracy; + this->d3DAccuracy = d3DAccuracy; + this->dMeridianConvergence = dMeridianConvergence; + this->dScale = dScale; + this->bWithinNorthernHemisphere = bWithinNorthernHemisphere; } + }; - /****************************************************************************** - * @brief Construct a new UTMCoordinate object. - * - * @param dEasting - The Easting of the UTM coordinate. - * @param dNorthing - The Northing of the UTM coordinate. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-23 - ******************************************************************************/ - UTMCoordinate(double dEasting, double dNorthing) - { - // Initialize member variables with given values. - this->dEasting = dEasting; - this->dNorthing = dNorthing; + /****************************************************************************** + * @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. - dAltitude = 0.0; - nZone = 0; - d2DAccuracy = -1.0; - d3DAccuracy = -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. - * - * @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; + // Return the converted UTM coordinate. + return stConvertCoord; + } - // Use default values for everything else. - nZone = 0; - d2DAccuracy = -1.0; - d2DAccuracy = -1.0; - } + /****************************************************************************** + * @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; - /****************************************************************************** - * @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; - } - }; + // 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; + } } // namespace geoops #endif From 0efea4d32580494f252f1c2d44a88acc476d53b0 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 12 Oct 2023 19:51:48 +0000 Subject: [PATCH 05/19] Added unit test for UTM/GPS conversion. --- .devcontainer/Dockerfile_Focal | 2 +- .../Autonomy-Dictionary.txt | 3 + src/util/GeospatialOperations.hpp | 52 ++++++--- tests/Unit/src/util/GeospatialOperations.cc | 107 ++++++++++++++++++ 4 files changed, 148 insertions(+), 16 deletions(-) create mode 100644 tests/Unit/src/util/GeospatialOperations.cc diff --git a/.devcontainer/Dockerfile_Focal b/.devcontainer/Dockerfile_Focal index e629db95..f9c65091 100644 --- a/.devcontainer/Dockerfile_Focal +++ b/.devcontainer/Dockerfile_Focal @@ -119,7 +119,7 @@ RUN git clone --depth 1 --branch ${GEOLIB_VERSION} https://github.com/geographic mkdir geographiclib/build && \ cd geographiclib/build && \ cmake \ - -D CMAKE_BUILD_TYPE=RelWithDebInfo .. && \ + -D CMAKE_BUILD_TYPE=Release .. && \ make && \ make install && \ cd ../.. && \ diff --git a/data/Custom_Dictionaries/Autonomy-Dictionary.txt b/data/Custom_Dictionaries/Autonomy-Dictionary.txt index b8ed2838..47141636 100644 --- a/data/Custom_Dictionaries/Autonomy-Dictionary.txt +++ b/data/Custom_Dictionaries/Autonomy-Dictionary.txt @@ -28,6 +28,7 @@ flightsticks geoops Geospatial gmock +GPSMDRS gruntfuggly gtest hyperthreaded @@ -62,6 +63,7 @@ reprojection RESOLUTIONX RESOLUTIONY RGBE +Rolla rovecomm searchbar SIMCAM @@ -73,6 +75,7 @@ threadpool tparam twxs UART +UTMMDRS UTMUPS valgrind Watchpoints diff --git a/src/util/GeospatialOperations.hpp b/src/util/GeospatialOperations.hpp index ebbfaa08..6ef1525f 100644 --- a/src/util/GeospatialOperations.hpp +++ b/src/util/GeospatialOperations.hpp @@ -141,53 +141,75 @@ namespace geoops // Declare struct public attributes. double dEasting; // The eastward displacement from the UTM zone's central meridian in meters. double dNorthing; // The northward displacement from the equator in meters. - double dAltitude; // The elevation above sea level 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. - bool bWithinNorthernHemisphere; // Indicates whether the coordinate is located in the northern hemisphere. ///////////////////////////////////////// // Declare public methods. ///////////////////////////////////////// + + /****************************************************************************** + * @brief Default Construct a new UTMCoordinate object. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-10-12 + ******************************************************************************/ + UTMCoordinate() + { + // 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 eastward displacement from the UTM zone's central meridian in meters. * @param dNorthing - The northward displacement from the equator in meters. - * @param dAltitude - The elevation above sea level 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. - * @param bWithinNorthernHemisphere - Indicates whether the coordinate is located in the northern hemisphere. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-23 ******************************************************************************/ - UTMCoordinate(double dEasting = 0.0, - double dNorthing = 0.0, - double dAltitude = 0.0, - int nZone = 0, - double d2DAccuracy = -1.0, - double d3DAccuracy = -1.0, - double dMeridianConvergence = -1.0, - double dScale = 0.0, - bool bWithinNorthernHemisphere = true) + 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; - this->dAltitude = dAltitude; this->nZone = nZone; + this->bWithinNorthernHemisphere = bWithinNorthernHemisphere; + this->dAltitude = dAltitude; this->d2DAccuracy = d2DAccuracy; this->d3DAccuracy = d3DAccuracy; this->dMeridianConvergence = dMeridianConvergence; this->dScale = dScale; - this->bWithinNorthernHemisphere = bWithinNorthernHemisphere; } }; diff --git a/tests/Unit/src/util/GeospatialOperations.cc b/tests/Unit/src/util/GeospatialOperations.cc new file mode 100644 index 00000000..6efe9c20 --- /dev/null +++ b/tests/Unit/src/util/GeospatialOperations.cc @@ -0,0 +1,107 @@ +/****************************************************************************** + * @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(34.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, 519116.71, 0.01); + EXPECT_NEAR(stUTMMDRSCoordinate.dNorthing, 3807223.16, 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(519116.71, 3807223.16, 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, 34.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); +} From 4e8a7c3785db21c1a976899c766c7edd8f9450cf Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 12 Oct 2023 19:52:06 +0000 Subject: [PATCH 06/19] Fixed linking of GeoLib. --- CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2050c532..a0d5deb1 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,13 +137,13 @@ target_link_libraries(${PROJECT_NAME} PRIVATE Threads::Threads Eigen3::Eigen quill::quill ${OpenCV_LIBS} - ${GeographicLib_LIBS} + ${GeographicLib_LIBRARIES} ${ZED_LIBS} RoveComm_CPP ) ## Package Executable -install(TARGETS ${PROJECT_NAME} RUNTIME_DEPENDENCIES DIRECTORIES ${OpenCV_LIBS} ${GeographicLib_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") @@ -154,7 +154,7 @@ list(LENGTH IntegrationTests_SRC IntegrationTests_LEN) if (UnitTests_LEN GREATER 0) add_executable(${PROJECT_NAME}_UnitTests ${UnitTests_SRC}) - target_link_libraries(${PROJECT_NAME}_UnitTests GTest::gtest GTest::gtest_main) + target_link_libraries(${PROJECT_NAME}_UnitTests GTest::gtest GTest::gtest_main ${OpenCV_LIBS} ${GeographicLib_LIBRARIES}) add_test(Unit_Tests ${PROJECT_NAME}_UnitTests) else() message("No Unit Tests!") @@ -162,7 +162,7 @@ endif() if (IntegrationTests_LEN GREATER 0) add_executable(${PROJECT_NAME}_IntegrationTests ${IntegrationTests_SRC}) - target_link_libraries(${PROJECT_NAME}_IntegrationTests GTest::gtest GTest::gtest_main) + target_link_libraries(${PROJECT_NAME}_IntegrationTests GTest::gtest GTest::gtest_main ${OpenCV_LIBS} ${GeographicLib_LIBRARIES}) add_test(Integration_Tests ${PROJECT_NAME}_IntegrationTests) else() message("No Integration Tests!") From c0115eb7dca6fee6ec59b90eb2d906bc1dbb9d77 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 13 Oct 2023 22:36:16 +0000 Subject: [PATCH 07/19] Fixed bugs within differential drive kinematics. --- src/algorithms/DifferentialDrive.hpp | 49 ++++++++++++++++++---------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/src/algorithms/DifferentialDrive.hpp b/src/algorithms/DifferentialDrive.hpp index d7f76a85..7ecfccf1 100644 --- a/src/algorithms/DifferentialDrive.hpp +++ b/src/algorithms/DifferentialDrive.hpp @@ -17,8 +17,6 @@ #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 +43,35 @@ ******************************************************************************/ namespace diffdrive { + /****************************************************************************** + * @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 +95,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 +114,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 +145,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 +169,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). From 02f61e9d11e640d9b0aadcf4085bbcc8d8b6991e Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 13 Oct 2023 22:36:33 +0000 Subject: [PATCH 08/19] Wrote Unit test for diffdrive. --- .../Unit/src/algorithms/DifferentialDrive.cc | 100 ++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 tests/Unit/src/algorithms/DifferentialDrive.cc diff --git a/tests/Unit/src/algorithms/DifferentialDrive.cc b/tests/Unit/src/algorithms/DifferentialDrive.cc new file mode 100644 index 00000000..fd89bb1f --- /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 double 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. + } +} From d0e0a7114c1b2805eb7701fd0cfb751ec12f8071 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 13 Oct 2023 22:36:54 +0000 Subject: [PATCH 09/19] Drivers are close to done. --- src/drivers/DriveBoard.cpp | 16 ++++++++-------- src/drivers/NavigationBoard.cpp | 26 +++++++++++++++++++------- src/drivers/NavigationBoard.h | 28 ++++++++++++++++++++-------- 3 files changed, 47 insertions(+), 23 deletions(-) diff --git a/src/drivers/DriveBoard.cpp b/src/drivers/DriveBoard.cpp index 3876e253..286e5881 100755 --- a/src/drivers/DriveBoard.cpp +++ b/src/drivers/DriveBoard.cpp @@ -58,23 +58,23 @@ DriveBoard::~DriveBoard() std::array DriveBoard::CalculateMove(const float fSpeed, const float fAngle, const DifferentialControlMethod eKinematicsMethod) { // Create instance variables. - std::array aDrivePowers; + diffdrive::DrivePowers stDrivePowers; // Check what kinematics model we should use. switch (eKinematicsMethod) { - case eArcadeDrive: aDrivePowers = diffdrive::CalculateArcadeDrive(double(fSpeed), double(fAngle), constants::DRIVE_MIN_POWER); break; + case eArcadeDrive: stDrivePowers = 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); + stDrivePowers = 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]); + m_fTargetSpeedLeft = double(stDrivePowers.dLeftDrivePower); + m_fTargetSpeedRight = double(stDrivePowers.dRightDrivePower); // Submit logger message. LOG_DEBUG(logging::g_qSharedLogger, "Driving at: ({}, {})", m_fTargetSpeedLeft, m_fTargetSpeedRight); 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 From 7994e2e3b0e79bd19869aa3592e32ac971beaf40 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 13 Oct 2023 22:37:04 +0000 Subject: [PATCH 10/19] Readded Clamp to numops. --- src/util/NumberOperations.hpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/util/NumberOperations.hpp b/src/util/NumberOperations.hpp index 12be446b..90afac3e 100755 --- a/src/util/NumberOperations.hpp +++ b/src/util/NumberOperations.hpp @@ -24,6 +24,24 @@ ******************************************************************************/ 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 + T Clamp(T tValue, T tMin, T tMax) + { + return std::max(std::min(tMax, tValue), tMin); + } + /****************************************************************************** * @brief Maps a value to a new range given the old range. * @@ -59,6 +77,5 @@ namespace numops // Return new mapped value. return tNewMinimum + tScaledValue * tNewValueRange; } - } // namespace numops #endif From 7387732c97487fd7b765c3252247f7c42c9a7a10 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 13 Oct 2023 23:49:53 +0000 Subject: [PATCH 11/19] Added CalculateGeoDistance function to geoops namespace. --- CONTRIBUTING.md | 1 + .../Autonomy-Dictionary.txt | 1 + src/util/GeospatialOperations.hpp | 53 +++++++++++++++++++ src/util/NumberOperations.hpp | 2 +- 4 files changed, 56 insertions(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 15fc9fba..8239f38b 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -102,6 +102,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 47141636..4ac67798 100644 --- a/data/Custom_Dictionaries/Autonomy-Dictionary.txt +++ b/data/Custom_Dictionaries/Autonomy-Dictionary.txt @@ -29,6 +29,7 @@ geoops Geospatial gmock GPSMDRS +GPSSDELC gruntfuggly gtest hyperthreaded diff --git a/src/util/GeospatialOperations.hpp b/src/util/GeospatialOperations.hpp index 6ef1525f..f8fbd2fb 100644 --- a/src/util/GeospatialOperations.hpp +++ b/src/util/GeospatialOperations.hpp @@ -12,6 +12,7 @@ #ifndef GEOSPATIAL_OPERATIONS_HPP #define GEOSPATIAL_OPERATIONS_HPP +#include #include /****************************************************************************** @@ -24,6 +25,30 @@ ******************************************************************************/ namespace geoops { + ///////////////////////////////////////// + // Declare public variables. + ///////////////////////////////////////// + // Constants retrieved from: https://nssdc.gsfc.nasa.gov/planetary/factsheet/earthfact.html + const double dEarthEquilateralRadius = 6378137.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. * @@ -274,5 +299,33 @@ namespace geoops // 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(dEarthEquilateralRadius, 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 90afac3e..3134fb0d 100755 --- a/src/util/NumberOperations.hpp +++ b/src/util/NumberOperations.hpp @@ -37,7 +37,7 @@ namespace numops * @date 2023-06-20 ******************************************************************************/ template - T Clamp(T tValue, T tMin, T tMax) + inline T Clamp(T tValue, T tMin, T tMax) { return std::max(std::min(tMax, tValue), tMin); } From 6b3eca09dbb580deb764cc6a023d688c35672b54 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 13 Oct 2023 23:50:07 +0000 Subject: [PATCH 12/19] Added test for CalculateGeoDistance --- tests/Unit/src/util/GeospatialOperations.cc | 50 ++++++++++++++++++--- 1 file changed, 45 insertions(+), 5 deletions(-) diff --git a/tests/Unit/src/util/GeospatialOperations.cc b/tests/Unit/src/util/GeospatialOperations.cc index 6efe9c20..253fecbd 100644 --- a/tests/Unit/src/util/GeospatialOperations.cc +++ b/tests/Unit/src/util/GeospatialOperations.cc @@ -24,7 +24,7 @@ TEST(GeoOpsTest, ConvertGPStoUTM) { // Initialize coordinates. geoops::GPSCoordinate stGPSRollaCoordinate(37.951766, -91.778187); - geoops::GPSCoordinate stGPSMDRSCoordinate(34.406267, -110.791997); + geoops::GPSCoordinate stGPSMDRSCoordinate(38.406267, -110.791997); geoops::GPSCoordinate stGPSAustraliaCoordinate(-23.249790, 141.016173); geoops::GPSCoordinate stGPSYucatanCoordinate(20.402472, -89.286368); @@ -42,8 +42,8 @@ TEST(GeoOpsTest, ConvertGPStoUTM) EXPECT_TRUE(stUTMRollaCoordinate.bWithinNorthernHemisphere); // Check Mars Desert Research Station conversion. - EXPECT_NEAR(stUTMMDRSCoordinate.dEasting, 519116.71, 0.01); - EXPECT_NEAR(stUTMMDRSCoordinate.dNorthing, 3807223.16, 0.01); + 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); @@ -75,7 +75,7 @@ TEST(GeoOpsTest, ConvertUTMtoGPS) { // Initialize coordinates. geoops::UTMCoordinate stUTMRollaCoordinate(607344.14, 4201167.33, 15, true); - geoops::UTMCoordinate stUTMMDRSCoordinate(519116.71, 3807223.16, 12, 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); @@ -91,7 +91,7 @@ TEST(GeoOpsTest, ConvertUTMtoGPS) EXPECT_NEAR(stGPSRollaCoordinate.dMeridianConvergence, 0.7, 0.1); // Check Mars Desert Research Station conversion. - EXPECT_NEAR(stGPSMDRSCoordinate.dLatitude, 34.406267, 0.02); + EXPECT_NEAR(stGPSMDRSCoordinate.dLatitude, 38.406267, 0.02); EXPECT_NEAR(stGPSMDRSCoordinate.dLongitude, -110.791997, 0.02); EXPECT_NEAR(stGPSMDRSCoordinate.dMeridianConvergence, 0.1, 0.1); @@ -105,3 +105,43 @@ TEST(GeoOpsTest, ConvertUTMtoGPS) 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, 1751754.58, 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.94, 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, 387.05, 0.02); + EXPECT_NEAR(dDegrees3, 0.003, 0.002); +} From 5f961a61c1ad4d7af051e319388aee3b814d5045 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Sat, 14 Oct 2023 00:30:07 +0000 Subject: [PATCH 13/19] Change earth radius. --- src/drivers/DriveBoard.cpp | 2 +- src/util/GeospatialOperations.hpp | 4 ++-- tests/Unit/src/util/GeospatialOperations.cc | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/DriveBoard.cpp b/src/drivers/DriveBoard.cpp index 286e5881..ce8d00d5 100755 --- a/src/drivers/DriveBoard.cpp +++ b/src/drivers/DriveBoard.cpp @@ -48,7 +48,7 @@ DriveBoard::~DriveBoard() * given heading at a given speed * * @param fSpeed - The speed to drive at (-1 to 1) - * @param fAngle - The angle to drive towards. + * @param fAngle - The angle to drive towards. (0 - 360) 0 is North. * @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) * diff --git a/src/util/GeospatialOperations.hpp b/src/util/GeospatialOperations.hpp index f8fbd2fb..70f5b922 100644 --- a/src/util/GeospatialOperations.hpp +++ b/src/util/GeospatialOperations.hpp @@ -29,7 +29,7 @@ namespace geoops // Declare public variables. ///////////////////////////////////////// // Constants retrieved from: https://nssdc.gsfc.nasa.gov/planetary/factsheet/earthfact.html - const double dEarthEquilateralRadius = 6378137.0; // Earth's radius in meters. + 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. /****************************************************************************** @@ -319,7 +319,7 @@ namespace geoops GeoDistance stDistances; // Construct a geodesic with earth characteristics. - GeographicLib::Geodesic geGeodesic(dEarthEquilateralRadius, dEarthEllipsoidFlattening); + GeographicLib::Geodesic geGeodesic(dEarthAverageRadius, dEarthEllipsoidFlattening); // Solve the inverse geodesic. stDistances.dArcLengthDegrees = geGeodesic.Inverse(stCoord1.dLatitude, stCoord1.dLongitude, stCoord2.dLatitude, stCoord2.dLongitude, stDistances.dDistanceMeters); diff --git a/tests/Unit/src/util/GeospatialOperations.cc b/tests/Unit/src/util/GeospatialOperations.cc index 253fecbd..3f4ed38f 100644 --- a/tests/Unit/src/util/GeospatialOperations.cc +++ b/tests/Unit/src/util/GeospatialOperations.cc @@ -130,18 +130,18 @@ TEST(GeoOpsTest, CalculateGeoDistance) // Calculate meter distance between the first two GPS points. auto [dDistance1, dDegrees1] = geoops::CalculateGeoDistance(stGPSRollaCoordinate, stGPSMDRSCoordinate); // Check distance calculation. - EXPECT_NEAR(dDistance1, 1751754.58, 0.02); + 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.94, 0.02); + 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, 387.05, 0.02); + EXPECT_NEAR(dDistance3, 386.61, 0.02); EXPECT_NEAR(dDegrees3, 0.003, 0.002); } From f339b883d3a8750bd23b825fc429026838e6b5de Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Tue, 17 Oct 2023 03:00:45 +0000 Subject: [PATCH 14/19] Began writing a universal PID controller. --- .../Autonomy-Dictionary.txt | 3 + src/algorithms/controllers/PIDController.cpp | 167 ++++++++++++++++++ src/algorithms/controllers/PIDController.h | 112 ++++++++++++ src/algorithms/controllers/README.md | 31 ++++ src/util/NumberOperations.hpp | 29 +++ .../Unit/src/algorithms/DifferentialDrive.cc | 12 +- tests/Unit/src/util/NumberOperations.cc | 54 ++++++ 7 files changed, 402 insertions(+), 6 deletions(-) create mode 100644 src/algorithms/controllers/PIDController.cpp create mode 100644 src/algorithms/controllers/PIDController.h create mode 100755 src/algorithms/controllers/README.md diff --git a/data/Custom_Dictionaries/Autonomy-Dictionary.txt b/data/Custom_Dictionaries/Autonomy-Dictionary.txt index 4ac67798..c4380491 100644 --- a/data/Custom_Dictionaries/Autonomy-Dictionary.txt +++ b/data/Custom_Dictionaries/Autonomy-Dictionary.txt @@ -24,6 +24,7 @@ doxdocgen Doxygen driveboard Extention +feedforward flightsticks geoops Geospatial @@ -67,6 +68,8 @@ RGBE Rolla rovecomm searchbar +setpoint +setpoints SIMCAM statechart Struct diff --git a/src/algorithms/controllers/PIDController.cpp b/src/algorithms/controllers/PIDController.cpp new file mode 100644 index 00000000..b8c78eb3 --- /dev/null +++ b/src/algorithms/controllers/PIDController.cpp @@ -0,0 +1,167 @@ +/****************************************************************************** + * @brief Implements the PIDController class. + * + * @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; + + // 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; +} diff --git a/src/algorithms/controllers/PIDController.h b/src/algorithms/controllers/PIDController.h new file mode 100644 index 00000000..317d4f77 --- /dev/null +++ b/src/algorithms/controllers/PIDController.h @@ -0,0 +1,112 @@ +/****************************************************************************** + * @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 Reset(); + + ///////////////////////////////////////// + // Setters + ///////////////////////////////////////// + void SetProportional(const double dKp); + void SetIntegral(const double sKi); + 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 dMaxDifference); + void SetMaxIntegralEffort(const double dMaxIEffort); + void SetOutputLimits(const double dMaxMin); + void SetOutputLimits(const double dMinEffort, const double dMaxEffort); + void SetOutputRampRate(const double dOutputRampRate); + void SetOutputFilter(const double dStrength); + void SetDirection(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 GetDirection() const; + + private: + ///////////////////////////////////////// + // Declare public methods. + ///////////////////////////////////////// + double 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. + 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/util/NumberOperations.hpp b/src/util/NumberOperations.hpp index 3134fb0d..f46ff604 100755 --- a/src/util/NumberOperations.hpp +++ b/src/util/NumberOperations.hpp @@ -42,6 +42,35 @@ namespace numops 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 + * @param tMax - + * @return true - + * @return false - + * + * @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. * diff --git a/tests/Unit/src/algorithms/DifferentialDrive.cc b/tests/Unit/src/algorithms/DifferentialDrive.cc index fd89bb1f..404bc4e2 100644 --- a/tests/Unit/src/algorithms/DifferentialDrive.cc +++ b/tests/Unit/src/algorithms/DifferentialDrive.cc @@ -80,12 +80,12 @@ TEST(DifferentialDriveTest, ArcadeDrive) 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 double 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}; + 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) diff --git a/tests/Unit/src/util/NumberOperations.cc b/tests/Unit/src/util/NumberOperations.cc index ec51e617..ab45823b 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. * From 082d9732bdb2174f4fb7288d341a668eab015461 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Tue, 17 Oct 2023 14:43:46 +0000 Subject: [PATCH 15/19] Little more progress on the PID controller. --- src/algorithms/controllers/PIDController.cpp | 45 ++++++++++++++++++++ src/util/NumberOperations.hpp | 8 ++-- 2 files changed, 49 insertions(+), 4 deletions(-) diff --git a/src/algorithms/controllers/PIDController.cpp b/src/algorithms/controllers/PIDController.cpp index b8c78eb3..a11a75fa 100644 --- a/src/algorithms/controllers/PIDController.cpp +++ b/src/algorithms/controllers/PIDController.cpp @@ -165,3 +165,48 @@ double PIDController::Calculate(const double dActual, const double dSetpoint) // 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 - + * + * @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 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; +} diff --git a/src/util/NumberOperations.hpp b/src/util/NumberOperations.hpp index f46ff604..6efe90ec 100755 --- a/src/util/NumberOperations.hpp +++ b/src/util/NumberOperations.hpp @@ -47,10 +47,10 @@ namespace numops * * @tparam T - Template argument for given value type. * @param tValue - The value to check. - * @param tMin - The - * @param tMax - - * @return true - - * @return false - + * @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 From b8269fbcfb8f54075e0484476f2333822290edea Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Tue, 17 Oct 2023 22:52:46 +0000 Subject: [PATCH 16/19] Wrote setters for PID controller. --- src/algorithms/controllers/PIDController.cpp | 302 +++++++++++++++++++ src/algorithms/controllers/PIDController.h | 8 +- 2 files changed, 306 insertions(+), 4 deletions(-) diff --git a/src/algorithms/controllers/PIDController.cpp b/src/algorithms/controllers/PIDController.cpp index a11a75fa..e082fe4e 100644 --- a/src/algorithms/controllers/PIDController.cpp +++ b/src/algorithms/controllers/PIDController.cpp @@ -210,3 +210,305 @@ void PIDController::Reset() 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; +} + +void PIDController::SetOutputFilter(const double dStrength) +{ + // Check if the +} + +/****************************************************************************** + * @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 index 317d4f77..99836097 100644 --- a/src/algorithms/controllers/PIDController.h +++ b/src/algorithms/controllers/PIDController.h @@ -54,16 +54,16 @@ class PIDController // Setters ///////////////////////////////////////// void SetProportional(const double dKp); - void SetIntegral(const double sKi); + 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 dMaxDifference); + void SetMaxSetpointDifference(const double dMaxSetpointDifference); void SetMaxIntegralEffort(const double dMaxIEffort); - void SetOutputLimits(const double dMaxMin); 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(bool bReversed = false); @@ -86,7 +86,7 @@ class PIDController ///////////////////////////////////////// // Declare public methods. ///////////////////////////////////////// - double CheckGainSigns(); + void CheckGainSigns(); ///////////////////////////////////////// // Declare private member variables. From 380ef22a64beb174988d32ce4af759ffd3df6fa0 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 19 Oct 2023 03:47:53 +0000 Subject: [PATCH 17/19] Finished writing setters for PIDController and fixed compile error. --- src/algorithms/controllers/PIDController.cpp | 31 +++++++++++++++++++- src/algorithms/controllers/PIDController.h | 2 +- src/util/NumberOperations.hpp | 1 + 3 files changed, 32 insertions(+), 2 deletions(-) diff --git a/src/algorithms/controllers/PIDController.cpp b/src/algorithms/controllers/PIDController.cpp index e082fe4e..f4574361 100644 --- a/src/algorithms/controllers/PIDController.cpp +++ b/src/algorithms/controllers/PIDController.cpp @@ -448,9 +448,38 @@ void PIDController::SetOutputRampRate(const double dOutputRampRate) 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 + // 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; } /****************************************************************************** diff --git a/src/algorithms/controllers/PIDController.h b/src/algorithms/controllers/PIDController.h index 99836097..b871387a 100644 --- a/src/algorithms/controllers/PIDController.h +++ b/src/algorithms/controllers/PIDController.h @@ -66,7 +66,7 @@ class PIDController void SetOutputLimits(const double dMaxMin); void SetOutputRampRate(const double dOutputRampRate); void SetOutputFilter(const double dStrength); - void SetDirection(bool bReversed = false); + void SetDirection(const bool bReversed = false); ///////////////////////////////////////// // Getters diff --git a/src/util/NumberOperations.hpp b/src/util/NumberOperations.hpp index 6efe90ec..b995753a 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 From 37b8f07431fa0914e30a2ccb7231d7a460ae0794 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 20 Oct 2023 02:30:02 +0000 Subject: [PATCH 18/19] Finished PID controller and drive board class. --- src/AutonomyConstants.h | 11 +- src/algorithms/DifferentialDrive.hpp | 65 +++++++ src/algorithms/controllers/PIDController.cpp | 193 +++++++++++++++++++ src/algorithms/controllers/PIDController.h | 47 +++-- src/drivers/DriveBoard.cpp | 101 ++++++---- src/drivers/DriveBoard.h | 27 ++- src/util/NumberOperations.hpp | 29 +++ tests/Unit/src/util/NumberOperations.cc | 27 +++ 8 files changed, 424 insertions(+), 76 deletions(-) diff --git a/src/AutonomyConstants.h b/src/AutonomyConstants.h index 674d36af..7a5de4b9 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 7ecfccf1..71172dd3 100644 --- a/src/algorithms/DifferentialDrive.hpp +++ b/src/algorithms/DifferentialDrive.hpp @@ -12,7 +12,9 @@ #ifndef DIFFERENTIAL_DRIVE_HPP #define DIFFERENTIAL_DRIVE_HPP +#include "../AutonomyConstants.h" #include "../util/NumberOperations.hpp" +#include "controllers/PIDController.h" #include #include @@ -43,6 +45,19 @@ ******************************************************************************/ 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 @@ -193,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 index f4574361..c3754b27 100644 --- a/src/algorithms/controllers/PIDController.cpp +++ b/src/algorithms/controllers/PIDController.cpp @@ -1,5 +1,7 @@ /****************************************************************************** * @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) @@ -76,6 +78,16 @@ double PIDController::Calculate(const double dActual, const double dSetpoint) // 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. @@ -196,6 +208,42 @@ double PIDController::Calculate() 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. @@ -482,6 +530,151 @@ void PIDController::SetDirection(const bool bReversed) 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. * diff --git a/src/algorithms/controllers/PIDController.h b/src/algorithms/controllers/PIDController.h index b871387a..506406a1 100644 --- a/src/algorithms/controllers/PIDController.h +++ b/src/algorithms/controllers/PIDController.h @@ -44,15 +44,19 @@ class PIDController ///////////////////////////////////////// // 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); @@ -71,6 +75,7 @@ class PIDController ///////////////////////////////////////// // Getters ///////////////////////////////////////// + double GetProportional() const; double GetIntegral() const; double GetDerivative() const; @@ -80,33 +85,39 @@ class PIDController double GetMaxIntegralEffort() const; double GetOutputRampRate() const; double GetOutputFilter() const; - double GetDirection() 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. - bool m_bFirstCalculation; // Whether or not this is the first iteration of the control loop. - bool m_bReversed; // Operating direction of the controller. + + 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/drivers/DriveBoard.cpp b/src/drivers/DriveBoard.cpp index ce8d00d5..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. (0 - 360) 0 is North. + * @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. - diffdrive::DrivePowers stDrivePowers; - - // Check what kinematics model we should use. - switch (eKinematicsMethod) - { - case eArcadeDrive: stDrivePowers = diffdrive::CalculateArcadeDrive(double(fSpeed), double(fAngle), constants::DRIVE_MIN_POWER); break; - case eCurvatureDrive: - stDrivePowers = 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(stDrivePowers.dLeftDrivePower); - m_fTargetSpeedRight = double(stDrivePowers.dRightDrivePower); + // 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/util/NumberOperations.hpp b/src/util/NumberOperations.hpp index b995753a..516428ea 100755 --- a/src/util/NumberOperations.hpp +++ b/src/util/NumberOperations.hpp @@ -107,5 +107,34 @@ namespace numops // Return new mapped value. 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/util/NumberOperations.cc b/tests/Unit/src/util/NumberOperations.cc index ab45823b..2b1bbe79 100644 --- a/tests/Unit/src/util/NumberOperations.cc +++ b/tests/Unit/src/util/NumberOperations.cc @@ -107,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]); + } +} From 809ce67b504a8031ea702d3c147ef08e76f9d0be Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Fri, 20 Oct 2023 17:14:26 +0000 Subject: [PATCH 19/19] Made requested PR changes. --- .devcontainer/devcontainer.json | 12 ++++++------ src/algorithms/controllers/PIDController.cpp | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 43d0208d..59c1296f 100755 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -28,13 +28,13 @@ "source=${localWorkspaceFolder}/data/models/zed,target=/usr/local/zed/resources,type=bind,consistency=delegated" ], // "image": "ghcr.io/missourimrdt/autonomy-jammy:latest", - // "image": "ghcr.io/missourimrdt/autonomy-focal:latest", + "image": "ghcr.io/missourimrdt/autonomy-focal:latest", // "image": "ghcr.io/missourimrdt/autonomy-jetpack:latest", - "build": { - // "dockerfile": "Dockerfile_Jammy" - "dockerfile": "Dockerfile_Focal" - // "dockerfile": "Dockerfile_JetPack" - }, + // "build": { + // // "dockerfile": "Dockerfile_Jammy" + // // "dockerfile": "Dockerfile_Focal" + // // "dockerfile": "Dockerfile_JetPack" + // }, // Features to add to the dev container. More info: https://containers.dev/features. // "features": {}, // Use 'forwardPorts' to make a list of ports inside the container available locally. diff --git a/src/algorithms/controllers/PIDController.cpp b/src/algorithms/controllers/PIDController.cpp index c3754b27..13bf29d6 100644 --- a/src/algorithms/controllers/PIDController.cpp +++ b/src/algorithms/controllers/PIDController.cpp @@ -197,7 +197,7 @@ double PIDController::Calculate(const double dActual) /****************************************************************************** * @brief Calculates the control output using the last provided setpoint and actual. * - * @return double - + * @return double - The resultant PID controller output. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-10-17