diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..5775c8c --- /dev/null +++ b/.dockerignore @@ -0,0 +1,3 @@ +build +.travis.yml +.git diff --git a/.gitmodules b/.gitmodules index af705d2..e7639d1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -22,3 +22,18 @@ [submodule "external/serial"] path = external/serial url = git@github.com:utk-robotics-2017/serial.git +[submodule "external/eigen"] + path = external/eigen + url = git@github.com:utk-robotics-2017/eigen.git +[submodule "external/g2o"] + path = external/g2o + url = git@github.com:utk-robotics-2017/g2o.git +[submodule "external/suitesparse"] + path = external/suitesparse + url = git@github.com:utk-robotics-2017/suitesparse.git +[submodule "external/optional-lite"] + path = external/optional-lite + url = git@github.com:utk-robotics-2017/optional-lite.git +[submodule "external/args"] + path = external/args + url = git@github.com:utk-robotics-2017/args.git diff --git a/.travis.yml b/.travis.yml index fc1183b..aef4d99 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,35 +1,36 @@ dist: trusty -sudo: false +sudo: required + +services: + - docker language: cpp compiler: - gcc - - clang install: - if [ "$CXX" = "g++" ]; then export CXX="g++-4.9" CC="gcc-4.9"; fi - -addons: - apt: - sources: - - ubuntu-toolchain-r-test - packages: - - g++-4.9 - - gcc-4.9 - - clang - - cppcheck - - cmake - - lcov - - libssh2-1-dev - +- docker pull utkrobotics/rip_deps before_script: - - ./travis/before_script.sh $TRAVIS_BRANCH $TRAVIS_PULL_REQUEST $TRAVIS_PULL_REQUEST_BRANCH + # get cached images from + - docker build --tag rip_ci . +# - ./travis/before_script.sh $TRAVIS_BRANCH $TRAVIS_PULL_REQUEST $TRAVIS_PULL_REQUEST_BRANCH +# - ./travis/compile_g2o.sh +# start the container persistence: + - ci_env=`bash <(curl -s https://codecov.io/env)` + - docker run --name rip_ci $ci_env -t -d rip_ci zsh -l +# run setup in container: + - docker exec -t rip_ci zsh -c "./travis/before_script.sh $TRAVIS_BRANCH $TRAVIS_PULL_REQUEST $TRAVIS_PULL_REQUEST_BRANCH" + - docker start rip_ci script: - - ./travis/script.sh $TRAVIS_BRANCH $TRAVIS_PULL_REQUEST $TRAVIS_PULL_REQUEST_BRANCH + - docker exec -t rip_ci zsh -c "./travis/script.sh $TRAVIS_BRANCH $TRAVIS_PULL_REQUEST $TRAVIS_PULL_REQUEST_BRANCH" after_script: - - ./travis/after_script.sh $TRAVIS_BRANCH $TRAVIS_PULL_REQUEST $TRAVIS_PULL_REQUEST_BRANCH +# - ./travis/after_script.sh $TRAVIS_BRANCH $TRAVIS_PULL_REQUEST $TRAVIS_PULL_REQUEST_BRANCH + - docker exec -t rip_ci zsh -c "./travis/after_script.sh $TRAVIS_BRANCH $TRAVIS_PULL_REQUEST $TRAVIS_PULL_REQUEST_BRANCH" + - docker stop rip_ci + - docker rm rip_ci diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ddd2de..3fa2175 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,76 +1,27 @@ cmake_minimum_required(VERSION 3.1) project(rip) message(building) -# Get the current working branch -execute_process( - COMMAND git rev-parse --abbrev-ref HEAD - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - OUTPUT_VARIABLE GIT_BRANCH - OUTPUT_STRIP_TRAILING_WHITESPACE -) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Can remove this if found to be not needed - was required on some machines # to correctly compile the codebase despite this line being in the # downstream CMake files. set (CMAKE_CXX_STANDARD 11) - -message("Git branch: ${GIT_BRANCH}") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}-fdiagnostics-color=always") LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules) +option(ENABLE_TESTING "Enable module testing" OFF) + add_subdirectory(external) +add_subdirectory(core) +add_subdirectory(appendages) +add_subdirectory(arduino_gen) -if(GIT_BRANCH MATCHES "arduino_gen/.*") - add_subdirectory(core/utilities/exceptions_base) - add_subdirectory(arduino_gen) - add_subdirectory(gui) -else() - add_subdirectory(core) - add_subdirectory(appendages) - add_subdirectory(arduino_gen) + +option(GUI "Build the GUI" OFF) +if(GUI) + message("Building the GUI") add_subdirectory(gui) endif() - -#option(ALL "Build everything" OFF) -# -## RIP Modules -#option(RIP "Build all of RIP" OFF) -#option(COMMUNICATION "Build RIP's communication module" OFF) -#option(NAVIGATION "Build RIP's navigation module" OFF) -#option(NAVX "Build RIP's navx submodule" OFF) -#option(PATHFINDER "Build RIP's pathfinder submodule" OFF) -#option(VISION "Build RIP's vision module" OFF) -#option(FSM "Build RIP's fsm and action scheduler modules" OFF) -# -## Other Modules -#option(RIPCOM "Build RIPCOM" OFF) -#option(ARDUINO_GEN "Build ArduinoGen" OFF) -# -## Utilities -#option(ROBOCLAW "Build Roboclaw" OFF) -#option(PATHMAN "Build Pathman" OFF) -#option(CMD_MESENGER "Build CmdMessenger" OFF) -# -## Testing -#option(ENABLE_TESTING "Enable testing of the modules" OFF) -# -#add_subdirectory(external) -# -#if(ALL OR RIP OR CORE OR RIPCOM) -# message("Adding appendages") -# add_subdirectory(appendages) -#endif() -# -#if(ALL OR ARDUINO_GEN) -# message("Adding ArduinoGen") -# add_subdirectory(arduino_gen) -#endif() -# -#if(ALL OR RIP OR COMMUNICATION OR NAVIGATION OR NAVX OR PATHFINDER OR VISION OR FSM) -# message("Adding RIP Core") -# add_subdirectory(core) -#endif() -# -#if(ALL OR RIPCOM) -# message("Adding RIPCOM") -# add_subdirectory(ripcom) diff --git a/CMakeModules/FileOutputs.cmake b/CMakeModules/FileOutputs.cmake new file mode 100644 index 0000000..a2effeb --- /dev/null +++ b/CMakeModules/FileOutputs.cmake @@ -0,0 +1,38 @@ +#Macro: make_outputs +#Parameters: rootdir Used for making relative paths +# files Files to be copied +# outdir Destination directory for the files +#Returns: outputs Destination file paths +# +#Use in conjunction with a custom target and add_dependencies to copy files +# when they are changed. +# +#Example usage: +# make_outputs("/home/user/project/data" +# "${data_files}" +# ${CMAKE_CURRENT_BINARY_DIR}/data +# outputs) +# add_executable(my_exe ... ${outputs}) +macro(make_outputs rootdir files outdir outputs) + set(_outputs "") + foreach(file ${files}) + if((NOT (IS_ABSOLUTE ${file})) AND (EXISTS "${rootdir}/${file}")) + set(file "${rootdir}/${file}") #File is not absolute, but is relative to rootdir + elseif((NOT (IS_ABSOLUTE ${file})) AND (NOT (EXISTS "${rootdir}/${file}"))) + message(SEND_ERROR "Filepath '${file}' is not absolute and is not relative to '${rootdir}'") + return() + endif() + file(RELATIVE_PATH file_name ${rootdir} ${file}) + + add_custom_command( + OUTPUT ${outdir}/${file_name} + COMMAND ${CMAKE_COMMAND} -E copy_if_different + ${file} + ${outdir}/${file_name} + DEPENDS ${file} + COMMENT "Copying ${file_name}" + VERBATIM) + list(APPEND _outputs ${outdir}/${file_name}) + endforeach() + set(${outputs} ${_outputs}) +endmacro() diff --git a/Docker.md b/Docker.md new file mode 100644 index 0000000..17d33b8 --- /dev/null +++ b/Docker.md @@ -0,0 +1,63 @@ +# RIP and Docker + +How to use Docker to manage building and testing RIP. + +## Installing Docker + +You need to be using a Linux system in order to use Docker, on Ubuntu + +If you're on Ubuntu there are docker packages available in the multiverse, but the CE version from [the official website](https://docs.docker.com/install/linux/docker-ce/ubuntu/) will perform much better and offers newer features. + +On MacOS, docker is available and will automatically run a Linux VM for you. On windows, you *can't* use the subsystem, since Docker depends on Linux kernel namespaces. + +Once installed, be sure to do whatever group management to your account so that you can run docker without the need of sudo. (For CE, this is something like `adduser $USER docker` and then rebooting.) + +## The Dependency Dockerfile + +Right now there are two dockerfiles, one that builds the dependencies (onto an Ubuntu base), and one that copies the source code of RIP. + +The dependency image is sourced from `external/Dockerfile` and is built automatically by dockerhub, as `utkrobotics/rip_deps`. + +To pull the latest deps image: + +```docker pull utkrobotics/rip_deps``` + +This includes g2o, eigen, suitesparse, etc, whatever is needed to build. + +## The building / interactive Dockerfile + +This is the Dockerfile in the root of the repo. + +It's based off the `rip_deps` image so it will have everything needed to build already installed. + +Upon building this container image, it will copy all the source code from your current directory into the container and set up an environment ready to run the build. + +To build the interactive container: + +``` +docker build --tag utkrobotics/rip:$(git symbolic-ref HEAD|cut -d'/' -f3-|sed -e 's;/;_;') . +``` + +This will create an image `utkrobotics/rip:yourbranch` which we can then run and create an instance of: + +``` +# --rm : removes the container when you exit it +# -t -i : creates an interactive container +# zsh -l : the command to run inside the container (use whatever your preferred shell is) +docker run --rm -t -i utkrobotics/rip:$(git symbolic-ref HEAD|cut -d'/' -f3-|sed -e 's;/;_;') zsh -l +``` + +Now that you're inside the container, you can immediately build it, mess around, do whatever, because everything you do inside the container is removed when you exit it, and it never affects the code outside the container. + +Normally, what I do is: + +``` +# build dat stuff +./build-linux.sh +# runs the built unit tests +./travis/script.sh +``` + +While you're in the container, feel free to do whatever you want, `rm -rf *`, or anything. Just remember if you make changes that you want to keep inside the container, you should make those same changes again outside the container on your local git repo. + +If you've made changed to your local git repo, you should run the build command again to update the image with your new changes. diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..d0e2b8a --- /dev/null +++ b/Dockerfile @@ -0,0 +1,8 @@ +FROM utkrobotics/rip_deps:latest +# $LUSER is user with 901:901 from home image + +# copy in RIP source code +COPY . /home/${LUSER}/code/rip +RUN sudo chown -R 901:901 /home/${LUSER}/code/rip + +WORKDIR /home/${LUSER}/code/rip diff --git a/README.md b/README.md index ab84050..4eccf0b 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,27 @@ # RIP +[![codecov](https://codecov.io/gh/utk-robotics-2017/rip/branch/master/graph/badge.svg?token=KqhG5MRr9F)](https://codecov.io/gh/utk-robotics-2017/rip) + +*Version 0.1* + The Robotics Integrated Platform is designed as an all encompassing library for small autonomous robots using a raspberry pi or beaglebone as the primary controller. +## Dependencies +There are two ways to deal with the dependencies of RIP. + +1. Use the provided Docker. +2. Install the packages natively on your local machine. + +### Docker +See the [Docker Readme](Docker.md). + +### Native Install +RIP requires several dependencies -- namely Eigen 3, Suitesparse, G2O, CMake, and GCC/G++ 4.9+. RIP GUIs require QT5. + ## How to Contribute +All contributions should be made through our [Git workflow](https://github.com/utk-robotics-2017/rip/wiki/Git-Workflow) and following our [coding standards](https://github.com/utk-robotics-2017/rip/wiki/Coding-Standards). + +Once your contributions are ready to be reviewed, please open a pull request to the dev branch. Ideally, mark your PR with the associated issues and the target milestone/version of RIP. ## Quick Start @@ -27,21 +46,20 @@ If you'll be working on Arduino Gen, you would switch to one of the `arduino_gen ### Setup CMake 1. Make a directory for the output files (i.e. `build` or `bin`), then go into it. -2. Build for what you're working on. - Replace `` with what you're building for. i.e. Arduino Gen is `ARDUINO_GEN`. - * Linux: `cmake -D =yes ..` - * Windows: `cmake -G "Unix Makefiles" -D =yes ..` +2. Build the project. + * Linux: `cmake ..` + * Windows: `cmake -G "Unix Makefiles" ..` ### Make Type in `make` and you should start building rip, arduino_gen, etc. Make can be sped up by passing `-j`, where is `N` is the number of cores on your computer. -## Core - -## ArduionGen +### Build Script +Optionally, rather than setting up the CMake build yourself, you may opt to use the included `build-linux.sh` script which will automatically build RIP in `build/` with testing enabled. -## Appendages +## License +**The RIP License (Revision 0.3)** -## Utilities +This software is available without warranty and without support. Use at your own risk. Literally. It might delete your filesystem or eat your cat. As long as you retain this notice, you can do whatever you want with this. If we meet some day, you owe me a beer. -## License +Go Vols! diff --git a/appendages/CMakeLists.txt b/appendages/CMakeLists.txt index c5b5894..064b31c 100644 --- a/appendages/CMakeLists.txt +++ b/appendages/CMakeLists.txt @@ -12,34 +12,27 @@ target_link_libraries(${PROJECT_NAME} cmd_messenger misc) target_include_directories(${PROJECT_NAME} PUBLIC include) -# TODO(Anthony): Refactor to a function or macro -file(GLOB_RECURSE ${PROJECT_NAME}_XMLS "xml/*.xml") -set(copiedXmls "") -foreach(template ${${PROJECT_NAME}_XMLS}) - file(RELATIVE_PATH name ${CMAKE_CURRENT_SOURCE_DIR} ${template}) - add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${name} - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${template} ${CMAKE_CURRENT_BINARY_DIR}/${name} - DEPENDS ${template} - COMMENT "Copying ${name}" - VERBATIM) - list(APPEND copiedXmls ${CMAKE_CURRENT_BINARY_DIR}/${name}) -endforeach() -add_custom_target(AppendageXmls SOURCES ${copiedXmls}) - -file(GLOB_RECURSE ${PROJECT_NAME}_JSONS "json/*.json") -set(copiedJsons "") -foreach(template ${${PROJECT_NAME}_JSONS}) - file(RELATIVE_PATH name ${CMAKE_CURRENT_SOURCE_DIR} ${template}) - add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${name} - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${template} ${CMAKE_CURRENT_BINARY_DIR}/${name} - DEPENDS ${template} - COMMENT "Copying ${name}" - VERBATIM) - list(APPEND copiedJsons ${CMAKE_CURRENT_BINARY_DIR}/${name}) -endforeach() -add_custom_target(AppendageJsons SOURCES ${copiedJsons}) - -add_custom_target(AppendageTemplates) -add_dependencies(AppendageTemplates AppendageXmls AppendageJsons) +set_property(GLOBAL PROPERTY AppendageRoot ${CMAKE_CURRENT_SOURCE_DIR}) + +file(GLOB _AppendageXmls "xml/*.xml") +set_property(GLOBAL PROPERTY AppendageXmls ${_AppendageXmls}) +file(GLOB _AppendageJsons "json/*.json") +set_property(GLOBAL PROPERTY AppendageJsons ${_AppendageJsons}) + +include(FileOutputs) + +function(make_appendage_xml_outputs outdir xml_outputs) + get_property(_AppendageRoot GLOBAL PROPERTY AppendageRoot) + get_property(_AppendageXmls GLOBAL PROPERTY AppendageXmls) + + make_outputs(${_AppendageRoot} "${_AppendageXmls}" ${outdir} _outputs) + set(${xml_outputs} ${_outputs} PARENT_SCOPE) +endfunction() + +function(make_appendage_json_outputs outdir json_outputs) + get_property(_AppendageRoot GLOBAL PROPERTY AppendageRoot) + get_property(_AppendageJsons GLOBAL PROPERTY AppendageJsons) + + make_outputs(${_AppendageRoot} "${_AppendageJsons}" ${outdir} _outputs) + set(${json_outputs} ${_outputs} PARENT_SCOPE) +endfunction() diff --git a/appendages/include/analog_input.hpp b/appendages/include/appendages/analog_input.hpp similarity index 87% rename from appendages/include/analog_input.hpp rename to appendages/include/appendages/analog_input.hpp index 9c3605c..5e0ef62 100644 --- a/appendages/include/analog_input.hpp +++ b/appendages/include/appendages/analog_input.hpp @@ -5,9 +5,10 @@ #include #include -#include -#include -#include +#include "appendages/appendage.hpp" + +#include +#include namespace rip { @@ -53,8 +54,8 @@ namespace rip */ AnalogInput(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); - cmdmessenger::Command m_read; - cmdmessenger::Command m_read_result; + std::shared_ptr m_read; + std::shared_ptr m_read_result; }; // class AnalogInput } // namespace appendages } diff --git a/appendages/include/appendage.hpp b/appendages/include/appendages/appendage.hpp similarity index 91% rename from appendages/include/appendage.hpp rename to appendages/include/appendages/appendage.hpp index a851ac3..bd24e70 100644 --- a/appendages/include/appendage.hpp +++ b/appendages/include/appendages/appendage.hpp @@ -6,8 +6,8 @@ #include #include -#include -#include +#include +#include namespace rip { @@ -85,7 +85,7 @@ namespace rip * @see {@link CmdMessenger} * @see {@link Command} */ - cmdmessenger::Command createCommand(const std::string& command_key, const std::map& command_map, const std::string& parameter_string); + std::shared_ptr createCommand(const std::string& command_key, const std::map& command_map, const std::string& parameter_string); std::shared_ptr m_device; std::string m_label; diff --git a/appendages/include/appendage_factory.hpp b/appendages/include/appendages/appendage_factory.hpp similarity index 77% rename from appendages/include/appendage_factory.hpp rename to appendages/include/appendages/appendage_factory.hpp index 5084346..3869e22 100644 --- a/appendages/include/appendage_factory.hpp +++ b/appendages/include/appendages/appendage_factory.hpp @@ -52,15 +52,15 @@ namespace rip */ void registerAppendage(const std::string& type, std::function(const nlohmann::json&, - const std::map&, - std::shared_ptr) + const std::map&, + std::shared_ptr) > constructor - ); + ); std::map(const nlohmann::json&, - const std::map&, - std::shared_ptr) - >> m_constructors; + const std::map&, + std::shared_ptr) + >> m_constructors; }; } } diff --git a/appendages/include/digital_input.hpp b/appendages/include/appendages/digital_input.hpp similarity index 86% rename from appendages/include/digital_input.hpp rename to appendages/include/appendages/digital_input.hpp index 1cc3a1b..da8f6a0 100644 --- a/appendages/include/digital_input.hpp +++ b/appendages/include/appendages/digital_input.hpp @@ -6,10 +6,10 @@ #include -#include "command.hpp" +#include -#include -#include +#include "appendages/appendage.hpp" +#include "appendages/appendage_factory.hpp" #include @@ -55,8 +55,8 @@ namespace rip */ DigitalInput(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); - cmdmessenger::Command m_read; - cmdmessenger::Command m_read_result; + std::shared_ptr m_read; + std::shared_ptr m_read_result; }; } } diff --git a/appendages/include/appendages_exceptions.hpp b/appendages/include/appendages/exceptions.hpp similarity index 97% rename from appendages/include/appendages_exceptions.hpp rename to appendages/include/appendages/exceptions.hpp index 57cf7f4..c7ddb44 100644 --- a/appendages/include/appendages_exceptions.hpp +++ b/appendages/include/appendages/exceptions.hpp @@ -19,7 +19,7 @@ */ #ifndef EXCEPTIONS_HPP #define EXCEPTIONS_HPP -#include +#include namespace rip { diff --git a/appendages/include/servo.hpp b/appendages/include/appendages/servo.hpp similarity index 89% rename from appendages/include/servo.hpp rename to appendages/include/appendages/servo.hpp index 5d0f80f..30ccd70 100644 --- a/appendages/include/servo.hpp +++ b/appendages/include/appendages/servo.hpp @@ -6,10 +6,10 @@ #include -#include "command.hpp" +#include -#include -#include +#include "appendages/appendage.hpp" +#include "appendages/appendage_factory.hpp" #include @@ -54,7 +54,7 @@ namespace rip */ Servo(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); - cmdmessenger::Command m_write; + std::shared_ptr m_write; }; } } diff --git a/appendages/include/ultrasonic.hpp b/appendages/include/appendages/ultrasonic.hpp similarity index 86% rename from appendages/include/ultrasonic.hpp rename to appendages/include/appendages/ultrasonic.hpp index 51799f1..d45cadd 100644 --- a/appendages/include/ultrasonic.hpp +++ b/appendages/include/appendages/ultrasonic.hpp @@ -6,10 +6,10 @@ #include -#include "command.hpp" +#include -#include -#include +#include "appendages/appendage.hpp" +#include "appendages/appendage_factory.hpp" #include @@ -54,8 +54,8 @@ namespace rip */ Ultrasonic(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); - cmdmessenger::Command m_read; - cmdmessenger::Command m_read_result; + std::shared_ptr m_read; + std::shared_ptr m_read_result; }; } } diff --git a/appendages/src/analog_input.cpp b/appendages/src/analog_input.cpp index 84e0b21..f412c92 100644 --- a/appendages/src/analog_input.cpp +++ b/appendages/src/analog_input.cpp @@ -1,10 +1,10 @@ -#include "analog_input.hpp" +#include "appendages/analog_input.hpp" #include #include #include -#include +#include namespace rip { diff --git a/appendages/src/appendage.cpp b/appendages/src/appendage.cpp index ae8732f..081fa4d 100644 --- a/appendages/src/appendage.cpp +++ b/appendages/src/appendage.cpp @@ -1,7 +1,7 @@ -#include "appendage.hpp" +#include "appendages/appendage.hpp" #include -#include "appendages_exceptions.hpp" +#include "appendages/exceptions.hpp" namespace rip { @@ -27,13 +27,13 @@ namespace rip assert(false); } - cmdmessenger::Command Appendage::createCommand(const std::string& command_key, const std::map& command_map, const std::string& parameter_string) + std::shared_ptr Appendage::createCommand(const std::string& command_key, const std::map& command_map, const std::string& parameter_string) { if (command_map.find(command_key) == command_map.end()) { throw CommandNotFound(fmt::format("Cannot find {} for appendage of type {}", command_key, m_type)); } - return cmdmessenger::Command(command_key, command_map.find(command_key)->second, parameter_string); + return std::make_shared(command_key, command_map.find(command_key)->second, parameter_string); } Appendage::Appendage(const nlohmann::json& config, std::shared_ptr device) @@ -52,7 +52,7 @@ namespace rip } m_label = config["label"]; - if(config.find("id") == config.end()) + if (config.find("id") == config.end()) { throw AppendageWithId(fmt::format("appendage missing id")); } diff --git a/appendages/src/appendage_factory.cpp b/appendages/src/appendage_factory.cpp index c5f7157..ae785d3 100644 --- a/appendages/src/appendage_factory.cpp +++ b/appendages/src/appendage_factory.cpp @@ -1,12 +1,12 @@ -#include "appendage_factory.hpp" +#include "appendages/appendage_factory.hpp" #include // Appendages -#include "digital_input.hpp" -#include "analog_input.hpp" +#include "appendages/digital_input.hpp" +#include "appendages/analog_input.hpp" -#include "appendages_exceptions.hpp" +#include "appendages/exceptions.hpp" namespace rip { @@ -34,9 +34,9 @@ namespace rip } void AppendageFactory::registerAppendage(const std::string& type, std::function(const nlohmann::json&, - const std::map&, - std::shared_ptr) - > constructor) + const std::map&, + std::shared_ptr) + > constructor) { m_constructors[type] = constructor; } diff --git a/appendages/src/digital_input.cpp b/appendages/src/digital_input.cpp index 431ad58..ff5c8d4 100644 --- a/appendages/src/digital_input.cpp +++ b/appendages/src/digital_input.cpp @@ -1,10 +1,10 @@ -#include "digital_input.hpp" +#include "appendages/digital_input.hpp" #include #include #include -#include "cmd_messenger.hpp" +#include namespace rip { diff --git a/appendages/src/servo.cpp b/appendages/src/servo.cpp index 4537aca..e20d711 100644 --- a/appendages/src/servo.cpp +++ b/appendages/src/servo.cpp @@ -1,10 +1,10 @@ -#include "servo.hpp" +#include "appendages/servo.hpp" #include #include #include -#include +#include namespace rip { diff --git a/appendages/src/ultrasonic.cpp b/appendages/src/ultrasonic.cpp index 5ddd5ba..20270b5 100644 --- a/appendages/src/ultrasonic.cpp +++ b/appendages/src/ultrasonic.cpp @@ -1,10 +1,10 @@ -#include "ultrasonic.hpp" +#include "appendages/ultrasonic.hpp" #include #include #include -#include +#include namespace rip { diff --git a/arduino_gen/CMakeLists.txt b/arduino_gen/CMakeLists.txt index db76a2f..1604f43 100644 --- a/arduino_gen/CMakeLists.txt +++ b/arduino_gen/CMakeLists.txt @@ -6,21 +6,25 @@ file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES "src/*.cpp") list(REMOVE_ITEM ${PROJECT_NAME}_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp") # Create the arduino_gen library, set to c++11, link external libraries, and set include dir -add_library(_${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${copiedAppendageTemplates}) +add_library(_${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES}) set_property(TARGET _${PROJECT_NAME} PROPERTY CXX_STANDARD 11) if(ENABLE_TESTING) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage -DTESTING") endif() -target_link_libraries(_${PROJECT_NAME} fmt json spdlog tinyxml2 cppfs exceptions_base) +target_link_libraries(_${PROJECT_NAME} fmt json spdlog tinyxml2 cppfs misc) target_link_libraries(_${PROJECT_NAME} gtest) target_include_directories(_${PROJECT_NAME} PUBLIC include) +include(FileOutputs) + +make_outputs(${CMAKE_CURRENT_SOURCE_DIR} "code_template.txt" ${CMAKE_CURRENT_BINARY_DIR} code_template) +make_appendage_xml_outputs(${CMAKE_CURRENT_BINARY_DIR}/appendages appendage_xml_outputs) +make_appendage_json_outputs(${CMAKE_CURRENT_BINARY_DIR}/appendages appendage_json_outputs) + # Create arduino_gen executable, set to c++11, and link arduino_gen library -add_executable(${PROJECT_NAME} "src/main.cpp") +add_executable(${PROJECT_NAME} "src/main.cpp" ${code_template} ${appendage_xml_outputs} ${appendage_json_outputs}) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) -target_link_libraries(${PROJECT_NAME} _${PROJECT_NAME}) -add_subdirectory(../appendages ./appendages) -add_dependencies(${PROJECT_NAME} AppendageTemplates) +target_link_libraries(${PROJECT_NAME} _${PROJECT_NAME} args fmt) if(ENABLE_TESTING) @@ -29,26 +33,11 @@ if(ENABLE_TESTING) file(GLOB_RECURSE ${PROJECT_NAME}_TEST_DATA "test/data/*") # Copy test data to outdir/test - set(copiedTestData "") - foreach(TestDataFile ${${PROJECT_NAME}_TEST_DATA}) - file(RELATIVE_PATH name ${CMAKE_CURRENT_SOURCE_DIR} ${TestDataFile}) - add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${name} - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${TestDataFile} ${CMAKE_CURRENT_BINARY_DIR}/${name} - DEPENDS ${TestDataFile} - COMMENT "Copying ${name}" - VERBATIM) - list(APPEND copiedTestData ${CMAKE_CURRENT_BINARY_DIR}/${name}) - endforeach() + make_outputs(${CMAKE_CURRENT_SOURCE_DIR} "${${PROJECT_NAME}_TEST_DATA}" ${CMAKE_CURRENT_BINARY_DIR} testDataOutputs) # Create arduino_gen_test executable, set to c++11, link arduino_gen library and google test libraries - add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES} ${copiedTestData}) + add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES} ${testDataOutputs}) set_property(TARGET ${PROJECT_NAME}_test PROPERTY CXX_STANDARD 11) target_link_libraries(${PROJECT_NAME}_test _${PROJECT_NAME}) target_link_libraries(${PROJECT_NAME}_test gmock gtest googletest_rip_macros) - - if(CMAKE_COMPILER_IS_GNUCXX) - include(CodeCoverage) - setup_target_for_coverage(${PROJECT_NAME}_coverage ${PROJECT_NAME}_test coverage) - endif() endif() diff --git a/arduino_gen/README.md b/arduino_gen/README.md index d35bcaf..dd644e9 100644 --- a/arduino_gen/README.md +++ b/arduino_gen/README.md @@ -1,2 +1,4 @@ -# arduino_gen_cpp -A C++ library that generates Arduino Code based on a config file +# Arduino Gen +*Now in C++11* + +Arduino Gen is a C++11 library within RIP to generate Arduino/Teensy code from templates and config files. It provides native integration with the RIP appendage system. diff --git a/arduino_gen/code_template.txt b/arduino_gen/code_template.txt new file mode 100644 index 0000000..2ccebfb --- /dev/null +++ b/arduino_gen/code_template.txt @@ -0,0 +1,79 @@ +// Auto-generated by ArduinoGen + +#include "CmdMessenger.h" +{includes} + +// Attach a new CmdMessenger object to the default Serial port +CmdMessenger cmdMessenger = CmdMessenger(Serial); + +const char LED = 13; + +{constructors} + +enum +{{ +{command_enums} +}}; + +void setup() +{{ + // Init LED pin + pinMode(LED, OUTPUT); + + // Initialize Serial Communication + Serial.begin(115200); + + attachCommandCallbacks(); + +{setup} + + // Flash led 3 times at the end of setup + for(int i = 0; i < 3; i++) + {{ + digitalWrite(LED, HIGH); + delay(250); + digitalWrite(LED, LOW); + delay(250); + }} +}} + +void loop() +{{ + // Process incoming serial data, and perform callbacks + cmdMessenger.feedinSerialData(); + +{loop} +}} + +//Callbacks define on which received commands we take action +void attachCommandCallbacks() +{{ + cmdMessenger.attach(unknownCommand); + cmdMessenger.attach(kPing, ping); + cmdMessenger.attach(kSetLed, setLed); +{command_attaches} +}} + +// Called when a received command has no attached function +void unknownCommand() +{{ + cmdMessenger.sendCmd(kError, kUnknown); +}} + +// Called upon initialization of Spine to check connection +void ping() +{{ + cmdMessenger.sendBinCmd(kAcknowledge, kPing); + cmdMessenger.sendBinCmd(kPingResult, kPong); +}} + +// Callback function that sets led on or off +void setLed() +{{ + // Read led state argument, interpret string as boolean + bool ledState = cmdMessenger.readBoolArg(); + digitalWrite(LED, ledState); + cmdMessenger.sendBinCmd(kAcknowledge, kSetLed); +}} + +{command_callbacks} diff --git a/arduino_gen/include/appendage.hpp b/arduino_gen/include/arduino_gen/appendage.hpp similarity index 95% rename from arduino_gen/include/appendage.hpp rename to arduino_gen/include/arduino_gen/appendage.hpp index 055ccd4..9bc0c21 100644 --- a/arduino_gen/include/appendage.hpp +++ b/arduino_gen/include/arduino_gen/appendage.hpp @@ -26,7 +26,7 @@ namespace rip * @param test Whether to test the type or not (default: true) */ Appendage(nlohmann::json data, - std::multimap>& appendages, + std::multimap>& appendages, std::string appendage_data_folder = "appendages", bool test = true); /** @@ -74,6 +74,11 @@ namespace rip */ bool isType(std::string data_name, std::string type) const; + /* + * TODO: Properly document + */ + nlohmann::json getCoreJson(int index) const; + private: /** * @brief Tests whether the type has all the parameters specified in a json file. diff --git a/arduino_gen/include/appendage_template.hpp b/arduino_gen/include/arduino_gen/appendage_template.hpp similarity index 100% rename from arduino_gen/include/appendage_template.hpp rename to arduino_gen/include/arduino_gen/appendage_template.hpp diff --git a/arduino_gen/include/arduino_gen.hpp b/arduino_gen/include/arduino_gen/arduino_gen.hpp similarity index 53% rename from arduino_gen/include/arduino_gen.hpp rename to arduino_gen/include/arduino_gen/arduino_gen.hpp index b5eb992..b734a74 100644 --- a/arduino_gen/include/arduino_gen.hpp +++ b/arduino_gen/include/arduino_gen/arduino_gen.hpp @@ -8,6 +8,8 @@ #include #include +#include "appendage_template.hpp" + namespace rip { namespace arduinogen @@ -25,36 +27,36 @@ namespace rip /** * @brief Constructor */ - ArduinoGen(std::string arduino, std::string parent_folder, std::string appendage_data_folder = "appendages", bool testing = false); + ArduinoGen(std::string arduino, std::string parent_folder, std::string current_arduino_code_dir = "/Robot/CurrentArduinoCode", std::string appendage_data_folder = "appendages"); /** * @brief Reads the config file with the appendages * * @param filepath The filepath to the appendages config file to read - * @param copy Whether to copy the appendages config file into the output folder */ - void readConfig(std::string filepath, bool copy = true); + void readConfig(std::string filepath); /** * @brief Writes the Arduino code, build script, and the config file used by RIP */ - void generateOutput(); + void generateOutput(bool copyOldFiles); private: /** - * @brief Removes the device folder if it exists and creates a new one + * TODO: Properly comment + * TODO: Make private again */ - void setupFolder(); + std::string getArduinoCode(); /** * TODO: Properly comment */ - std::string getArduinoCode(); + void loadTemplates(); /** * TODO: Properly comment */ - void loadTemplates(); + void loadCommandEnums(); /** * @brief Returns the includes for the Arduino Code @@ -99,23 +101,28 @@ namespace rip std::string getCommandCallbacks(); /** - * @brief Returns the code for all the extra functions - * @returns The code for all the extra functions + * TODO: Properly comment */ - std::string getExtras(); + std::string getCoreConfig(); /** * TODO: Properly comment */ - std::string getCoreConfig(); + std::string getUploadScript(); /** * TODO: Properly comment */ - std::string getUploadScript(); + std::string getSerialScript(); + + /** + * TODO: Properly comment + */ + std::string getPlatformIo(); std::string m_arduino; std::string m_parent_folder; + std::string m_current_arduino_code_dir; std::map m_commands; @@ -125,6 +132,9 @@ namespace rip std::vector m_appendage_templates; + std::string config; + std::string platformio_config; + #ifdef TESTING private: friend class ArduinoGenTest; @@ -140,6 +150,54 @@ namespace rip FRIEND_TEST(ArduinoGenTest, constructors_one_appendage); FRIEND_TEST(ArduinoGenTest, constructors_two_appendages_same); FRIEND_TEST(ArduinoGenTest, constructors_two_appendages_different); + + FRIEND_TEST(ArduinoGenTest, setup_no_appendages); + FRIEND_TEST(ArduinoGenTest, setup_one_empty_appendage); + FRIEND_TEST(ArduinoGenTest, setup_one_appendage); + FRIEND_TEST(ArduinoGenTest, setup_two_appendages_same); + FRIEND_TEST(ArduinoGenTest, setup_two_appendages_different); + + FRIEND_TEST(ArduinoGenTest, loop_no_appendages); + FRIEND_TEST(ArduinoGenTest, loop_one_empty_appendage); + FRIEND_TEST(ArduinoGenTest, loop_one_appendage); + FRIEND_TEST(ArduinoGenTest, loop_two_appendages_same); + FRIEND_TEST(ArduinoGenTest, loop_two_appendages_different); + + FRIEND_TEST(ArduinoGenTest, command_enums_no_appendages); + FRIEND_TEST(ArduinoGenTest, command_enums_one_empty_appendage); + FRIEND_TEST(ArduinoGenTest, command_enums_one_appendage); + FRIEND_TEST(ArduinoGenTest, command_enums_two_appendages_same); + FRIEND_TEST(ArduinoGenTest, command_enums_two_appendages_different); + + FRIEND_TEST(ArduinoGenTest, command_attaches_no_appendages); + FRIEND_TEST(ArduinoGenTest, command_attaches_one_empty_appendage); + FRIEND_TEST(ArduinoGenTest, command_attaches_one_appendage); + FRIEND_TEST(ArduinoGenTest, command_attaches_two_appendages_same); + FRIEND_TEST(ArduinoGenTest, command_attaches_two_appendages_different); + + FRIEND_TEST(ArduinoGenTest, command_callbacks_no_appendages); + FRIEND_TEST(ArduinoGenTest, command_callbacks_one_empty_appendage); + FRIEND_TEST(ArduinoGenTest, command_callbacks_one_appendage); + FRIEND_TEST(ArduinoGenTest, command_callbacks_two_appendages_same); + FRIEND_TEST(ArduinoGenTest, command_callbacks_two_appendages_different); + + FRIEND_TEST(ArduinoGenTest, arduino_code_no_appendages); + FRIEND_TEST(ArduinoGenTest, arduino_code_one_empty_appendage); + FRIEND_TEST(ArduinoGenTest, arduino_code_one_appendage); + FRIEND_TEST(ArduinoGenTest, arduino_code_two_appendages_same); + FRIEND_TEST(ArduinoGenTest, arduino_code_two_appendages_different); + + FRIEND_TEST(ArduinoGenTest, get_core_config_no_appendages); + FRIEND_TEST(ArduinoGenTest, get_core_config_one_empty_appendage); + FRIEND_TEST(ArduinoGenTest, get_core_config_one_appendage); + FRIEND_TEST(ArduinoGenTest, get_core_config_two_appendages_same); + FRIEND_TEST(ArduinoGenTest, get_core_config_two_appendages_different); + + FRIEND_TEST(ArduinoGenTest, get_upload_script); + + FRIEND_TEST(ArduinoGenTest, setup_folder); + + FRIEND_TEST(ArduinoGenTest, generate_output); #endif }; } diff --git a/arduino_gen/include/argument.hpp b/arduino_gen/include/arduino_gen/argument.hpp similarity index 100% rename from arduino_gen/include/argument.hpp rename to arduino_gen/include/arduino_gen/argument.hpp diff --git a/arduino_gen/include/code.hpp b/arduino_gen/include/arduino_gen/code.hpp similarity index 100% rename from arduino_gen/include/code.hpp rename to arduino_gen/include/arduino_gen/code.hpp diff --git a/arduino_gen/include/command.hpp b/arduino_gen/include/arduino_gen/command.hpp similarity index 100% rename from arduino_gen/include/command.hpp rename to arduino_gen/include/arduino_gen/command.hpp diff --git a/arduino_gen/include/constructors.hpp b/arduino_gen/include/arduino_gen/constructor.hpp similarity index 78% rename from arduino_gen/include/constructors.hpp rename to arduino_gen/include/arduino_gen/constructor.hpp index 28ac651..45d2cde 100644 --- a/arduino_gen/include/constructors.hpp +++ b/arduino_gen/include/arduino_gen/constructor.hpp @@ -1,10 +1,13 @@ -#ifndef CONSTRUCTORS_HPP -#define CONSTRUCTORS_HPP +#ifndef CONSTRUCTOR_HPP +#define CONSTRUCTOR_HPP #include #include #include +#include "xml_element.hpp" +#include "argument.hpp" + namespace tinyxml2 { class XMLElement; @@ -14,25 +17,21 @@ namespace rip { namespace arduinogen { - class Argument; class Appendage; /** * @class Constructors * @brief A container for the constructors part of the arduino code for this type */ - class Constructors + class Constructor : private XmlElement { public: - Constructors(); - ~Constructors(); - /** * @brief Constructor * * @param xml The xml element to parse */ - Constructors(const tinyxml2::XMLElement* xml); + Constructor(const tinyxml2::XMLElement* xml); /** * @brief Returns the arduino code for these constructors @@ -44,11 +43,11 @@ namespace rip private: std::vector m_arguments; - bool m_exists; std::string m_type; std::string m_variable; + bool m_type_is_class; }; } // arduinogen } -#endif // CONSTRUCTORS_HPP +#endif // CONSTRUCTOR_HPP diff --git a/arduino_gen/include/arduino_gen/constructors.hpp b/arduino_gen/include/arduino_gen/constructors.hpp new file mode 100644 index 0000000..e3d9f98 --- /dev/null +++ b/arduino_gen/include/arduino_gen/constructors.hpp @@ -0,0 +1,36 @@ +#ifndef CONSTRUCTORS_HPP +#define CONSTRUCTORS_HPP + +#include "constructor.hpp" +#include "xml_element.hpp" + +#include + +namespace rip +{ + namespace arduinogen + { + class Constructors : private XmlElement + { + public: + /** + * @brief Constructor + * + * @param xml The xml element to parse + */ + Constructors(const tinyxml2::XMLElement* xml); + + /** + * @brief Returns the arduino code for these constructors + * @param appendages The appendages of this type + * @return The code for these constructors + */ + std::string toString(const std::vector>& appendages) const; + + private: + std::vector m_constructors; + }; + } +} + +#endif // CONSTRUCTORS_HPP diff --git a/arduino_gen/include/exceptions.hpp b/arduino_gen/include/arduino_gen/exceptions.hpp similarity index 79% rename from arduino_gen/include/exceptions.hpp rename to arduino_gen/include/arduino_gen/exceptions.hpp index affcbca..3a1c97e 100644 --- a/arduino_gen/include/exceptions.hpp +++ b/arduino_gen/include/arduino_gen/exceptions.hpp @@ -1,7 +1,7 @@ #ifndef EXCEPTIONS_HPP #define EXCEPTIONS_HPP #include -#include +#include namespace rip { @@ -11,37 +11,37 @@ namespace rip * @class AppendageDataException * @brief An exception for when there is an issue with data in an appendage */ - NEW_EX(AppendageDataException); + NEW_EX(AppendageDataException) /** * @class AttributeException * @brief An exception for when there is an xml attribute issue */ - NEW_EX(AttributeException); + NEW_EX(AttributeException) /** * @class ElementException * @brief An exception for when there is an xml element issue */ - NEW_EX(ElementException); + NEW_EX(ElementException) /** * @class TypeException * @brief An exception for when there is a type error */ - NEW_EX(TypeException); + NEW_EX(TypeException) /** * @class FileIoException * @brief An exception for when there is an error opening, reading, or writing a file. */ - NEW_EX(FileIoException); + NEW_EX(FileIoException) /** * @class PatternNotFoundException * @brief An exception for when a regex pattern was not found. */ - NEW_EX(PatternNotFoundException); + NEW_EX(PatternNotFoundException) } } diff --git a/arduino_gen/include/includes.hpp b/arduino_gen/include/arduino_gen/includes.hpp similarity index 69% rename from arduino_gen/include/includes.hpp rename to arduino_gen/include/arduino_gen/includes.hpp index 30432ce..c8c1850 100644 --- a/arduino_gen/include/includes.hpp +++ b/arduino_gen/include/arduino_gen/includes.hpp @@ -10,12 +10,6 @@ namespace rip class Includes : private XmlElement { public: - //Includes(); - //~Includes(); - - //Includes(const Includes& other) = delete; - //Includes& operator=(const Includes& other) = delete; - Includes(const tinyxml2::XMLElement* xml); std::vector GetIncludes(); diff --git a/arduino_gen/include/loop.hpp b/arduino_gen/include/arduino_gen/loop.hpp similarity index 100% rename from arduino_gen/include/loop.hpp rename to arduino_gen/include/arduino_gen/loop.hpp diff --git a/arduino_gen/include/parameter.hpp b/arduino_gen/include/arduino_gen/parameter.hpp similarity index 100% rename from arduino_gen/include/parameter.hpp rename to arduino_gen/include/arduino_gen/parameter.hpp diff --git a/arduino_gen/include/return_value.hpp b/arduino_gen/include/arduino_gen/return_value.hpp similarity index 100% rename from arduino_gen/include/return_value.hpp rename to arduino_gen/include/arduino_gen/return_value.hpp diff --git a/arduino_gen/include/setup.hpp b/arduino_gen/include/arduino_gen/setup.hpp similarity index 100% rename from arduino_gen/include/setup.hpp rename to arduino_gen/include/arduino_gen/setup.hpp diff --git a/arduino_gen/include/arduino_gen/utils.hpp b/arduino_gen/include/arduino_gen/utils.hpp new file mode 100644 index 0000000..2f27b97 --- /dev/null +++ b/arduino_gen/include/arduino_gen/utils.hpp @@ -0,0 +1,67 @@ +#ifndef UTILS_HPP +#define UTILS_HPP + +#include +#include +#include + +namespace rip +{ + namespace arduinogen + { + template + K select_first(std::pair pair) + { + return pair.first; + } + + template + V select_second(std::pair pair) + { + return pair.second; + } + + template + struct identity { typedef T type; }; + + template + std::vector get_map_keys(std::map map) + { + std::vector keys; + std::transform(map.begin(), map.end(), std::back_inserter(keys), select_first); + return keys; + } + + template + std::vector get_mmap_keys(std::multimap mmap) + { + std::vector keys; + + // auto -> std::multimap::iterator + for(auto it = mmap.begin(), end = mmap.end(); + it != end; it = mmap.upper_bound(it->first)) + { + keys.emplace_back(it->first); + } + + return keys; + } + + template + std::vector get_mmap_values_at_index(std::multimap mmap, typename identity::type key) + { + std::vector values; + + std::transform(mmap.lower_bound(key), + mmap.upper_bound(key), + std::back_inserter(values), + select_second); + + return values; + } + + void replace(std::string& base, const std::string& replacee, const std::string& replacer); + } +} + +#endif // UTILS_HPP diff --git a/arduino_gen/include/xml_element.hpp b/arduino_gen/include/arduino_gen/xml_element.hpp similarity index 100% rename from arduino_gen/include/xml_element.hpp rename to arduino_gen/include/arduino_gen/xml_element.hpp diff --git a/arduino_gen/include/xml_utils.hpp b/arduino_gen/include/arduino_gen/xml_utils.hpp similarity index 100% rename from arduino_gen/include/xml_utils.hpp rename to arduino_gen/include/arduino_gen/xml_utils.hpp diff --git a/arduino_gen/include/utils.hpp b/arduino_gen/include/utils.hpp deleted file mode 100644 index afce144..0000000 --- a/arduino_gen/include/utils.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef UTILS_HPP -#define UTILS_HPP - -#include -#include -#include - -namespace rip -{ - namespace arduinogen - { - template - struct identity { typedef T type; }; - - template - std::vector mmap_to_vector(std::multimap mmap, typename identity::type key) - { - std::vector values; - - std::transform(mmap.lower_bound(key), - mmap.upper_bound(key), - std::back_inserter(values), - [](std::pair element) - { - return element.second; - }); - - return values; - } - - void replace(std::string& base, const std::string& replacee, const std::string& replacer); - } -} - -#endif // UTILS_HPP diff --git a/arduino_gen/src/appendage.cpp b/arduino_gen/src/appendage.cpp index 1f811eb..5ea1382 100644 --- a/arduino_gen/src/appendage.cpp +++ b/arduino_gen/src/appendage.cpp @@ -1,11 +1,11 @@ -#include "appendage.hpp" +#include "arduino_gen/appendage.hpp" #include #include #include #include -#include "exceptions.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { @@ -225,5 +225,16 @@ namespace rip } } } + + nlohmann::json Appendage::getCoreJson(int index) const + { + nlohmann::json json; + + json["type"] = m_data["type"]; + json["label"] = m_data["label"]; + json["index"] = index; + + return json; + } } } diff --git a/arduino_gen/src/appendage_template.cpp b/arduino_gen/src/appendage_template.cpp index 81b7ef6..8e1a63f 100644 --- a/arduino_gen/src/appendage_template.cpp +++ b/arduino_gen/src/appendage_template.cpp @@ -1,14 +1,14 @@ -#include "appendage_template.hpp" +#include "arduino_gen/appendage_template.hpp" #include #include -#include "includes.hpp" -#include "constructors.hpp" -#include "setup.hpp" -#include "loop.hpp" -#include "command.hpp" -#include "exceptions.hpp" +#include "arduino_gen/includes.hpp" +#include "arduino_gen/constructors.hpp" +#include "arduino_gen/setup.hpp" +#include "arduino_gen/loop.hpp" +#include "arduino_gen/command.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { diff --git a/arduino_gen/src/arduino_gen.cpp b/arduino_gen/src/arduino_gen.cpp index 36e8bc1..8051a65 100644 --- a/arduino_gen/src/arduino_gen.cpp +++ b/arduino_gen/src/arduino_gen.cpp @@ -1,4 +1,4 @@ -#include "arduino_gen.hpp" +#include "arduino_gen/arduino_gen.hpp" #include #include @@ -7,77 +7,54 @@ #include #include #include +#include #include #include -#include "appendage_template.hpp" -#include "appendage.hpp" -#include "includes.hpp" -#include "setup.hpp" -#include "constructors.hpp" -#include "argument.hpp" -#include "loop.hpp" -#include "command.hpp" -#include "utils.hpp" -#include "xml_utils.hpp" -#include "exceptions.hpp" +#include "arduino_gen/appendage_template.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/includes.hpp" +#include "arduino_gen/setup.hpp" +#include "arduino_gen/constructors.hpp" +#include "arduino_gen/argument.hpp" +#include "arduino_gen/loop.hpp" +#include "arduino_gen/command.hpp" +#include "arduino_gen/utils.hpp" +#include "arduino_gen/xml_utils.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { namespace arduinogen { - ArduinoGen::ArduinoGen(std::string arduino, std::string parent_folder, std::string appendage_data_folder, bool testing) + ArduinoGen::ArduinoGen(std::string arduino, std::string parent_folder, std::string current_arduino_code_dir, std::string appendage_data_folder) : m_arduino(arduino) , m_parent_folder(parent_folder) + , m_current_arduino_code_dir(current_arduino_code_dir) , m_appendage_data_folder(appendage_data_folder) { assert(m_arduino.size() > 0); assert(m_parent_folder.size() > 0); - - if (!testing) - { - setupFolder(); - } } - void ArduinoGen::setupFolder() + void ArduinoGen::readConfig(std::string config_path) { - cppfs::FileHandle device_folder = cppfs::fs::open(fmt::format("{}/{}", m_parent_folder, m_arduino)); + using namespace cppfs; - if (device_folder.exists() && device_folder.isDirectory()) - { - device_folder.removeDirectory(); - } - device_folder.createDirectory(); - // TODO: chmod + FileHandle config_file = fs::open(config_path); - cppfs::FileHandle source_folder = cppfs::fs::open(fmt::format("{}/{}/{}", m_parent_folder, m_arduino, "src")); - source_folder.createDirectory(); - // TODO: chmod - } - - void ArduinoGen::readConfig(std::string config_path, bool copy) - { - if(copy) + if (!config_file.exists() || !config_file.isFile()) { - // TODO: copy and chmod + throw FileIoException("Config file does not exist"); } + config = config_file.readFile(); + // Read the config file in as json - // REVIEW: We could change this to use cppfs - std::ifstream config_stream; - config_stream.open(config_path); - std::string config_string = ""; - std::string line = ""; - while(getline(config_stream, line)) - { - config_string += line; - } - config_stream.close(); - nlohmann::json config = nlohmann::json::parse(config_string); + nlohmann::json config_json = nlohmann::json::parse(config); - for(nlohmann::json appendage_json : config) + for(nlohmann::json appendage_json : config_json) { std::string type = appendage_json["type"]; @@ -86,62 +63,136 @@ namespace rip } loadTemplates(); + loadCommandEnums(); } - void ArduinoGen::generateOutput() + void ArduinoGen::generateOutput(bool copyOldFiles) { - // Write the Arduino Code - std::ofstream source; - source.open(fmt::format("{0}/{1}/src/{1}.ino", m_parent_folder, m_arduino), - std::ios::out | std::ios::trunc); - source << getArduinoCode(); - source.close(); - // TODO: chmod - - // Write config for core - std::ofstream core_config; - core_config.open(fmt::format("{0}/{1}/core_config.json", m_parent_folder, m_arduino), - std::ios::out | std::ios::trunc); - core_config << getCoreConfig(); - core_config.close(); - - // Write build and upload script - std::ofstream upload; - upload.open(fmt::format("{0}/{1}/upload.sh", m_parent_folder, m_arduino), - std::ios::out | std::ios::trunc); - upload << getUploadScript(); - upload.close(); + using namespace cppfs; + + // Read or generate platformio.ini + std::string platformio_str; + FileHandle platformio_fh = fs::open(fmt::format("{}/{}/platformio.ini", m_parent_folder, m_arduino)); + if (copyOldFiles && platformio_fh.exists() && platformio_fh.isFile()) + { + platformio_str = platformio_fh.readFile(); + } + else + { + platformio_str = getPlatformIo(); + } + + // Read or generate serial.sh + FileHandle serial_fh = fs::open(fmt::format("{0}/{1}/serial.sh", m_parent_folder, m_arduino)); + std::string serial_str; + if (copyOldFiles && serial_fh.exists() && serial_fh.isFile()) + { + serial_str = serial_fh.readFile(); + } + else + { + serial_str = getSerialScript(); + } + + // Read or generate upload.sh + FileHandle upload_fh = fs::open(fmt::format("{0}/{1}/upload.sh", m_parent_folder, m_arduino)); + std::string upload_str; + if (copyOldFiles && upload_fh.exists() && upload_fh.isFile()) + { + upload_str = upload_fh.readFile(); + } + else + { + upload_str = getUploadScript(); + } + + // TODO: Move generation code to before the device folder gets removed. + // If an exception occurs during generation, we don't want to remove the old device_folder. + + // REVIEW: Do we need to change the file and directory owners in addition to setting permissions? + + // Delete and create output dir + FileHandle device_folder = fs::open(fmt::format("{}/{}", m_parent_folder, m_arduino)); + if (device_folder.exists() && device_folder.isDirectory()) + { + device_folder.removeDirectoryRec(); + } + device_folder.createDirectory(); + device_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + + // Create src dir + FileHandle source_folder = fs::open(fmt::format("{}/{}/{}", m_parent_folder, m_arduino, "src")); + source_folder.createDirectory(); + source_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + + // Create ino file + FileHandle source = fs::open(fmt::format("{0}/{1}/src/{1}.ino", m_parent_folder, m_arduino)); + source.writeFile(getArduinoCode()); + source.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + + // Create config file + FileHandle json_config = fs::open(fmt::format("{0}/{1}/{1}.json", m_parent_folder, m_arduino)); + json_config.writeFile(config); + json_config.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + + // Create core file + FileHandle core_config = fs::open(fmt::format("{0}/{1}/{1}_core.json", m_parent_folder, m_arduino)); + core_config.writeFile(getCoreConfig()); + core_config.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + + // Create platformio.ini + FileHandle platformio_ini = fs::open(fmt::format("{}/{}/platformio.ini", m_parent_folder, m_arduino)); + platformio_ini.writeFile(platformio_str); + platformio_ini.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + + // Create serial script + FileHandle serial = fs::open(fmt::format("{0}/{1}/serial.sh", m_parent_folder, m_arduino)); + serial.writeFile(serial_str); + serial.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + + // Create upload script + FileHandle upload = fs::open(fmt::format("{0}/{1}/upload.sh", m_parent_folder, m_arduino)); + upload.writeFile(upload_str); + upload.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); } std::string ArduinoGen::getArduinoCode() { - return fmt::format( - "{includes}\n" - "{constructors}\n" - "{setup}\n" - "{loop}\n" - "{commands}\n" - "{command_attaches}\n" - "{command_callbacks}\n" - "{extra}\n", + std::unique_ptr code_template_istream = cppfs::fs::open("code_template.txt").createInputStream(); + std::string code_template(std::istreambuf_iterator(*code_template_istream), {}); + + return fmt::format(code_template, fmt::arg("includes", getIncludes()), fmt::arg("constructors", getConstructors()), fmt::arg("setup", getSetup()), fmt::arg("loop", getLoop()), - fmt::arg("commands", getCommandEnums()), + fmt::arg("command_enums", getCommandEnums()), fmt::arg("command_attaches", getCommandAttaches()), - fmt::arg("command_callbacks", getCommandCallbacks()), - fmt::arg("extra", getExtras()) + fmt::arg("command_callbacks", getCommandCallbacks()) ); } void ArduinoGen::loadTemplates() { - // auto -> std::multimap>::iterator - for(auto it = m_appendages.begin(), end = m_appendages.end(); it != end; - it = m_appendages.upper_bound(it->first)) + for (std::string type : get_mmap_keys(m_appendages)) { - std::string type_file = it->first; + std::string type_file = type; for(char& c : type_file) { if(c == ' ') @@ -154,7 +205,7 @@ namespace rip } } - std::vector> appendages = mmap_to_vector(m_appendages, it->first); + std::vector> appendages = get_mmap_values_at_index(m_appendages, type); tinyxml2::XMLDocument doc; loadXmlFile(doc, fmt::format("{0}/xml/{1}.xml", m_appendage_data_folder, type_file), { "code", "setup", "loop" }); @@ -167,7 +218,7 @@ namespace rip "AppendageTemplate xml files must have a root of appendage-template."); } - m_appendage_templates.emplace_back(element, it->first, appendages); + m_appendage_templates.emplace_back(element, type, appendages); } sort(begin(m_appendage_templates), end(m_appendage_templates), [](AppendageTemplate a, AppendageTemplate b) { @@ -175,6 +226,33 @@ namespace rip }); } + void ArduinoGen::loadCommandEnums() + { + int cmd_idx = 0; + + m_commands["kAcknowledge"] = cmd_idx++; + m_commands["kError"] = cmd_idx++; + m_commands["kUnknown"] = cmd_idx++; + m_commands["kSetLed"] = cmd_idx++; + m_commands["kPing"] = cmd_idx++; + m_commands["kPingResult"] = cmd_idx++; + m_commands["kPong"] = cmd_idx++; + + for(const AppendageTemplate& at : m_appendage_templates) + { + for(const std::shared_ptr& cmd : at.GetCommands()) + { + m_commands[cmd->getId()] = cmd_idx++; + + std::string result_id = cmd->getResultId(); + if(result_id.size()) + { + m_commands[result_id] = cmd_idx++; + } + } + } + } + std::string ArduinoGen::getIncludes() { // Loop through all the includes and let the set handle duplicates @@ -196,6 +274,12 @@ namespace rip { rv += fmt::format("#include {}\n", include); } + + if (rv.size() > 0) + { + rv.pop_back(); + } + return rv; } @@ -203,6 +287,7 @@ namespace rip std::string ArduinoGen::getConstructors() { std::string rv = ""; + for(const AppendageTemplate& at : m_appendage_templates) { std::shared_ptr constructors = at.GetConstructors(); @@ -211,6 +296,13 @@ namespace rip rv += constructors->toString(at.GetAppendages()) + "\n"; } } + + if (rv.size() > 0) + { + rv.pop_back(); + rv.pop_back(); + } + return rv; } @@ -225,6 +317,13 @@ namespace rip rv += setup->toString(at.GetAppendages()) + "\n"; } } + + if (rv.size() > 0) + { + rv.pop_back(); + rv.pop_back(); + } + return rv; } @@ -239,42 +338,35 @@ namespace rip rv += loop->toString(at.GetAppendages()) + "\n"; } } + + if (rv.size() > 0) + { + rv.pop_back(); + rv.pop_back(); + } + return rv; } std::string ArduinoGen::getCommandEnums() { - std::string rv = ""; - m_commands["kAcknowledge"] = 0; - m_commands["kError"] = 1; - m_commands["kUnknown"] = 2; - m_commands["kSetLed"] = 3; - m_commands["kPing"] = 4; - m_commands["kPingResult"] = 5; - m_commands["kPong"] = 6; - int cmd_idx = 7; - - std::string cmd_id; - std::string result_id; - for(const AppendageTemplate& at : m_appendage_templates) + std::map sorted_commands; + + // auto -> std::map::iterator + for (auto it : m_commands) { - for(const std::shared_ptr& cmd : at.GetCommands()) - { - cmd_id = cmd->getId(); - rv += fmt::format("\t{},\n", cmd_id); - m_commands[cmd_id] = cmd_idx; - cmd_idx ++; + sorted_commands.emplace(it.second, it.first); + } - result_id = cmd->getResultId(); - if(result_id.size()) - { - rv += fmt::format("\t{},\n", result_id); - m_commands[result_id] = cmd_idx; - cmd_idx ++; - } - } + std::string rv = ""; + + for (auto it : sorted_commands) + { + rv += fmt::format("\t{},\n", it.second); } - rv = rv.substr(0, rv.size() - 2); // Remove last comma + + rv.pop_back(); // Remove last newline + rv.pop_back(); // Remove last comma return rv; } @@ -289,6 +381,12 @@ namespace rip rv += fmt::format("\tcmdMessenger.attach({}, {});\n", cmd->getId(), cmd->getName()); } } + + if (rv.size() > 0) + { + rv.pop_back(); + } + return rv; } @@ -302,32 +400,77 @@ namespace rip rv += command->callback(at.GetAppendages().size()) + "\n"; } } + + if (rv.size() > 0) + { + rv.pop_back(); + rv.pop_back(); + } + return rv; } - std::string ArduinoGen::getExtras() + std::string ArduinoGen::getCoreConfig() { + nlohmann::json config; - std::string rv = ""; - /* - for(const AppendageTemplate& at : m_appendage_templates) + config["commands"] = m_commands; + + config["appendages"] = nlohmann::json::array(); + for(std::string type : get_mmap_keys(m_appendages)) { - rv += at.extra + "\n"; + std::vector> appendages_of_type = get_mmap_values_at_index(m_appendages, type); + + for (int i = 0; i < appendages_of_type.size(); i++) + { + config["appendages"].emplace_back(appendages_of_type[i]->getCoreJson(i)); + } } - */ - return rv; + + return config.dump(4); } - std::string ArduinoGen::getCoreConfig() + std::string ArduinoGen::getUploadScript() { - // TODO: Implement - return ""; + return fmt::format( + "#!/usr/bin/env bash\n" + "\n" + "cd {current_arduino_code_dir}/{arduino}\n" // REVIEW: Shouldn't this be m_parent_folder + "\n" + "git add -A\n" + "git commit -m \"New code for {arduino}\"\n" + "git push\n" + "\n" + "pio run -t upload\n", + fmt::arg("current_arduino_code_dir", m_current_arduino_code_dir), + fmt::arg("arduino", m_arduino) + ); } - std::string ArduinoGen::getUploadScript() + std::string ArduinoGen::getSerialScript() { - // TODO: Implement - return ""; + return fmt::format( + "#!/usr/bin/env bash\n" + "\n" + "picocom /dev/{} -b 115200 --echo\n", + fmt::arg("arduino", m_arduino) + ); + } + + std::string ArduinoGen::getPlatformIo() + { + return fmt::format( + "[platformio]\n" + "lib_dir = /Robot/ArduinoLibraries\n" // REVIEW: Should this be hard coded? + "env_default = {0}\n" + "\n" + "[env:{0}]\n" + "platform = atmelavr\n" + "framework = arduino\n" + "board = uno\n" + "upload_port = /dev/{0}\n", + m_arduino + ); } } } diff --git a/arduino_gen/src/argument.cpp b/arduino_gen/src/argument.cpp index 6cb6583..f8d6640 100644 --- a/arduino_gen/src/argument.cpp +++ b/arduino_gen/src/argument.cpp @@ -1,10 +1,10 @@ -#include "argument.hpp" +#include "arduino_gen/argument.hpp" #include #include -#include "appendage.hpp" -#include "exceptions.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { diff --git a/arduino_gen/src/code.cpp b/arduino_gen/src/code.cpp index dac098e..2de8c89 100644 --- a/arduino_gen/src/code.cpp +++ b/arduino_gen/src/code.cpp @@ -1,4 +1,4 @@ -#include "code.hpp" +#include "arduino_gen/code.hpp" #include #include @@ -9,7 +9,7 @@ #include #include -#include "exceptions.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { diff --git a/arduino_gen/src/command.cpp b/arduino_gen/src/command.cpp index 0db0634..1d8c97f 100644 --- a/arduino_gen/src/command.cpp +++ b/arduino_gen/src/command.cpp @@ -1,4 +1,4 @@ -#include "command.hpp" +#include "arduino_gen/command.hpp" #include #include @@ -8,11 +8,11 @@ #include #include -#include "code.hpp" -#include "parameter.hpp" -#include "return_value.hpp" -#include "appendage.hpp" -#include "exceptions.hpp" +#include "arduino_gen/code.hpp" +#include "arduino_gen/parameter.hpp" +#include "arduino_gen/return_value.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { @@ -127,7 +127,7 @@ namespace rip rv += m_code->getCode(); - rv += fmt::format("\tcmdMessenger.sendBindCmd(kAcknowledge, {});\n", m_id); + rv += fmt::format("\tcmdMessenger.sendBinCmd(kAcknowledge, {});\n", m_id); if(m_return_values.size()) { @@ -138,7 +138,7 @@ namespace rip rv += return_value.send(); } - rv += "\tcmdMeessenger.sendCmdEnd();\n"; + rv += "\tcmdMessenger.sendCmdEnd();\n"; } rv += "}\n"; diff --git a/arduino_gen/src/constructor.cpp b/arduino_gen/src/constructor.cpp new file mode 100644 index 0000000..806ac3b --- /dev/null +++ b/arduino_gen/src/constructor.cpp @@ -0,0 +1,118 @@ +#include "arduino_gen/constructors.hpp" + +#include +#include + +#include "arduino_gen/argument.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/exceptions.hpp" + +namespace rip +{ + namespace arduinogen + { + Constructor::Constructor(const tinyxml2::XMLElement* xml) + : XmlElement(xml) + { + m_type = getAttribute("type")->Value(); + m_variable = getAttribute("variable")->Value(); + + try + { + m_type_is_class = getAttribute("type-is-class")->BoolValue(); + } + catch (AttributeException) + { + m_type_is_class = true; + } + + // Loop through the parameters for the constructor + for (const tinyxml2::XMLElement* ele : getElements("argument")) + { + m_arguments.emplace_back(ele); + } + + // If there are any extra attributes, throw an exception + if (!isAttributesEmpty()) + { + throw AttributeException(fmt::format("Extra attribute for Constructors on line number {}", + xml->GetLineNum())); + } + + // If there are any extra elements, throw an exception + if (!isElementsEmpty()) + { + throw ElementException(fmt::format("Extra element for Constructors on line number {}", + xml->GetLineNum())); + } + } + + std::string Constructor::toString(const std::vector>& appendages) const + { + std::string rv; + + rv += fmt::format("{} {} [{}] = {{\n", m_type, m_variable, appendages.size()); + + for(std::shared_ptr appendage : appendages) + { + rv += "\t"; + + if (m_type_is_class) + { + rv += fmt::format("{}(", m_type); + } + + for(const Argument& argument : m_arguments) + { + if (appendage->has(argument.getName())) + { + if (appendage->isType(argument.getName(), "string")) + { + rv += fmt::format("\"{}\"", argument.toString(appendage)); + } + else + { + rv += argument.toString(appendage); + } + } + else + { + std::string value = argument.getValue(); + if (value.size() > 0) + { + rv += value; + } + else + { + // TODO(Anthony): Throw Exception + } + } + + rv += ", "; + } + + // Remove last comma and add the end of the single constructor + if (m_arguments.size() > 0) + { + rv = rv.substr(0, rv.size() - 2); + } + + if (m_type_is_class) + { + rv += ")"; + } + + rv += ",\n"; + } + + // Remove the last comma and new line and add the end of the constructor list + if (appendages.size() > 0) + { + rv = rv.substr(0, rv.size() - 2) + "\n"; + } + rv += "};\n"; + + return rv; + } + } // arduinogen +} diff --git a/arduino_gen/src/constructors.cpp b/arduino_gen/src/constructors.cpp index d063779..847c0c9 100644 --- a/arduino_gen/src/constructors.cpp +++ b/arduino_gen/src/constructors.cpp @@ -1,112 +1,53 @@ -#include "constructors.hpp" +#include "arduino_gen/constructors.hpp" #include #include -#include "argument.hpp" -#include "appendage.hpp" -#include "exceptions.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { namespace arduinogen { - Constructors::Constructors() = default; - Constructors::~Constructors() = default; - Constructors::Constructors(const tinyxml2::XMLElement* xml) - : m_exists(true) + : XmlElement(xml) { - if(!xml) - { - m_exists = false; - return; - } - - const char* type = xml->Attribute("type"); - if(!type) + // Loop through the parameters for the constructor + for (const tinyxml2::XMLElement* ele : getElements("constructor")) { - throw AttributeException(fmt::format("Constructor type missing on line number {}", - xml->GetLineNum())); + m_constructors.emplace_back(ele); } - m_type = type; - const char* variable = xml->Attribute("variable"); - if(!variable) + // If there are any extra attributes, throw an exception + if (!isAttributesEmpty()) { - throw AttributeException(fmt::format("Constructor variable missing on line number {}", + throw AttributeException(fmt::format("Extra attribute for Constructors on line number {}", xml->GetLineNum())); } - m_variable = variable; - // Loop through the parameters for the constructor - for(const tinyxml2::XMLElement* argument = xml->FirstChildElement("argument"); - argument != nullptr; argument = argument->NextSiblingElement("argument")) + // If there are any extra elements, throw an exception + if (!isElementsEmpty()) { - m_arguments.emplace_back(argument); + throw ElementException(fmt::format("Extra element for Constructors on line number {}", + xml->GetLineNum())); } } std::string Constructors::toString(const std::vector>& appendages) const { - if(!m_exists) - { - return ""; - } - - std::string rv; - - rv += fmt::format("{} {} [{}] = {{\n", m_type, m_variable, appendages.size()); + std::string rv = ""; - for(std::shared_ptr appendage : appendages) + for (Constructor constructor : m_constructors) { - rv += fmt::format("\t{}(", m_type); - - for(const Argument& argument : m_arguments) - { - if (appendage->has(argument.getName())) - { - if (appendage->isType(argument.getName(), "string")) - { - rv += fmt::format("\"{}\"", argument.toString(appendage)); - } - else - { - rv += argument.toString(appendage); - } - } - else - { - std::string value = argument.getValue(); - if (value.size() > 0) - { - rv += value; - } - else - { - // TODO(Anthony): Throw Exception - } - } - - rv += ", "; - } - - // Remove last comma and add the end of the single constructor - if (m_arguments.size() > 0) - { - rv = rv.substr(0, rv.size() - 2); - } - rv += "),\n"; + rv += constructor.toString(appendages) + "\n"; } - // Remove the last comma and new line and add the end of the constructor list - if (appendages.size() > 0) + if (rv.size() > 0) { - rv = rv.substr(0, rv.size() - 2) + "\n"; + rv.pop_back(); } - rv += "};\n"; return rv; } - } // arduinogen + } } diff --git a/arduino_gen/src/includes.cpp b/arduino_gen/src/includes.cpp index cccc617..45fc735 100644 --- a/arduino_gen/src/includes.cpp +++ b/arduino_gen/src/includes.cpp @@ -1,13 +1,13 @@ -#include "includes.hpp" +#include "arduino_gen/includes.hpp" #include #include -#include "constructors.hpp" -#include "setup.hpp" -#include "loop.hpp" -#include "command.hpp" -#include "exceptions.hpp" +#include "arduino_gen/constructors.hpp" +#include "arduino_gen/setup.hpp" +#include "arduino_gen/loop.hpp" +#include "arduino_gen/command.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { diff --git a/arduino_gen/src/loop.cpp b/arduino_gen/src/loop.cpp index 073442e..7baaa71 100644 --- a/arduino_gen/src/loop.cpp +++ b/arduino_gen/src/loop.cpp @@ -1,4 +1,4 @@ -#include "loop.hpp" +#include "arduino_gen/loop.hpp" #include #include @@ -6,8 +6,8 @@ #include #include -#include "appendage.hpp" -#include "exceptions.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { @@ -20,11 +20,13 @@ namespace rip std::string Loop::toString(std::vector< std::shared_ptr > appendages) { - std::string appendage_loop = getCode(); + std::string rv = ""; std::regex replace_regex("\\$(\\w+)\\$"); for(std::shared_ptr appendage : appendages) { + std::string appendage_loop = getCode(); + std::sregex_iterator reg_begin = std::sregex_iterator(appendage_loop.begin(), appendage_loop.end(), replace_regex); std::sregex_iterator reg_end = std::sregex_iterator(); @@ -55,9 +57,16 @@ namespace rip str_iter = appendage_loop.find(replacee); } } + + rv += appendage_loop + "\n"; + } + + if (rv.size() > 0) + { + rv.pop_back(); } - return appendage_loop; + return rv; } } // namespace arduinogen } diff --git a/arduino_gen/src/main.cpp b/arduino_gen/src/main.cpp index 67f892b..69920f6 100644 --- a/arduino_gen/src/main.cpp +++ b/arduino_gen/src/main.cpp @@ -1,6 +1,144 @@ -#include "arduino_gen.hpp" +#include + +#include +#include +#include +#include +#include + +#include "arduino_gen/arduino_gen.hpp" int main(int argc, char* argv[]) { - return 0; + args::ArgumentParser parser("ArduinoGen generates arduino code to be used with RIP."); + args::HelpFlag help(parser, "help", "Display this help menu", {'h', "help"}, args::Options::Single); + args::CompletionFlag completion(parser, {"complete"}); + parser.Prog("arduino_gen"); + + args::ValueFlag arduino(parser, "ARDUINO", "Name of the arduino", {'a', "arduino"}, args::Options::Required | args::Options::Single); + args::ValueFlag config(parser, "CONFIG", "Location of the config json file", {'c', "config"}, args::Options::Required | args::Options::Single); + args::ValueFlag parent_folder(parser, "PARENT_FOLDER", "Parent folder of the folder to put all the output files", {"parent_folder"}, "/Robot/CurrentArduinoCode", args::Options::Single); + args::ValueFlag appendages(parser, "APPENDAGES_FOLDER", "Folder of where to look for appendage files", {"appendages"}, "./appendages", args::Options::Single); + args::Flag noCopy(parser, "NO_COPY", "Don't copy existing files", {"no_copy"}, args::Options::Single); + args::Flag noGit(parser, "NO_GIT", "Don't git commit the files when uploading", {"no_git"}, args::Options::Single); + + args::Group buildUploadGroup(parser, "Build or Upload arduino code", args::Group::Validators::AtMostOne); + args::Flag build(buildUploadGroup, "build", "Build the ino file into something that can be uploaded to the Arduino", {'b', "build"}, args::Options::Single); + args::Flag upload(buildUploadGroup, "upload", "Build the ino file and upload that onto the arduino", {'u', "upload"}, args::Options::Single); + + try + { + parser.ParseCLI(argc, argv); + } + catch (args::Completion e) + { + std::cout << e.what(); + return 0; + } + catch (args::Help) + { + std::cout << parser; + return 0; + } + catch (args::ParseError e) + { + std::cerr << "ParseError: " << e.what() << std::endl << std::endl; + std::cerr << parser; + return EXIT_FAILURE; + } + catch (args::RequiredError e) + { + std::cerr << "RequiredError: " << e.what() << std::endl << std::endl; + std::cerr << parser << std::endl; + return EXIT_FAILURE; + } + catch (args::ValidationError e) + { + std::cerr << "ValidationError: " << e.what() << std::endl << std::endl; + std::cerr << parser << std::endl; + return EXIT_FAILURE; + } + + /* + * args::get(arduino) is guarenteed to have a value + * args::get(config) is guarenteed to have a value + * args::get(parent_folder) is guarenteed to have a value, defaults to: /Robot/CurrentArduinoCode + * args::get(appendages) is guarenteed to have a value, defaults to: ./appendages + * args::get(build) defaults to: false + * args::get(upload) defaults to: false + * build and upload are guarenteed to be mutually exclusive + */ + + cppfs::FileHandle config_fh = cppfs::fs::open(args::get(config)); + if (!config_fh.exists() || !config_fh.isFile()) + { + std::cerr << fmt::format("The config file \"{}\" doesn't exist.", args::get(config)) << std::endl; + return EXIT_FAILURE; + } + + cppfs::FileHandle parent_folder_fh = cppfs::fs::open(args::get(parent_folder)); + if (!parent_folder_fh.exists() || !parent_folder_fh.isDirectory()) + { + std::cerr << fmt::format("The parent folder \"{}\" doesn't exist.", args::get(parent_folder)) << std::endl; + return EXIT_FAILURE; + } + + cppfs::FileHandle appendages_fh = cppfs::fs::open(args::get(appendages)); + if (!appendages_fh.exists() || !appendages_fh.isDirectory()) + { + std::cerr << fmt::format("The appendages folder \"{}\" doesn't exist.", args::get(appendages)) << std::endl; + return EXIT_FAILURE; + } + + rip::arduinogen::ArduinoGen ag(args::get(arduino), args::get(parent_folder), "/Robot/CurrentArduinoCode", args::get(appendages)); + + try + { + ag.readConfig(args::get(config)); + } + catch (std::exception e) + { + std::cerr << "ArduinoGen failed to read the config file.\n" << e.what() << std::endl; + return EXIT_FAILURE; + } + + try + { + ag.generateOutput(!args::get(noCopy)); + } + catch (std::exception e) + { + std::cerr << "ArduinoGen failed to generate the output.\n" << e.what() << std::endl; + return EXIT_FAILURE; + } + + std::cout << "Arduino code generated." << std::endl; + + // REVIEW: Do we want to check the build/upload flags before generating the new .ino? + + // REVIEW: Is using std::system fine? + + if (build) + { + std::system(fmt::format("pio run --project-dir {}/{}", args::get(parent_folder), args::get(arduino)).c_str()); + } + else if (upload) + { + if (noGit) + { + std::system(fmt::format("pio run --project-dir {}/{} -t upload", args::get(parent_folder), args::get(arduino)).c_str()); + } + else if (args::get(parent_folder) != "/Robot/CurrentArduinoCode") + { + std::cerr << "ArduinoGen will not upload when the parent_folder is not \"/Robot/CurrentArduinoCode\".\n" + "You can override this using --no_git" << std::endl; + return EXIT_FAILURE; + } + else + { + std::system(fmt::format("sh {}/{}/upload.sh", args::get(parent_folder), args::get(arduino)).c_str()); + } + } + + return EXIT_SUCCESS; } diff --git a/arduino_gen/src/parameter.cpp b/arduino_gen/src/parameter.cpp index e145a59..2964385 100644 --- a/arduino_gen/src/parameter.cpp +++ b/arduino_gen/src/parameter.cpp @@ -1,11 +1,11 @@ -#include "parameter.hpp" +#include "arduino_gen/parameter.hpp" #include #include #include -#include "exceptions.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { diff --git a/arduino_gen/src/return_value.cpp b/arduino_gen/src/return_value.cpp index bf78dd9..8135cf7 100644 --- a/arduino_gen/src/return_value.cpp +++ b/arduino_gen/src/return_value.cpp @@ -1,11 +1,11 @@ -#include "return_value.hpp" +#include "arduino_gen/return_value.hpp" #include #include #include -#include "exceptions.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { @@ -39,7 +39,7 @@ namespace rip std::string ReturnValue::send() const { - return fmt::format("\tcmdMessenger.sendBinArg({});\n", m_name); + return fmt::format("\tcmdMessenger.sendCmdBinArg({});\n", m_name); } std::string ReturnValue::getName() const diff --git a/arduino_gen/src/setup.cpp b/arduino_gen/src/setup.cpp index 9f0ec48..18f5050 100644 --- a/arduino_gen/src/setup.cpp +++ b/arduino_gen/src/setup.cpp @@ -1,4 +1,4 @@ -#include "setup.hpp" +#include "arduino_gen/setup.hpp" #include #include @@ -7,8 +7,8 @@ #include #include -#include "appendage.hpp" -#include "exceptions.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { @@ -21,11 +21,15 @@ namespace rip std::string Setup::toString(std::vector< std::shared_ptr > appendages) const { - std::string appendage_setup = getCode(); + std::string rv = ""; std::regex replace_regex("\\$(\\w+)\\$"); - for(std::shared_ptr appendage : appendages) + for(size_t i = 0; i < appendages.size(); i++) { + std::shared_ptr appendage = appendages[i]; + + std::string appendage_setup = getCode(); + std::sregex_iterator reg_begin = std::sregex_iterator(appendage_setup.begin(), appendage_setup.end(), replace_regex); std::sregex_iterator reg_end = std::sregex_iterator(); @@ -44,7 +48,17 @@ namespace rip std::string replacee = fmt::format("${}$", match); std::size_t str_iter = appendage_setup.find(replacee); - if (!appendage->has(match)) + std::string value = ""; + + if (match == "i") + { + value = std::to_string(i); + } + else if (appendage->has(match)) + { + value = appendage->getString(match); + } + else { throw PatternNotFoundException(fmt::format("Appendage \"{}\" does not have the variable \"{}\"", appendage->getType(), match)); @@ -52,13 +66,20 @@ namespace rip while(str_iter != std::string::npos) { - appendage_setup.replace(str_iter, replacee.length(), appendage->getString(match)); + appendage_setup.replace(str_iter, replacee.length(), value); str_iter = appendage_setup.find(replacee); } } + + rv += appendage_setup + "\n"; + } + + if (rv.size() > 0) + { + rv.pop_back(); } - return appendage_setup; + return rv; } } // arduinogen } diff --git a/arduino_gen/src/utils.cpp b/arduino_gen/src/utils.cpp index d1fba2f..b1de271 100644 --- a/arduino_gen/src/utils.cpp +++ b/arduino_gen/src/utils.cpp @@ -1,4 +1,4 @@ -#include "utils.hpp" +#include "arduino_gen/utils.hpp" namespace rip { diff --git a/arduino_gen/src/xml_element.cpp b/arduino_gen/src/xml_element.cpp index 9662287..971b8f0 100644 --- a/arduino_gen/src/xml_element.cpp +++ b/arduino_gen/src/xml_element.cpp @@ -1,9 +1,9 @@ -#include "xml_element.hpp" +#include "arduino_gen/xml_element.hpp" #include #include -#include "exceptions.hpp" +#include "arduino_gen/exceptions.hpp" namespace rip { diff --git a/arduino_gen/src/xml_utils.cpp b/arduino_gen/src/xml_utils.cpp index 20bc534..385c426 100644 --- a/arduino_gen/src/xml_utils.cpp +++ b/arduino_gen/src/xml_utils.cpp @@ -1,4 +1,4 @@ -#include "xml_utils.hpp" +#include "arduino_gen/xml_utils.hpp" #include #include @@ -10,7 +10,10 @@ #include -#include "exceptions.hpp" +#include +#include + +#include "arduino_gen/exceptions.hpp" namespace rip { @@ -20,14 +23,7 @@ namespace rip { std::string file, escaped, expression, suffix; - // Read in file - std::ifstream in(filename, std::ios::in); - if (!in) - { - throw FileIoException("Error reading file"); - } - file = std::string{std::istreambuf_iterator{in}, {}}; - in.close(); + file = cppfs::fs::open(filename).readFile(); for (std::string element : elements_to_escape) { diff --git a/arduino_gen/test/data/appendage_template/constructors.xml b/arduino_gen/test/data/appendage_template/constructors.xml index e738c23..270c164 100644 --- a/arduino_gen/test/data/appendage_template/constructors.xml +++ b/arduino_gen/test/data/appendage_template/constructors.xml @@ -1,5 +1,7 @@ - - + + + + diff --git a/arduino_gen/test/data/appendage_template/everything.xml b/arduino_gen/test/data/appendage_template/everything.xml index 1c130f1..1cfa201 100644 --- a/arduino_gen/test/data/appendage_template/everything.xml +++ b/arduino_gen/test/data/appendage_template/everything.xml @@ -4,11 +4,13 @@ 4 5 - - - - - + + + + + + + //Setup 2, Electric Boogaloo diff --git a/arduino_gen/test/data/appendage_template/two_constructors.xml b/arduino_gen/test/data/appendage_template/two_constructors.xml index 3669900..7a3a7a2 100644 --- a/arduino_gen/test/data/appendage_template/two_constructors.xml +++ b/arduino_gen/test/data/appendage_template/two_constructors.xml @@ -1,8 +1,12 @@ - - + + + + - - + + + + diff --git a/arduino_gen/test/data/arduino_gen/xml/servo.xml b/arduino_gen/test/data/arduino_gen/xml/servo.xml index e9449c7..a6f78d2 100644 --- a/arduino_gen/test/data/arduino_gen/xml/servo.xml +++ b/arduino_gen/test/data/arduino_gen/xml/servo.xml @@ -2,10 +2,15 @@ Servo.h - + + + + + + - servo.attach($pin$); + servos[$i$].attach($pin$); // Servo pin: $pin$ diff --git a/arduino_gen/test/data/arduino_gen/xml/ultrasonic.xml b/arduino_gen/test/data/arduino_gen/xml/ultrasonic.xml index 3cb7f1f..52d927b 100644 --- a/arduino_gen/test/data/arduino_gen/xml/ultrasonic.xml +++ b/arduino_gen/test/data/arduino_gen/xml/ultrasonic.xml @@ -2,10 +2,12 @@ NewPing.h - - - - + + + + + + // Ultrasonic triggerPin: $triggerPin$ @@ -17,7 +19,7 @@ - rv = sonar[index_num].ping_cm(); + rv = sonar[indexNum].ping_cm(); diff --git a/arduino_gen/test/data/constructor/bool_argument.xml b/arduino_gen/test/data/constructor/bool_argument.xml new file mode 100644 index 0000000..976fbee --- /dev/null +++ b/arduino_gen/test/data/constructor/bool_argument.xml @@ -0,0 +1,3 @@ + + + diff --git a/arduino_gen/test/data/constructor/empty.xml b/arduino_gen/test/data/constructor/empty.xml new file mode 100644 index 0000000..73aad4e --- /dev/null +++ b/arduino_gen/test/data/constructor/empty.xml @@ -0,0 +1,2 @@ + + diff --git a/arduino_gen/test/data/constructor/float_argument.xml b/arduino_gen/test/data/constructor/float_argument.xml new file mode 100644 index 0000000..35955c7 --- /dev/null +++ b/arduino_gen/test/data/constructor/float_argument.xml @@ -0,0 +1,3 @@ + + + diff --git a/arduino_gen/test/data/constructor/int_argument.xml b/arduino_gen/test/data/constructor/int_argument.xml new file mode 100644 index 0000000..878a616 --- /dev/null +++ b/arduino_gen/test/data/constructor/int_argument.xml @@ -0,0 +1,3 @@ + + + diff --git a/arduino_gen/test/data/constructors/multiple_argument_constructors.xml b/arduino_gen/test/data/constructor/multiple_argument.xml similarity index 77% rename from arduino_gen/test/data/constructors/multiple_argument_constructors.xml rename to arduino_gen/test/data/constructor/multiple_argument.xml index 1f8d6d2..4ad6894 100644 --- a/arduino_gen/test/data/constructors/multiple_argument_constructors.xml +++ b/arduino_gen/test/data/constructor/multiple_argument.xml @@ -1,6 +1,6 @@ - + - \ No newline at end of file + diff --git a/arduino_gen/test/data/constructor/no_type.xml b/arduino_gen/test/data/constructor/no_type.xml new file mode 100644 index 0000000..e5e3b5f --- /dev/null +++ b/arduino_gen/test/data/constructor/no_type.xml @@ -0,0 +1,2 @@ + + diff --git a/arduino_gen/test/data/constructor/no_type_no_variable.xml b/arduino_gen/test/data/constructor/no_type_no_variable.xml new file mode 100644 index 0000000..a0e2827 --- /dev/null +++ b/arduino_gen/test/data/constructor/no_type_no_variable.xml @@ -0,0 +1,2 @@ + + diff --git a/arduino_gen/test/data/constructor/no_variable.xml b/arduino_gen/test/data/constructor/no_variable.xml new file mode 100644 index 0000000..2dc393d --- /dev/null +++ b/arduino_gen/test/data/constructor/no_variable.xml @@ -0,0 +1,2 @@ + + diff --git a/arduino_gen/test/data/constructor/string_argument.xml b/arduino_gen/test/data/constructor/string_argument.xml new file mode 100644 index 0000000..c6b9e65 --- /dev/null +++ b/arduino_gen/test/data/constructor/string_argument.xml @@ -0,0 +1,3 @@ + + + diff --git a/arduino_gen/test/data/constructor/type_is_class_false.xml b/arduino_gen/test/data/constructor/type_is_class_false.xml new file mode 100644 index 0000000..6e9ca3c --- /dev/null +++ b/arduino_gen/test/data/constructor/type_is_class_false.xml @@ -0,0 +1,3 @@ + + + diff --git a/arduino_gen/test/data/constructor/type_is_class_true.xml b/arduino_gen/test/data/constructor/type_is_class_true.xml new file mode 100644 index 0000000..7c11c93 --- /dev/null +++ b/arduino_gen/test/data/constructor/type_is_class_true.xml @@ -0,0 +1,3 @@ + + + diff --git a/arduino_gen/test/data/constructors/bool_argument_constructors.xml b/arduino_gen/test/data/constructors/bool_argument_constructors.xml deleted file mode 100644 index 9ffdcf8..0000000 --- a/arduino_gen/test/data/constructors/bool_argument_constructors.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/arduino_gen/test/data/constructors/empty_constructors.xml b/arduino_gen/test/data/constructors/empty_constructors.xml deleted file mode 100644 index 39876ee..0000000 --- a/arduino_gen/test/data/constructors/empty_constructors.xml +++ /dev/null @@ -1,2 +0,0 @@ - - diff --git a/arduino_gen/test/data/constructors/extra_attribute.xml b/arduino_gen/test/data/constructors/extra_attribute.xml new file mode 100644 index 0000000..5af6378 --- /dev/null +++ b/arduino_gen/test/data/constructors/extra_attribute.xml @@ -0,0 +1,2 @@ + + diff --git a/arduino_gen/test/data/constructors/extra_element.xml b/arduino_gen/test/data/constructors/extra_element.xml new file mode 100644 index 0000000..aefc667 --- /dev/null +++ b/arduino_gen/test/data/constructors/extra_element.xml @@ -0,0 +1,3 @@ + + + diff --git a/arduino_gen/test/data/constructors/float_argument_constructors.xml b/arduino_gen/test/data/constructors/float_argument_constructors.xml deleted file mode 100644 index aaf4dbe..0000000 --- a/arduino_gen/test/data/constructors/float_argument_constructors.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/arduino_gen/test/data/constructors/int_argument_constructors.xml b/arduino_gen/test/data/constructors/int_argument_constructors.xml deleted file mode 100644 index 2b197ba..0000000 --- a/arduino_gen/test/data/constructors/int_argument_constructors.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/arduino_gen/test/data/constructors/no_type_constructors.xml b/arduino_gen/test/data/constructors/no_type_constructors.xml deleted file mode 100644 index 5eab536..0000000 --- a/arduino_gen/test/data/constructors/no_type_constructors.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/arduino_gen/test/data/constructors/no_variable_constructors.xml b/arduino_gen/test/data/constructors/no_variable_constructors.xml deleted file mode 100644 index 30717dd..0000000 --- a/arduino_gen/test/data/constructors/no_variable_constructors.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/arduino_gen/test/data/constructors/one_constructor.xml b/arduino_gen/test/data/constructors/one_constructor.xml new file mode 100644 index 0000000..3e300a1 --- /dev/null +++ b/arduino_gen/test/data/constructors/one_constructor.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/arduino_gen/test/data/constructors/string_argument_constructors.xml b/arduino_gen/test/data/constructors/string_argument_constructors.xml deleted file mode 100644 index 8205def..0000000 --- a/arduino_gen/test/data/constructors/string_argument_constructors.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/arduino_gen/test/data/constructors/two_constructors.xml b/arduino_gen/test/data/constructors/two_constructors.xml new file mode 100644 index 0000000..942a87a --- /dev/null +++ b/arduino_gen/test/data/constructors/two_constructors.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/arduino_gen/test/data/constructors/no_type_no_variable_constructors.xml b/arduino_gen/test/data/constructors/zero_constructors.xml similarity index 100% rename from arduino_gen/test/data/constructors/no_type_no_variable_constructors.xml rename to arduino_gen/test/data/constructors/zero_constructors.xml diff --git a/arduino_gen/test/test_appendage.cpp b/arduino_gen/test/test_appendage.cpp index c8ca4e6..50699ae 100644 --- a/arduino_gen/test/test_appendage.cpp +++ b/arduino_gen/test/test_appendage.cpp @@ -1,5 +1,5 @@ -#include "appendage.hpp" -#include "exceptions.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/exceptions.hpp" #include #include @@ -188,6 +188,49 @@ namespace rip RIP_ASSERT_NO_THROW(std::make_shared(j, appendage_map)); } + + TEST(Appendage_core_config, getCoreJson) + { + nlohmann::json j; + j["label"] = "whatever"; + j["type"] = "something"; + j["pin"] = 1; + + std::multimap> appendage_map; + std::shared_ptr appendage; + + RIP_ASSERT_NO_THROW(appendage = std::make_shared(j, appendage_map, "", false)); + + ASSERT_EQ(appendage->getCoreJson(0).dump(4), + "{\n" + " \"index\": 0,\n" + " \"label\": \"whatever\",\n" + " \"type\": \"something\"\n" + "}" + ); + } + + TEST(Appendage_core_config, getCoreJson2) + { + nlohmann::json j; + j["label"] = "something"; + j["type"] = "whatever"; + j["pin"] = 2; + j["not"] = "used"; + + std::multimap> appendage_map; + std::shared_ptr appendage; + + RIP_ASSERT_NO_THROW(appendage = std::make_shared(j, appendage_map, "", false)); + + ASSERT_EQ(appendage->getCoreJson(7).dump(4), + "{\n" + " \"index\": 7,\n" + " \"label\": \"something\",\n" + " \"type\": \"whatever\"\n" + "}" + ); + } } } } diff --git a/arduino_gen/test/test_appendage_template.cpp b/arduino_gen/test/test_appendage_template.cpp index 48a015e..a2e9a6c 100644 --- a/arduino_gen/test/test_appendage_template.cpp +++ b/arduino_gen/test/test_appendage_template.cpp @@ -1,8 +1,8 @@ -#include "appendage.hpp" -#include "command.hpp" -#include "appendage_template.hpp" -#include "exceptions.hpp" -#include "xml_utils.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/command.hpp" +#include "arduino_gen/appendage_template.hpp" +#include "arduino_gen/exceptions.hpp" +#include "arduino_gen/xml_utils.hpp" #include #include @@ -41,7 +41,7 @@ namespace rip EXPECT_EQ(appendageTemplate->GetConstructors(), nullptr); EXPECT_EQ(appendageTemplate->GetSetup(), nullptr); EXPECT_EQ(appendageTemplate->GetLoop(), nullptr); - EXPECT_EQ(appendageTemplate->GetCommands().size(), 0); + EXPECT_EQ(appendageTemplate->GetCommands().size(), 0u); } TEST(AppendageTemplate_constructor, includes) diff --git a/arduino_gen/test/test_arduino_gen.cpp b/arduino_gen/test/test_arduino_gen.cpp index d0b47e7..e04bef8 100644 --- a/arduino_gen/test/test_arduino_gen.cpp +++ b/arduino_gen/test/test_arduino_gen.cpp @@ -1,11 +1,15 @@ -#include "arduino_gen.hpp" -#include "appendage.hpp" -#include "appendage_template.hpp" -#include "exceptions.hpp" +#include "arduino_gen/arduino_gen.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/appendage_template.hpp" +#include "arduino_gen/exceptions.hpp" #include #include +#include +#include +#include + #include #include @@ -25,9 +29,9 @@ namespace rip TEST_F(ArduinoGenTest, includes_no_appendages) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); ASSERT_EQ(ag->getIncludes(), "" @@ -36,9 +40,9 @@ namespace rip TEST_F(ArduinoGenTest, includes_one_empty_appendage) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); ASSERT_EQ(ag->getIncludes(), "" @@ -47,43 +51,43 @@ namespace rip TEST_F(ArduinoGenTest, includes_one_appendage) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); ASSERT_EQ(ag->getIncludes(), - "#include \n" + "#include " ); } TEST_F(ArduinoGenTest, includes_two_appendages_same) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); ASSERT_EQ(ag->getIncludes(), - "#include \n" + "#include " ); } TEST_F(ArduinoGenTest, includes_two_appendages_different) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); ASSERT_EQ(ag->getIncludes(), "#include \"Servo.h\"\n" - "#include \n" + "#include " ); } TEST_F(ArduinoGenTest, constructors_no_appendages) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); ASSERT_EQ(ag->getConstructors(), "" @@ -92,9 +96,9 @@ namespace rip TEST_F(ArduinoGenTest, constructors_one_empty_appendage) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); ASSERT_EQ(ag->getConstructors(), "" @@ -103,45 +107,1784 @@ namespace rip TEST_F(ArduinoGenTest, constructors_one_appendage) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); ASSERT_EQ(ag->getConstructors(), "NewPing sonar [1] = {\n" "\tNewPing(1, 2, 200)\n" - "};\n\n" + "};" ); } TEST_F(ArduinoGenTest, constructors_two_appendages_same) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); ASSERT_EQ(ag->getConstructors(), "NewPing sonar [2] = {\n" "\tNewPing(1, 2, 200),\n" "\tNewPing(3, 4, 200)\n" - "};\n\n" + "};" ); } TEST_F(ArduinoGenTest, constructors_two_appendages_different) { - std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "/", "test/data/arduino_gen", true)); + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); - RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json", false)); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); ASSERT_EQ(ag->getConstructors(), - "Servo servo [1] = {\n" + "Servo servos [1] = {\n" "\tServo()\n" "};\n\n" + "unsigned char servo_pins [1] = {\n" + "\t3\n" + "};\n\n" "NewPing sonar [1] = {\n" "\tNewPing(1, 2, 200)\n" + "};" + ); + } + + TEST_F(ArduinoGenTest, setup_no_appendages) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); + + ASSERT_EQ(ag->getSetup(), + "" + ); + } + + TEST_F(ArduinoGenTest, setup_one_empty_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); + + ASSERT_EQ(ag->getSetup(), + "" + ); + } + + TEST_F(ArduinoGenTest, setup_one_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); + + ASSERT_EQ(ag->getSetup(), + "\t// Ultrasonic triggerPin: 1" + ); + } + + TEST_F(ArduinoGenTest, setup_two_appendages_same) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); + + ASSERT_EQ(ag->getSetup(), + "\t// Ultrasonic triggerPin: 1\n\n" + "\t// Ultrasonic triggerPin: 3" + ); + } + + TEST_F(ArduinoGenTest, setup_two_appendages_different) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + + ASSERT_EQ(ag->getSetup(), + "\tservos[0].attach(3);\n\n" + "\t// Ultrasonic triggerPin: 1" + ); + } + + TEST_F(ArduinoGenTest, loop_no_appendages) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); + + ASSERT_EQ(ag->getLoop(), + "" + ); + } + + TEST_F(ArduinoGenTest, loop_one_empty_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); + + ASSERT_EQ(ag->getLoop(), + "" + ); + } + + TEST_F(ArduinoGenTest, loop_one_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); + + ASSERT_EQ(ag->getLoop(), + "\t// Ultrasonic echoPin: 2" + ); + } + + TEST_F(ArduinoGenTest, loop_two_appendages_same) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); + + ASSERT_EQ(ag->getLoop(), + "\t// Ultrasonic echoPin: 2\n\n" + "\t// Ultrasonic echoPin: 4" + ); + } + + TEST_F(ArduinoGenTest, loop_two_appendages_different) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + + ASSERT_EQ(ag->getLoop(), + "\t// Servo pin: 3\n\n" + "\t// Ultrasonic echoPin: 2" + ); + } + + TEST_F(ArduinoGenTest, command_enums_no_appendages) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); + + ASSERT_EQ(ag->getCommandEnums(), + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong" + ); + } + + TEST_F(ArduinoGenTest, command_enums_one_empty_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); + + ASSERT_EQ(ag->getCommandEnums(), + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong" + ); + } + + TEST_F(ArduinoGenTest, command_enums_one_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); + + ASSERT_EQ(ag->getCommandEnums(), + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong,\n" + "\tkReadUltrasonic,\n" + "\tkReadUltrasonicResult" + ); + } + + TEST_F(ArduinoGenTest, command_enums_two_appendages_same) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); + + ASSERT_EQ(ag->getCommandEnums(), + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong,\n" + "\tkReadUltrasonic,\n" + "\tkReadUltrasonicResult" + ); + } + + TEST_F(ArduinoGenTest, command_enums_two_appendages_different) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + + ASSERT_EQ(ag->getCommandEnums(), + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong,\n" + "\tkSetServo,\n" + "\tkReadUltrasonic,\n" + "\tkReadUltrasonicResult" + ); + } + + TEST_F(ArduinoGenTest, command_attaches_no_appendages) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); + + ASSERT_EQ(ag->getCommandAttaches(), + "" + ); + } + + TEST_F(ArduinoGenTest, command_attaches_one_empty_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); + + ASSERT_EQ(ag->getCommandAttaches(), + "" + ); + } + + TEST_F(ArduinoGenTest, command_attaches_one_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); + + ASSERT_EQ(ag->getCommandAttaches(), + "\tcmdMessenger.attach(kReadUltrasonic, readUltraSonic);" + ); + } + + TEST_F(ArduinoGenTest, command_attaches_two_appendages_same) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); + + ASSERT_EQ(ag->getCommandAttaches(), + "\tcmdMessenger.attach(kReadUltrasonic, readUltraSonic);" + ); + } + + TEST_F(ArduinoGenTest, command_attaches_two_appendages_different) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + + ASSERT_EQ(ag->getCommandAttaches(), + "\tcmdMessenger.attach(kSetServo, setServo);\n" + "\tcmdMessenger.attach(kReadUltrasonic, readUltraSonic);" + ); + } + + TEST_F(ArduinoGenTest, command_callbacks_no_appendages) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); + + ASSERT_EQ(ag->getCommandCallbacks(), + "" + ); + } + + TEST_F(ArduinoGenTest, command_callbacks_one_empty_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); + + ASSERT_EQ(ag->getCommandCallbacks(), + "" + ); + } + + TEST_F(ArduinoGenTest, command_callbacks_one_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); + + ASSERT_EQ(ag->getCommandCallbacks(), + "void readUltraSonic() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kReadUltrasonic);\n" + "\t\treturn;\n" + "\t}\n" + "\tunsigned long rv;\n" + "\trv = sonar[indexNum].ping_cm();\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kReadUltrasonic);\n" + "\tcmdMessenger.sendCmdStart(kReadUltrasonicResult);\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" + "}" + ); + } + + TEST_F(ArduinoGenTest, command_callbacks_two_appendages_same) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); + + ASSERT_EQ(ag->getCommandCallbacks(), + "void readUltraSonic() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 2) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kReadUltrasonic);\n" + "\t\treturn;\n" + "\t}\n" + "\tunsigned long rv;\n" + "\trv = sonar[indexNum].ping_cm();\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kReadUltrasonic);\n" + "\tcmdMessenger.sendCmdStart(kReadUltrasonicResult);\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" + "}" + ); + } + + TEST_F(ArduinoGenTest, command_callbacks_two_appendages_different) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + + ASSERT_EQ(ag->getCommandCallbacks(), + "void setServo() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kSetServo);\n" + "\t\treturn;\n" + "\t}\n" + "\tint value = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk()) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kSetServo);\n" + "\t\treturn;\n" + "\t}\n" + "\tif (!servos[indexNum].attached()) {\n" + "\t servos[indexNum].attach(servo_pins[indexNum]);\n" + "\t}\n" + "\tservos[indexNum].write(value);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetServo);\n" + "}\n" + "\n" + "void readUltraSonic() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kReadUltrasonic);\n" + "\t\treturn;\n" + "\t}\n" + "\tunsigned long rv;\n" + "\trv = sonar[indexNum].ping_cm();\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kReadUltrasonic);\n" + "\tcmdMessenger.sendCmdStart(kReadUltrasonicResult);\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" + "}" + ); + } + + TEST_F(ArduinoGenTest, arduino_code_no_appendages) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); + + ASSERT_EQ(ag->getArduinoCode(), + "// Auto-generated by ArduinoGen\n" + "\n" + "#include \"CmdMessenger.h\"\n" + "\n" + "\n" + "// Attach a new CmdMessenger object to the default Serial port\n" + "CmdMessenger cmdMessenger = CmdMessenger(Serial);\n" + "\n" + "const char LED = 13;\n" + "\n" + "\n" + "\n" + "enum\n" + "{\n" + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong\n" + "};\n" + "\n" + "void setup()\n" + "{\n" + "\t// Init LED pin\n" + "\tpinMode(LED, OUTPUT);\n" + "\n" + "\t// Initialize Serial Communication\n" + "\tSerial.begin(115200);\n" + "\n" + "\tattachCommandCallbacks();\n" + "\n" + "\n" + "\n" + "\t// Flash led 3 times at the end of setup\n" + "\tfor(int i = 0; i < 3; i++)\n" + "\t{\n" + "\t\tdigitalWrite(LED, HIGH);\n" + "\t\tdelay(250);\n" + "\t\tdigitalWrite(LED, LOW);\n" + "\t\tdelay(250);\n" + "\t}\n" + "}\n" + "\n" + "void loop()\n" + "{\n" + "\t// Process incoming serial data, and perform callbacks\n" + "\tcmdMessenger.feedinSerialData();\n" + "\n" + "\n" + "}\n" + "\n" + "//Callbacks define on which received commands we take action\n" + "void attachCommandCallbacks()\n" + "{\n" + "\tcmdMessenger.attach(unknownCommand);\n" + "\tcmdMessenger.attach(kPing, ping);\n" + "\tcmdMessenger.attach(kSetLed, setLed);\n" + "\n" + "}\n" + "\n" + "// Called when a received command has no attached function\n" + "void unknownCommand()\n" + "{\n" + "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "}\n" + "\n" + "// Called upon initialization of Spine to check connection\n" + "void ping()\n" + "{\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kPing);\n" + "\tcmdMessenger.sendBinCmd(kPingResult, kPong);\n" + "}\n" + "\n" + "// Callback function that sets led on or off\n" + "void setLed()\n" + "{\n" + "\t// Read led state argument, interpret string as boolean\n" + "\tbool ledState = cmdMessenger.readBoolArg();\n" + "\tdigitalWrite(LED, ledState);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetLed);\n" + "}\n" + "\n" + "\n" + ); + } + + TEST_F(ArduinoGenTest, arduino_code_one_empty_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); + + ASSERT_EQ(ag->getArduinoCode(), + "// Auto-generated by ArduinoGen\n" + "\n" + "#include \"CmdMessenger.h\"\n" + "\n" + "\n" + "// Attach a new CmdMessenger object to the default Serial port\n" + "CmdMessenger cmdMessenger = CmdMessenger(Serial);\n" + "\n" + "const char LED = 13;\n" + "\n" + "\n" + "\n" + "enum\n" + "{\n" + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong\n" + "};\n" + "\n" + "void setup()\n" + "{\n" + "\t// Init LED pin\n" + "\tpinMode(LED, OUTPUT);\n" + "\n" + "\t// Initialize Serial Communication\n" + "\tSerial.begin(115200);\n" + "\n" + "\tattachCommandCallbacks();\n" + "\n" + "\n" + "\n" + "\t// Flash led 3 times at the end of setup\n" + "\tfor(int i = 0; i < 3; i++)\n" + "\t{\n" + "\t\tdigitalWrite(LED, HIGH);\n" + "\t\tdelay(250);\n" + "\t\tdigitalWrite(LED, LOW);\n" + "\t\tdelay(250);\n" + "\t}\n" + "}\n" + "\n" + "void loop()\n" + "{\n" + "\t// Process incoming serial data, and perform callbacks\n" + "\tcmdMessenger.feedinSerialData();\n" + "\n" + "\n" + "}\n" + "\n" + "//Callbacks define on which received commands we take action\n" + "void attachCommandCallbacks()\n" + "{\n" + "\tcmdMessenger.attach(unknownCommand);\n" + "\tcmdMessenger.attach(kPing, ping);\n" + "\tcmdMessenger.attach(kSetLed, setLed);\n" + "\n" + "}\n" + "\n" + "// Called when a received command has no attached function\n" + "void unknownCommand()\n" + "{\n" + "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "}\n" + "\n" + "// Called upon initialization of Spine to check connection\n" + "void ping()\n" + "{\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kPing);\n" + "\tcmdMessenger.sendBinCmd(kPingResult, kPong);\n" + "}\n" + "\n" + "// Callback function that sets led on or off\n" + "void setLed()\n" + "{\n" + "\t// Read led state argument, interpret string as boolean\n" + "\tbool ledState = cmdMessenger.readBoolArg();\n" + "\tdigitalWrite(LED, ledState);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetLed);\n" + "}\n" + "\n" + "\n" + ); + } + + TEST_F(ArduinoGenTest, arduino_code_one_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); + + ASSERT_EQ(ag->getArduinoCode(), + "// Auto-generated by ArduinoGen\n" + "\n" + "#include \"CmdMessenger.h\"\n" + "#include \n" + "\n" + "// Attach a new CmdMessenger object to the default Serial port\n" + "CmdMessenger cmdMessenger = CmdMessenger(Serial);\n" + "\n" + "const char LED = 13;\n" + "\n" + "NewPing sonar [1] = {\n" + "\tNewPing(1, 2, 200)\n" + "};\n" + "\n" + "enum\n" + "{\n" + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong,\n" + "\tkReadUltrasonic,\n" + "\tkReadUltrasonicResult\n" + "};\n" + "\n" + "void setup()\n" + "{\n" + "\t// Init LED pin\n" + "\tpinMode(LED, OUTPUT);\n" + "\n" + "\t// Initialize Serial Communication\n" + "\tSerial.begin(115200);\n" + "\n" + "\tattachCommandCallbacks();\n" + "\n" + "\t// Ultrasonic triggerPin: 1\n" + "\n" + "\t// Flash led 3 times at the end of setup\n" + "\tfor(int i = 0; i < 3; i++)\n" + "\t{\n" + "\t\tdigitalWrite(LED, HIGH);\n" + "\t\tdelay(250);\n" + "\t\tdigitalWrite(LED, LOW);\n" + "\t\tdelay(250);\n" + "\t}\n" + "}\n" + "\n" + "void loop()\n" + "{\n" + "\t// Process incoming serial data, and perform callbacks\n" + "\tcmdMessenger.feedinSerialData();\n" + "\n" + "\t// Ultrasonic echoPin: 2\n" + "}\n" + "\n" + "//Callbacks define on which received commands we take action\n" + "void attachCommandCallbacks()\n" + "{\n" + "\tcmdMessenger.attach(unknownCommand);\n" + "\tcmdMessenger.attach(kPing, ping);\n" + "\tcmdMessenger.attach(kSetLed, setLed);\n" + "\tcmdMessenger.attach(kReadUltrasonic, readUltraSonic);\n" + "}\n" + "\n" + "// Called when a received command has no attached function\n" + "void unknownCommand()\n" + "{\n" + "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "}\n" + "\n" + "// Called upon initialization of Spine to check connection\n" + "void ping()\n" + "{\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kPing);\n" + "\tcmdMessenger.sendBinCmd(kPingResult, kPong);\n" + "}\n" + "\n" + "// Callback function that sets led on or off\n" + "void setLed()\n" + "{\n" + "\t// Read led state argument, interpret string as boolean\n" + "\tbool ledState = cmdMessenger.readBoolArg();\n" + "\tdigitalWrite(LED, ledState);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetLed);\n" + "}\n" + "\n" + "void readUltraSonic() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kReadUltrasonic);\n" + "\t\treturn;\n" + "\t}\n" + "\tunsigned long rv;\n" + "\trv = sonar[indexNum].ping_cm();\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kReadUltrasonic);\n" + "\tcmdMessenger.sendCmdStart(kReadUltrasonicResult);\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" + "}\n" + ); + } + + TEST_F(ArduinoGenTest, arduino_code_two_appendages_same) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); + + ASSERT_EQ(ag->getArduinoCode(), + "// Auto-generated by ArduinoGen\n" + "\n" + "#include \"CmdMessenger.h\"\n" + "#include \n" + "\n" + "// Attach a new CmdMessenger object to the default Serial port\n" + "CmdMessenger cmdMessenger = CmdMessenger(Serial);\n" + "\n" + "const char LED = 13;\n" + "\n" + "NewPing sonar [2] = {\n" + "\tNewPing(1, 2, 200),\n" + "\tNewPing(3, 4, 200)\n" + "};\n" + "\n" + "enum\n" + "{\n" + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong,\n" + "\tkReadUltrasonic,\n" + "\tkReadUltrasonicResult\n" + "};\n" + "\n" + "void setup()\n" + "{\n" + "\t// Init LED pin\n" + "\tpinMode(LED, OUTPUT);\n" + "\n" + "\t// Initialize Serial Communication\n" + "\tSerial.begin(115200);\n" + "\n" + "\tattachCommandCallbacks();\n" + "\n" + "\t// Ultrasonic triggerPin: 1\n\n" + "\t// Ultrasonic triggerPin: 3\n" + "\n" + "\t// Flash led 3 times at the end of setup\n" + "\tfor(int i = 0; i < 3; i++)\n" + "\t{\n" + "\t\tdigitalWrite(LED, HIGH);\n" + "\t\tdelay(250);\n" + "\t\tdigitalWrite(LED, LOW);\n" + "\t\tdelay(250);\n" + "\t}\n" + "}\n" + "\n" + "void loop()\n" + "{\n" + "\t// Process incoming serial data, and perform callbacks\n" + "\tcmdMessenger.feedinSerialData();\n" + "\n" + "\t// Ultrasonic echoPin: 2\n\n" + "\t// Ultrasonic echoPin: 4\n" + "}\n" + "\n" + "//Callbacks define on which received commands we take action\n" + "void attachCommandCallbacks()\n" + "{\n" + "\tcmdMessenger.attach(unknownCommand);\n" + "\tcmdMessenger.attach(kPing, ping);\n" + "\tcmdMessenger.attach(kSetLed, setLed);\n" + "\tcmdMessenger.attach(kReadUltrasonic, readUltraSonic);\n" + "}\n" + "\n" + "// Called when a received command has no attached function\n" + "void unknownCommand()\n" + "{\n" + "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "}\n" + "\n" + "// Called upon initialization of Spine to check connection\n" + "void ping()\n" + "{\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kPing);\n" + "\tcmdMessenger.sendBinCmd(kPingResult, kPong);\n" + "}\n" + "\n" + "// Callback function that sets led on or off\n" + "void setLed()\n" + "{\n" + "\t// Read led state argument, interpret string as boolean\n" + "\tbool ledState = cmdMessenger.readBoolArg();\n" + "\tdigitalWrite(LED, ledState);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetLed);\n" + "}\n" + "\n" + "void readUltraSonic() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 2) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kReadUltrasonic);\n" + "\t\treturn;\n" + "\t}\n" + "\tunsigned long rv;\n" + "\trv = sonar[indexNum].ping_cm();\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kReadUltrasonic);\n" + "\tcmdMessenger.sendCmdStart(kReadUltrasonicResult);\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" + "}\n" + ); + } + + TEST_F(ArduinoGenTest, arduino_code_two_appendages_different) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + + ASSERT_EQ(ag->getArduinoCode(), + "// Auto-generated by ArduinoGen\n" + "\n" + "#include \"CmdMessenger.h\"\n" + "#include \"Servo.h\"\n" + "#include \n" + "\n" + "// Attach a new CmdMessenger object to the default Serial port\n" + "CmdMessenger cmdMessenger = CmdMessenger(Serial);\n" + "\n" + "const char LED = 13;\n" + "\n" + "Servo servos [1] = {\n" + "\tServo()\n" + "};\n\n" + "unsigned char servo_pins [1] = {\n" + "\t3\n" + "};\n\n" + "NewPing sonar [1] = {\n" + "\tNewPing(1, 2, 200)\n" + "};\n" + "\n" + "enum\n" + "{\n" + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong,\n" + "\tkSetServo,\n" + "\tkReadUltrasonic,\n" + "\tkReadUltrasonicResult\n" + "};\n" + "\n" + "void setup()\n" + "{\n" + "\t// Init LED pin\n" + "\tpinMode(LED, OUTPUT);\n" + "\n" + "\t// Initialize Serial Communication\n" + "\tSerial.begin(115200);\n" + "\n" + "\tattachCommandCallbacks();\n" + "\n" + "\tservos[0].attach(3);\n\n" + "\t// Ultrasonic triggerPin: 1\n" + "\n" + "\t// Flash led 3 times at the end of setup\n" + "\tfor(int i = 0; i < 3; i++)\n" + "\t{\n" + "\t\tdigitalWrite(LED, HIGH);\n" + "\t\tdelay(250);\n" + "\t\tdigitalWrite(LED, LOW);\n" + "\t\tdelay(250);\n" + "\t}\n" + "}\n" + "\n" + "void loop()\n" + "{\n" + "\t// Process incoming serial data, and perform callbacks\n" + "\tcmdMessenger.feedinSerialData();\n" + "\n" + "\t// Servo pin: 3\n\n" + "\t// Ultrasonic echoPin: 2\n" + "}\n" + "\n" + "//Callbacks define on which received commands we take action\n" + "void attachCommandCallbacks()\n" + "{\n" + "\tcmdMessenger.attach(unknownCommand);\n" + "\tcmdMessenger.attach(kPing, ping);\n" + "\tcmdMessenger.attach(kSetLed, setLed);\n" + "\tcmdMessenger.attach(kSetServo, setServo);\n" + "\tcmdMessenger.attach(kReadUltrasonic, readUltraSonic);\n" + "}\n" + "\n" + "// Called when a received command has no attached function\n" + "void unknownCommand()\n" + "{\n" + "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "}\n" + "\n" + "// Called upon initialization of Spine to check connection\n" + "void ping()\n" + "{\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kPing);\n" + "\tcmdMessenger.sendBinCmd(kPingResult, kPong);\n" + "}\n" + "\n" + "// Callback function that sets led on or off\n" + "void setLed()\n" + "{\n" + "\t// Read led state argument, interpret string as boolean\n" + "\tbool ledState = cmdMessenger.readBoolArg();\n" + "\tdigitalWrite(LED, ledState);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetLed);\n" + "}\n" + "\n" + "void setServo() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kSetServo);\n" + "\t\treturn;\n" + "\t}\n" + "\tint value = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk()) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kSetServo);\n" + "\t\treturn;\n" + "\t}\n" + "\tif (!servos[indexNum].attached()) {\n" + "\t servos[indexNum].attach(servo_pins[indexNum]);\n" + "\t}\n" + "\tservos[indexNum].write(value);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetServo);\n" + "}\n" + "\n" + "void readUltraSonic() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kReadUltrasonic);\n" + "\t\treturn;\n" + "\t}\n" + "\tunsigned long rv;\n" + "\trv = sonar[indexNum].ping_cm();\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kReadUltrasonic);\n" + "\tcmdMessenger.sendCmdStart(kReadUltrasonicResult);\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" + "}\n" + ); + } + + TEST_F(ArduinoGenTest, get_core_config_no_appendages) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/no_appendages.json")); + + ASSERT_EQ(ag->getCoreConfig(), + "{\n" + " \"appendages\": [],\n" + " \"commands\": {\n" + " \"kAcknowledge\": 0,\n" + " \"kError\": 1,\n" + " \"kPing\": 4,\n" + " \"kPingResult\": 5,\n" + " \"kPong\": 6,\n" + " \"kSetLed\": 3,\n" + " \"kUnknown\": 2\n" + " }\n" + "}" + ); + } + + TEST_F(ArduinoGenTest, get_core_config_one_empty_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_empty_appendage.json")); + + ASSERT_EQ(ag->getCoreConfig(), + "{\n" + " \"appendages\": [\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Empty1\",\n" + " \"type\": \"Empty Appendage\"\n" + " }\n" + " ],\n" + " \"commands\": {\n" + " \"kAcknowledge\": 0,\n" + " \"kError\": 1,\n" + " \"kPing\": 4,\n" + " \"kPingResult\": 5,\n" + " \"kPong\": 6,\n" + " \"kSetLed\": 3,\n" + " \"kUnknown\": 2\n" + " }\n" + "}" + ); + } + + TEST_F(ArduinoGenTest, get_core_config_one_appendage) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/one_appendage.json")); + + ASSERT_EQ(ag->getCoreConfig(), + "{\n" + " \"appendages\": [\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Ultrasonic1\",\n" + " \"type\": \"Ultrasonic\"\n" + " }\n" + " ],\n" + " \"commands\": {\n" + " \"kAcknowledge\": 0,\n" + " \"kError\": 1,\n" + " \"kPing\": 4,\n" + " \"kPingResult\": 5,\n" + " \"kPong\": 6,\n" + " \"kReadUltrasonic\": 7,\n" + " \"kReadUltrasonicResult\": 8,\n" + " \"kSetLed\": 3,\n" + " \"kUnknown\": 2\n" + " }\n" + "}" + ); + } + + TEST_F(ArduinoGenTest, get_core_config_two_appendages_same) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_same.json")); + + ASSERT_EQ(ag->getCoreConfig(), + "{\n" + " \"appendages\": [\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Ultrasonic1\",\n" + " \"type\": \"Ultrasonic\"\n" + " },\n" + " {\n" + " \"index\": 1,\n" + " \"label\": \"Ultrasonic2\",\n" + " \"type\": \"Ultrasonic\"\n" + " }\n" + " ],\n" + " \"commands\": {\n" + " \"kAcknowledge\": 0,\n" + " \"kError\": 1,\n" + " \"kPing\": 4,\n" + " \"kPingResult\": 5,\n" + " \"kPong\": 6,\n" + " \"kReadUltrasonic\": 7,\n" + " \"kReadUltrasonicResult\": 8,\n" + " \"kSetLed\": 3,\n" + " \"kUnknown\": 2\n" + " }\n" + "}" + ); + } + + TEST_F(ArduinoGenTest, get_core_config_two_appendages_different) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + + ASSERT_EQ(ag->getCoreConfig(), + "{\n" + " \"appendages\": [\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Servo1\",\n" + " \"type\": \"Servo\"\n" + " },\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Ultrasonic1\",\n" + " \"type\": \"Ultrasonic\"\n" + " }\n" + " ],\n" + " \"commands\": {\n" + " \"kAcknowledge\": 0,\n" + " \"kError\": 1,\n" + " \"kPing\": 4,\n" + " \"kPingResult\": 5,\n" + " \"kPong\": 6,\n" + " \"kReadUltrasonic\": 8,\n" + " \"kReadUltrasonicResult\": 9,\n" + " \"kSetLed\": 3,\n" + " \"kSetServo\": 7,\n" + " \"kUnknown\": 2\n" + " }\n" + "}" + ); + } + + TEST_F(ArduinoGenTest, get_upload_script) + { + std::unique_ptr ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen")); + + ASSERT_EQ(ag->getUploadScript(), + "#!/usr/bin/env bash\n" + "\n" + "cd ./test/CurrentArduinoCode/mega\n" + "\n" + "git add -A\n" + "git commit -m \"New code for mega\"\n" + "git push\n" + "\n" + "pio run -t upload\n" + ); + } + + TEST_F(ArduinoGenTest, generate_output) + { + using namespace cppfs; + + //Create cppfs file handles + FileHandle device_folder = fs::open("./test/mega"); + FileHandle source_folder = fs::open("./test/mega/src"); + + FileHandle source = fs::open("./test/mega/src/mega.ino"); + FileHandle config = fs::open("./test/mega/mega.json"); + FileHandle core = fs::open("./test/mega/mega_core.json"); + FileHandle upload = fs::open("./test/mega/upload.sh"); + FileHandle serial = fs::open("./test/mega/serial.sh"); + FileHandle platformio = fs::open("./test/mega/platformio.ini"); + + FileHandle erase_me = fs::open("./test/mega/erase_me"); + + // Create Blank slate + if (device_folder.exists()) + { + device_folder.removeDirectoryRec(); + } + + // Create directory with an empty file to make sure setupFolder erases it. + device_folder.createDirectory(); + FileHandle test_file = fs::open("./test/mega/erase_me"); + test_file.writeFile(""); + + // Construct, readConfig, and generateOutput + std::unique_ptr ag; + RIP_ASSERT_NO_THROW(ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen"))); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + RIP_ASSERT_NO_THROW(ag->generateOutput(false)); + + // Update file handles + device_folder.updateFileInfo(); + source_folder.updateFileInfo(); + source.updateFileInfo(); + config.updateFileInfo(); + core.updateFileInfo(); + upload.updateFileInfo(); + serial.updateFileInfo(); + platformio.updateFileInfo(); + erase_me.updateFileInfo(); + + // Test device folder + ASSERT_TRUE(device_folder.exists()); + ASSERT_TRUE(device_folder.isDirectory()); + + // Get all files in the device folder + std::vector files = device_folder.listFiles(); + + // Test source folder and remove the name from the files vector + ASSERT_TRUE(source_folder.exists()); + ASSERT_TRUE(source_folder.isDirectory()); + files.erase(std::find(files.begin(), files.end(), "src")); + + // Get all files in the source folder, prepend "src/" to the source folder files, and + // append the source folder files to the files vector + std::vector source_folder_files = source_folder.listFiles(); + std::for_each(source_folder_files.begin(), source_folder_files.end(), [](std::string &str){ str = "src/" + str; }); + files.insert(files.end(), source_folder_files.begin(), source_folder_files.end()); + + // Make sure the test file doesn't exist + ASSERT_FALSE(test_file.exists()); + + // Test src/mega.ino and remove it from the files vector + ASSERT_TRUE(source.exists()); + ASSERT_TRUE(source.isFile()); + EXPECT_EQ(source.readFile(), + "// Auto-generated by ArduinoGen\n" + "\n" + "#include \"CmdMessenger.h\"\n" + "#include \"Servo.h\"\n" + "#include \n" + "\n" + "// Attach a new CmdMessenger object to the default Serial port\n" + "CmdMessenger cmdMessenger = CmdMessenger(Serial);\n" + "\n" + "const char LED = 13;\n" + "\n" + "Servo servos [1] = {\n" + "\tServo()\n" + "};\n\n" + "unsigned char servo_pins [1] = {\n" + "\t3\n" "};\n\n" + "NewPing sonar [1] = {\n" + "\tNewPing(1, 2, 200)\n" + "};\n" + "\n" + "enum\n" + "{\n" + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong,\n" + "\tkSetServo,\n" + "\tkReadUltrasonic,\n" + "\tkReadUltrasonicResult\n" + "};\n" + "\n" + "void setup()\n" + "{\n" + "\t// Init LED pin\n" + "\tpinMode(LED, OUTPUT);\n" + "\n" + "\t// Initialize Serial Communication\n" + "\tSerial.begin(115200);\n" + "\n" + "\tattachCommandCallbacks();\n" + "\n" + "\tservos[0].attach(3);\n\n" + "\t// Ultrasonic triggerPin: 1\n" + "\n" + "\t// Flash led 3 times at the end of setup\n" + "\tfor(int i = 0; i < 3; i++)\n" + "\t{\n" + "\t\tdigitalWrite(LED, HIGH);\n" + "\t\tdelay(250);\n" + "\t\tdigitalWrite(LED, LOW);\n" + "\t\tdelay(250);\n" + "\t}\n" + "}\n" + "\n" + "void loop()\n" + "{\n" + "\t// Process incoming serial data, and perform callbacks\n" + "\tcmdMessenger.feedinSerialData();\n" + "\n" + "\t// Servo pin: 3\n\n" + "\t// Ultrasonic echoPin: 2\n" + "}\n" + "\n" + "//Callbacks define on which received commands we take action\n" + "void attachCommandCallbacks()\n" + "{\n" + "\tcmdMessenger.attach(unknownCommand);\n" + "\tcmdMessenger.attach(kPing, ping);\n" + "\tcmdMessenger.attach(kSetLed, setLed);\n" + "\tcmdMessenger.attach(kSetServo, setServo);\n" + "\tcmdMessenger.attach(kReadUltrasonic, readUltraSonic);\n" + "}\n" + "\n" + "// Called when a received command has no attached function\n" + "void unknownCommand()\n" + "{\n" + "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "}\n" + "\n" + "// Called upon initialization of Spine to check connection\n" + "void ping()\n" + "{\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kPing);\n" + "\tcmdMessenger.sendBinCmd(kPingResult, kPong);\n" + "}\n" + "\n" + "// Callback function that sets led on or off\n" + "void setLed()\n" + "{\n" + "\t// Read led state argument, interpret string as boolean\n" + "\tbool ledState = cmdMessenger.readBoolArg();\n" + "\tdigitalWrite(LED, ledState);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetLed);\n" + "}\n" + "\n" + "void setServo() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kSetServo);\n" + "\t\treturn;\n" + "\t}\n" + "\tint value = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk()) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kSetServo);\n" + "\t\treturn;\n" + "\t}\n" + "\tif (!servos[indexNum].attached()) {\n" + "\t servos[indexNum].attach(servo_pins[indexNum]);\n" + "\t}\n" + "\tservos[indexNum].write(value);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetServo);\n" + "}\n" + "\n" + "void readUltraSonic() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kReadUltrasonic);\n" + "\t\treturn;\n" + "\t}\n" + "\tunsigned long rv;\n" + "\trv = sonar[indexNum].ping_cm();\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kReadUltrasonic);\n" + "\tcmdMessenger.sendCmdStart(kReadUltrasonicResult);\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" + "}\n" + ); + files.erase(std::find(files.begin(), files.end(), "src/mega.ino")); + + // Test mega.json and remove it from the files vector + ASSERT_TRUE(config.exists()); + ASSERT_TRUE(config.isFile()); + EXPECT_EQ(config.readFile(), + "[\n" + " {\n" + " \"type\": \"Ultrasonic\",\n" + " \"label\": \"Ultrasonic1\",\n" + " \"triggerPin\": 1,\n" + " \"echoPin\": 2\n" + " },\n" + " {\n" + " \"type\": \"Servo\",\n" + " \"label\": \"Servo1\",\n" + " \"pin\": 3\n" + " }\n" + "]\n" + ); + files.erase(std::find(files.begin(), files.end(), "mega.json")); + + // Test mega_core.json and remove it from the files vector + ASSERT_TRUE(core.exists()); + ASSERT_TRUE(core.isFile()); + EXPECT_EQ(core.readFile(), + "{\n" + " \"appendages\": [\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Servo1\",\n" + " \"type\": \"Servo\"\n" + " },\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Ultrasonic1\",\n" + " \"type\": \"Ultrasonic\"\n" + " }\n" + " ],\n" + " \"commands\": {\n" + " \"kAcknowledge\": 0,\n" + " \"kError\": 1,\n" + " \"kPing\": 4,\n" + " \"kPingResult\": 5,\n" + " \"kPong\": 6,\n" + " \"kReadUltrasonic\": 8,\n" + " \"kReadUltrasonicResult\": 9,\n" + " \"kSetLed\": 3,\n" + " \"kSetServo\": 7,\n" + " \"kUnknown\": 2\n" + " }\n" + "}" ); + files.erase(std::find(files.begin(), files.end(), "mega_core.json")); + + // Test upload.sh and remove it from the files vector + ASSERT_TRUE(upload.exists()); + ASSERT_TRUE(upload.isFile()); + EXPECT_EQ(upload.readFile(), + "#!/usr/bin/env bash\n" + "\n" + "cd ./test/CurrentArduinoCode/mega\n" + "\n" + "git add -A\n" + "git commit -m \"New code for mega\"\n" + "git push\n" + "\n" + "pio run -t upload\n" + ); + files.erase(std::find(files.begin(), files.end(), "upload.sh")); + + // Test serial.sh and remove it from the files vector + ASSERT_TRUE(serial.exists()); + ASSERT_TRUE(serial.isFile()); + EXPECT_EQ(serial.readFile(), + "#!/usr/bin/env bash\n" + "\n" + "picocom /dev/mega -b 115200 --echo\n" + ); + files.erase(std::find(files.begin(), files.end(), "serial.sh")); + + // Test platformio.ini and remove it from the files vector + ASSERT_TRUE(platformio.exists()); + ASSERT_TRUE(platformio.isFile()); + EXPECT_EQ(platformio.readFile(), + "[platformio]\n" + "lib_dir = /Robot/ArduinoLibraries\n" + "env_default = mega\n" + "\n" + "[env:mega]\n" + "platform = atmelavr\n" + "framework = arduino\n" + "board = uno\n" + "upload_port = /dev/mega\n" + ); + files.erase(std::find(files.begin(), files.end(), "platformio.ini")); + + // Make sure there aren't any extra files + ASSERT_EQ(files.size(), 0); + + // Check the file permissions + #ifndef _WIN32 + EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + EXPECT_EQ(source.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + EXPECT_EQ(config.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + EXPECT_EQ(core.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + EXPECT_EQ(upload.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + EXPECT_EQ(serial.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + EXPECT_EQ(platformio.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + #endif + + // Change the platformio.ini file + platformio.writeFile( + "[platformio]\n" + "lib_dir = /Robot/ArduinoLibraries\n" + "env_default = mega\n" + "\n" + "[env:mega]\n" + "platform = atmelavr\n" + "framework = arduino\n" + "board = mega\n" // from "uno" to "mega" + "upload_port = /dev/mega\n" + ); + + // Construct, readConfig, and generateOutput + RIP_ASSERT_NO_THROW(ag = std::unique_ptr(new ArduinoGen("mega", "./test", "./test/CurrentArduinoCode", "test/data/arduino_gen"))); + RIP_ASSERT_NO_THROW(ag->readConfig("test/data/arduino_gen/two_appendages_different.json")); + RIP_ASSERT_NO_THROW(ag->generateOutput(true)); + + // Update file handles + device_folder.updateFileInfo(); + source_folder.updateFileInfo(); + source.updateFileInfo(); + config.updateFileInfo(); + core.updateFileInfo(); + upload.updateFileInfo(); + serial.updateFileInfo(); + platformio.updateFileInfo(); + + // Test device folder + ASSERT_TRUE(device_folder.exists()); + ASSERT_TRUE(device_folder.isDirectory()); + + // Get all files in the device folder + files = device_folder.listFiles(); + + // Test source folder and remove the name from the files vector + ASSERT_TRUE(source_folder.exists()); + ASSERT_TRUE(source_folder.isDirectory()); + files.erase(std::find(files.begin(), files.end(), "src")); + + // Get all files in the source folder, prepend "src/" to the source folder files, and + // append the source folder files to the files vector + source_folder_files = source_folder.listFiles(); + std::for_each(source_folder_files.begin(), source_folder_files.end(), [](std::string &str){ str = "src/" + str; }); + files.insert(files.end(), source_folder_files.begin(), source_folder_files.end()); + + // Test src/mega.ino and remove it from the files vector + ASSERT_TRUE(source.exists()); + ASSERT_TRUE(source.isFile()); + EXPECT_EQ(source.readFile(), + "// Auto-generated by ArduinoGen\n" + "\n" + "#include \"CmdMessenger.h\"\n" + "#include \"Servo.h\"\n" + "#include \n" + "\n" + "// Attach a new CmdMessenger object to the default Serial port\n" + "CmdMessenger cmdMessenger = CmdMessenger(Serial);\n" + "\n" + "const char LED = 13;\n" + "\n" + "Servo servos [1] = {\n" + "\tServo()\n" + "};\n\n" + "unsigned char servo_pins [1] = {\n" + "\t3\n" + "};\n\n" + "NewPing sonar [1] = {\n" + "\tNewPing(1, 2, 200)\n" + "};\n" + "\n" + "enum\n" + "{\n" + "\tkAcknowledge,\n" + "\tkError,\n" + "\tkUnknown,\n" + "\tkSetLed,\n" + "\tkPing,\n" + "\tkPingResult,\n" + "\tkPong,\n" + "\tkSetServo,\n" + "\tkReadUltrasonic,\n" + "\tkReadUltrasonicResult\n" + "};\n" + "\n" + "void setup()\n" + "{\n" + "\t// Init LED pin\n" + "\tpinMode(LED, OUTPUT);\n" + "\n" + "\t// Initialize Serial Communication\n" + "\tSerial.begin(115200);\n" + "\n" + "\tattachCommandCallbacks();\n" + "\n" + "\tservos[0].attach(3);\n\n" + "\t// Ultrasonic triggerPin: 1\n" + "\n" + "\t// Flash led 3 times at the end of setup\n" + "\tfor(int i = 0; i < 3; i++)\n" + "\t{\n" + "\t\tdigitalWrite(LED, HIGH);\n" + "\t\tdelay(250);\n" + "\t\tdigitalWrite(LED, LOW);\n" + "\t\tdelay(250);\n" + "\t}\n" + "}\n" + "\n" + "void loop()\n" + "{\n" + "\t// Process incoming serial data, and perform callbacks\n" + "\tcmdMessenger.feedinSerialData();\n" + "\n" + "\t// Servo pin: 3\n\n" + "\t// Ultrasonic echoPin: 2\n" + "}\n" + "\n" + "//Callbacks define on which received commands we take action\n" + "void attachCommandCallbacks()\n" + "{\n" + "\tcmdMessenger.attach(unknownCommand);\n" + "\tcmdMessenger.attach(kPing, ping);\n" + "\tcmdMessenger.attach(kSetLed, setLed);\n" + "\tcmdMessenger.attach(kSetServo, setServo);\n" + "\tcmdMessenger.attach(kReadUltrasonic, readUltraSonic);\n" + "}\n" + "\n" + "// Called when a received command has no attached function\n" + "void unknownCommand()\n" + "{\n" + "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "}\n" + "\n" + "// Called upon initialization of Spine to check connection\n" + "void ping()\n" + "{\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kPing);\n" + "\tcmdMessenger.sendBinCmd(kPingResult, kPong);\n" + "}\n" + "\n" + "// Callback function that sets led on or off\n" + "void setLed()\n" + "{\n" + "\t// Read led state argument, interpret string as boolean\n" + "\tbool ledState = cmdMessenger.readBoolArg();\n" + "\tdigitalWrite(LED, ledState);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetLed);\n" + "}\n" + "\n" + "void setServo() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kSetServo);\n" + "\t\treturn;\n" + "\t}\n" + "\tint value = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk()) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kSetServo);\n" + "\t\treturn;\n" + "\t}\n" + "\tif (!servos[indexNum].attached()) {\n" + "\t servos[indexNum].attach(servo_pins[indexNum]);\n" + "\t}\n" + "\tservos[indexNum].write(value);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kSetServo);\n" + "}\n" + "\n" + "void readUltraSonic() {\n" + "\tint indexNum = cmdMessenger.readBinArg();\n" + "\tif(!cmdMessenger.isArgOk() || indexNum < 0 || indexNum > 1) {\n" + "\t\tcmdMessenger.sendBinCmd(kError, kReadUltrasonic);\n" + "\t\treturn;\n" + "\t}\n" + "\tunsigned long rv;\n" + "\trv = sonar[indexNum].ping_cm();\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kReadUltrasonic);\n" + "\tcmdMessenger.sendCmdStart(kReadUltrasonicResult);\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" + "}\n" + ); + files.erase(std::find(files.begin(), files.end(), "src/mega.ino")); + + // Test mega.json and remove it from the files vector + ASSERT_TRUE(config.exists()); + ASSERT_TRUE(config.isFile()); + EXPECT_EQ(config.readFile(), + "[\n" + " {\n" + " \"type\": \"Ultrasonic\",\n" + " \"label\": \"Ultrasonic1\",\n" + " \"triggerPin\": 1,\n" + " \"echoPin\": 2\n" + " },\n" + " {\n" + " \"type\": \"Servo\",\n" + " \"label\": \"Servo1\",\n" + " \"pin\": 3\n" + " }\n" + "]\n" + ); + files.erase(std::find(files.begin(), files.end(), "mega.json")); + + // Test mega_core.json and remove it from the files vector + ASSERT_TRUE(core.exists()); + ASSERT_TRUE(core.isFile()); + EXPECT_EQ(core.readFile(), + "{\n" + " \"appendages\": [\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Servo1\",\n" + " \"type\": \"Servo\"\n" + " },\n" + " {\n" + " \"index\": 0,\n" + " \"label\": \"Ultrasonic1\",\n" + " \"type\": \"Ultrasonic\"\n" + " }\n" + " ],\n" + " \"commands\": {\n" + " \"kAcknowledge\": 0,\n" + " \"kError\": 1,\n" + " \"kPing\": 4,\n" + " \"kPingResult\": 5,\n" + " \"kPong\": 6,\n" + " \"kReadUltrasonic\": 8,\n" + " \"kReadUltrasonicResult\": 9,\n" + " \"kSetLed\": 3,\n" + " \"kSetServo\": 7,\n" + " \"kUnknown\": 2\n" + " }\n" + "}" + ); + files.erase(std::find(files.begin(), files.end(), "mega_core.json")); + + // Test upload.sh and remove it from the files vector + ASSERT_TRUE(upload.exists()); + ASSERT_TRUE(upload.isFile()); + EXPECT_EQ(upload.readFile(), + "#!/usr/bin/env bash\n" + "\n" + "cd ./test/CurrentArduinoCode/mega\n" + "\n" + "git add -A\n" + "git commit -m \"New code for mega\"\n" + "git push\n" + "\n" + "pio run -t upload\n" + ); + files.erase(std::find(files.begin(), files.end(), "upload.sh")); + + // Test serial.sh and remove it from the files vector + ASSERT_TRUE(serial.exists()); + ASSERT_TRUE(serial.isFile()); + EXPECT_EQ(serial.readFile(), + "#!/usr/bin/env bash\n" + "\n" + "picocom /dev/mega -b 115200 --echo\n" + ); + files.erase(std::find(files.begin(), files.end(), "serial.sh")); + + // Test platformio.ini and remove it from the files vector + ASSERT_TRUE(platformio.exists()); + ASSERT_TRUE(platformio.isFile()); + EXPECT_EQ(platformio.readFile(), + "[platformio]\n" + "lib_dir = /Robot/ArduinoLibraries\n" + "env_default = mega\n" + "\n" + "[env:mega]\n" + "platform = atmelavr\n" + "framework = arduino\n" + "board = mega\n" + "upload_port = /dev/mega\n" + ); + files.erase(std::find(files.begin(), files.end(), "platformio.ini")); + + // Make sure there aren't any extra files + ASSERT_EQ(files.size(), 0); + + // Check the file permissions + #ifndef _WIN32 + EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + EXPECT_EQ(source.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + EXPECT_EQ(config.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + EXPECT_EQ(core.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + EXPECT_EQ(upload.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + EXPECT_EQ(serial.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + EXPECT_EQ(platformio.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | + FilePermissions::GroupRead | FilePermissions::GroupWrite | + FilePermissions::OtherRead | FilePermissions::OtherWrite); + #endif + + // Cleanup + fs::open("./test/mega").removeDirectoryRec(); } } } diff --git a/arduino_gen/test/test_code.cpp b/arduino_gen/test/test_code.cpp index 6c0cc9a..d3ca400 100644 --- a/arduino_gen/test/test_code.cpp +++ b/arduino_gen/test/test_code.cpp @@ -1,6 +1,6 @@ -#include "code.hpp" -#include "exceptions.hpp" -#include "xml_utils.hpp" +#include "arduino_gen/code.hpp" +#include "arduino_gen/exceptions.hpp" +#include "arduino_gen/xml_utils.hpp" #include #include diff --git a/arduino_gen/test/test_command.cpp b/arduino_gen/test/test_command.cpp index 1f31c96..792a6f1 100644 --- a/arduino_gen/test/test_command.cpp +++ b/arduino_gen/test/test_command.cpp @@ -1,6 +1,6 @@ -#include "command.hpp" -#include "exceptions.hpp" -#include "xml_utils.hpp" +#include "arduino_gen/command.hpp" +#include "arduino_gen/exceptions.hpp" +#include "arduino_gen/xml_utils.hpp" #include #include @@ -180,7 +180,7 @@ namespace rip "\t}\n" "\t// Proper indentation is semi-important\n" "\t// Even on the second line\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, No);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, No);\n" "}\n"); } @@ -209,7 +209,7 @@ namespace rip "\t\treturn;\n" "\t}\n" "\t// All i'm saying, is that I need an index-num\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, Easter);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, Easter);\n" "}\n"); } @@ -233,7 +233,7 @@ namespace rip ASSERT_EQ(command->callback(1), "void Picky() {\n" "\t// That other guy is too needy, I don't need an index-num\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, Not);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, Not);\n" "}\n"); } @@ -267,7 +267,7 @@ namespace rip "\t\treturn;\n" "\t}\n" "\t// Got your nose!\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, Jurassic);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, Jurassic);\n" "}\n"); } @@ -306,7 +306,7 @@ namespace rip "\t\treturn;\n" "\t}\n" "\t// I should probably do something with \"a\" and \"b\"\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, ACleverId);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, ACleverId);\n" "}\n"); } @@ -336,10 +336,10 @@ namespace rip "\t}\n" "\tint cloud;\n" "\tcloud = 9;\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, Euphoria);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, Euphoria);\n" "\tcmdMessenger.sendCmdStart(EuphoriaResult);\n" - "\tcmdMessenger.sendBinArg(cloud);\n" - "\tcmdMeessenger.sendCmdEnd();\n" + "\tcmdMessenger.sendCmdBinArg(cloud);\n" + "\tcmdMessenger.sendCmdEnd();\n" "}\n"); } @@ -372,11 +372,11 @@ namespace rip "\tint NotAsBadAsOne;\n" "\tTheLoneliestNumber = 1;\n" "\tNotAsBadAsOne = 2;\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, ThreeDog);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, ThreeDog);\n" "\tcmdMessenger.sendCmdStart(ThreeDogResult);\n" - "\tcmdMessenger.sendBinArg(TheLoneliestNumber);\n" - "\tcmdMessenger.sendBinArg(NotAsBadAsOne);\n" - "\tcmdMeessenger.sendCmdEnd();\n" + "\tcmdMessenger.sendCmdBinArg(TheLoneliestNumber);\n" + "\tcmdMessenger.sendCmdBinArg(NotAsBadAsOne);\n" + "\tcmdMessenger.sendCmdEnd();\n" "}\n"); } @@ -411,10 +411,10 @@ namespace rip "\t}\n" "\tchar out;\n" "\tout = in;\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, kEcho);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kEcho);\n" "\tcmdMessenger.sendCmdStart(kEchoResult);\n" - "\tcmdMessenger.sendBinArg(out);\n" - "\tcmdMeessenger.sendCmdEnd();\n" + "\tcmdMessenger.sendCmdBinArg(out);\n" + "\tcmdMessenger.sendCmdEnd();\n" "}\n"); } @@ -458,10 +458,10 @@ namespace rip "\trv = 1;\n" "\tfor (int i = 0; i < n; i++)\n" "\t rv *= x;\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, kExponent);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kExponent);\n" "\tcmdMessenger.sendCmdStart(kExponentResult);\n" - "\tcmdMessenger.sendBinArg(rv);\n" - "\tcmdMeessenger.sendCmdEnd();\n" + "\tcmdMessenger.sendCmdBinArg(rv);\n" + "\tcmdMessenger.sendCmdEnd();\n" "}\n"); } @@ -497,11 +497,11 @@ namespace rip "\tint rv1;\n" "\tint rv2;\n" "\t// Make two outputs from one input\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, kOneToTwo);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kOneToTwo);\n" "\tcmdMessenger.sendCmdStart(kOneToTwoResult);\n" - "\tcmdMessenger.sendBinArg(rv1);\n" - "\tcmdMessenger.sendBinArg(rv2);\n" - "\tcmdMeessenger.sendCmdEnd();\n" + "\tcmdMessenger.sendCmdBinArg(rv1);\n" + "\tcmdMessenger.sendCmdBinArg(rv2);\n" + "\tcmdMessenger.sendCmdEnd();\n" "}\n"); } @@ -544,11 +544,11 @@ namespace rip "\tint remainder;\n" "\tquotient = a / b;\n" "\tremainder = a % b;\n" - "\tcmdMessenger.sendBindCmd(kAcknowledge, kDivide);\n" + "\tcmdMessenger.sendBinCmd(kAcknowledge, kDivide);\n" "\tcmdMessenger.sendCmdStart(kDivideResult);\n" - "\tcmdMessenger.sendBinArg(quotient);\n" - "\tcmdMessenger.sendBinArg(remainder);\n" - "\tcmdMeessenger.sendCmdEnd();\n" + "\tcmdMessenger.sendCmdBinArg(quotient);\n" + "\tcmdMessenger.sendCmdBinArg(remainder);\n" + "\tcmdMessenger.sendCmdEnd();\n" "}\n"); } } diff --git a/arduino_gen/test/test_constructor.cpp b/arduino_gen/test/test_constructor.cpp new file mode 100644 index 0000000..20a497e --- /dev/null +++ b/arduino_gen/test/test_constructor.cpp @@ -0,0 +1,334 @@ +#include "arduino_gen/constructor.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/exceptions.hpp" +#include "arduino_gen/xml_utils.hpp" +#include "arduino_gen/utils.hpp" + +#include +#include +#include + +#include +#include +#include + +using Constructor = rip::arduinogen::Constructor; +using AttributeException = rip::arduinogen::AttributeException; +using ElementException = rip::arduinogen::ElementException; +using rip::arduinogen::loadXmlFile; +using rip::arduinogen::get_mmap_values_at_index; + +namespace rip +{ + namespace arduinogen + { + namespace test + { + TEST(Constructor, NoTypeNoVariable) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/no_type_no_variable.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + ASSERT_THROW(constructor = std::unique_ptr(new Constructor(xml)), AttributeException); + } + + TEST(Constructor, NoType) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/no_type.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + ASSERT_THROW(constructor = std::unique_ptr(new Constructor(xml)), AttributeException); + } + + TEST(Constructor, NoVariable) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/no_variable.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + ASSERT_THROW(constructor = std::unique_ptr(new Constructor(xml)), AttributeException); + } + + TEST(Constructor, Empty) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/empty.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + std::vector> appendages; + + ASSERT_EQ(constructor->toString(appendages), + "Type type [0] = {\n" + "};\n" + ); + } + + TEST(Constructor, IntArgument) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/int_argument.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + nlohmann::json j; + j["something"] = 1; + + std::multimap> appendage_map; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); + + ASSERT_EQ(constructor->toString(appendages), + "Type type [1] = {\n" + "\tType(1)\n" + "};\n" + ); + } + + TEST(Constructor, FloatArgument) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/float_argument.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + nlohmann::json j; + j["something"] = 3.0f; + + std::multimap< std::string, std::shared_ptr > appendage_map; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); + + ASSERT_EQ(constructor->toString(appendages), + "Type type [1] = {\n" + "\tType(3.000000)\n" + "};\n" + ); + } + + TEST(Constructor, BoolArgument) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/bool_argument.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + nlohmann::json j; + j["something"] = true; + + std::multimap< std::string, std::shared_ptr > appendage_map; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); + + ASSERT_EQ(constructor->toString(appendages), + "Type type [1] = {\n" + "\tType(true)\n" + "};\n" + ); + } + + TEST(Constructor, StringArgument) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/string_argument.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + nlohmann::json j; + j["something"] = "asdf"; + + std::multimap< std::string, std::shared_ptr > appendage_map; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); + + ASSERT_EQ(appendages.size(), 1); + + ASSERT_EQ(constructor->toString(appendages), + "Type type [1] = {\n" + "\tType(\"asdf\")\n" + "};\n" + ); + } + + TEST(Constructor, SingleAppendageMultipleArgument) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/multiple_argument.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + nlohmann::json j; + j["something_int"] = 1; + j["something_float"] = 16.0f; + j["something_string"] = "four"; + j["something_bool"] = true; + + std::multimap< std::string, std::shared_ptr > appendage_map; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + appendage_map.emplace(std::make_pair("Other Type", std::make_shared(j, appendage_map, "", false))); + appendage_map.emplace(std::make_pair("abc Type", std::make_shared(j, appendage_map, "", false))); + appendage_map.emplace(std::make_pair("one more Type", std::make_shared(j, appendage_map, "", false))); + + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); + + ASSERT_EQ(appendages.size(), 1); + + ASSERT_EQ(constructor->toString(appendages), + "Type type [1] = {\n" + "\tType(1, true, 16.000000, \"four\")\n" + "};\n" + ); + } + + TEST(Constructor, MultipleAppendageMultipleArgument) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/multiple_argument.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + nlohmann::json j; + j["something_int"] = 1; + j["something_float"] = 16.0f; + j["something_string"] = "four"; + j["something_bool"] = true; + + std::multimap< std::string, std::shared_ptr > appendage_map; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + j["something_int"] = 2; + j["something_float"] = 1290.0f; + j["something_string"] = "three"; + j["something_bool"] = false; + + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + j["something_int"] = 3; + j["something_float"] = 2.5f; + j["something_string"] = "two"; + j["something_bool"] = true; + + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + j["something_int"] = 4; + j["something_float"] = 1.125f; + j["something_string"] = "one"; + j["something_bool"] = false; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + appendage_map.emplace(std::make_pair("Other Type", std::make_shared(j, appendage_map, "", false))); + appendage_map.emplace(std::make_pair("abc Type", std::make_shared(j, appendage_map, "", false))); + appendage_map.emplace(std::make_pair("one more Type", std::make_shared(j, appendage_map, "", false))); + + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); + + ASSERT_EQ(appendages.size(), 4); + + ASSERT_EQ(constructor->toString(appendages), + "Type type [4] = {\n" + "\tType(1, true, 16.000000, \"four\"),\n" + "\tType(2, false, 1290.000000, \"three\"),\n" + "\tType(3, true, 2.500000, \"two\"),\n" + "\tType(4, false, 1.125000, \"one\")\n" + "};\n" + ); + } + + TEST(Constructor, type_is_class_true) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/type_is_class_true.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + nlohmann::json j; + j["pin"] = 1; + + std::multimap> appendage_map; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); + + ASSERT_EQ(constructor->toString(appendages), + "Type type [1] = {\n" + "\tType(1)\n" + "};\n" + ); + } + + TEST(Constructor, type_is_class_false) + { + tinyxml2::XMLDocument doc; + ASSERT_EQ(loadXmlFile(doc, "test/data/constructor/type_is_class_false.xml", { }), tinyxml2::XML_SUCCESS); + + tinyxml2::XMLElement* xml = doc.FirstChildElement("constructor"); + ASSERT_NE(xml, nullptr); + + std::unique_ptr constructor; + RIP_ASSERT_NO_THROW(constructor = std::unique_ptr(new Constructor(xml))); + + nlohmann::json j; + j["pin"] = 1; + + std::multimap> appendage_map; + appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); + + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); + + ASSERT_EQ(constructor->toString(appendages), + "unsigned char pins [1] = {\n" + "\t1\n" + "};\n" + ); + } + } + } +} diff --git a/arduino_gen/test/test_constructors.cpp b/arduino_gen/test/test_constructors.cpp index 7bbaf03..ac00e4c 100644 --- a/arduino_gen/test/test_constructors.cpp +++ b/arduino_gen/test/test_constructors.cpp @@ -1,8 +1,8 @@ -#include "constructors.hpp" -#include "appendage.hpp" -#include "exceptions.hpp" -#include "xml_utils.hpp" -#include "utils.hpp" +#include "arduino_gen/constructors.hpp" +#include "arduino_gen/appendage.hpp" +#include "arduino_gen/exceptions.hpp" +#include "arduino_gen/xml_utils.hpp" +#include "arduino_gen/utils.hpp" #include #include @@ -16,7 +16,7 @@ using Constructors = rip::arduinogen::Constructors; using AttributeException = rip::arduinogen::AttributeException; using ElementException = rip::arduinogen::ElementException; using rip::arduinogen::loadXmlFile; -using rip::arduinogen::mmap_to_vector; +using rip::arduinogen::get_mmap_values_at_index; namespace rip { @@ -24,10 +24,10 @@ namespace rip { namespace test { - TEST(Constructors, NoTypeNoVariableConstructor) + TEST(Constructors, extra_attribute) { tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/no_type_no_variable_constructors.xml", { }), tinyxml2::XML_SUCCESS); + ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/extra_attribute.xml", { }), tinyxml2::XML_SUCCESS); tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); ASSERT_NE(xml, nullptr); @@ -36,53 +36,22 @@ namespace rip ASSERT_THROW(constructors = std::unique_ptr(new Constructors(xml)), AttributeException); } - TEST(Constructors, NoTypeConstructor) + TEST(Constructors, extra_element) { tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/no_type_constructors.xml", { }), tinyxml2::XML_SUCCESS); + ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/extra_element.xml", { }), tinyxml2::XML_SUCCESS); tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); ASSERT_NE(xml, nullptr); std::unique_ptr constructors; - ASSERT_THROW(constructors = std::unique_ptr(new Constructors(xml)), AttributeException); - } - - TEST(Constructors, NoVariableConstructor) - { - tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/no_variable_constructors.xml", { }), tinyxml2::XML_SUCCESS); - - tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); - ASSERT_NE(xml, nullptr); - - std::unique_ptr constructors; - ASSERT_THROW(constructors = std::unique_ptr(new Constructors(xml)), AttributeException); + ASSERT_THROW(constructors = std::unique_ptr(new Constructors(xml)), ElementException); } - TEST(Constructors, EmptyConstructor) + TEST(Constructors, zero_constructors) { tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/empty_constructors.xml", { }), tinyxml2::XML_SUCCESS); - - tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); - ASSERT_NE(xml, nullptr); - - std::unique_ptr constructors; - RIP_ASSERT_NO_THROW(constructors = std::unique_ptr(new Constructors(xml))); - - std::vector> appendages; - - ASSERT_EQ(constructors->toString(appendages), - "Type type [0] = {\n" - "};\n" - ); - } - - TEST(Constructors, IntArgumentConstructor) - { - tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/int_argument_constructors.xml", { }), tinyxml2::XML_SUCCESS); + ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/zero_constructors.xml", { }), tinyxml2::XML_SUCCESS); tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); ASSERT_NE(xml, nullptr); @@ -92,75 +61,22 @@ namespace rip nlohmann::json j; j["something"] = 1; + j["somethingElse"] = false; std::multimap> appendage_map; appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - std::vector> appendages = mmap_to_vector(appendage_map, "A Type"); - - ASSERT_EQ(constructors->toString(appendages), - "Type type [1] = {\n" - "\tType(1)\n" - "};\n" - ); - } - - TEST(Constructors, FloatArgumentConstructor) - { - tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/float_argument_constructors.xml", { }), tinyxml2::XML_SUCCESS); - - tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); - ASSERT_NE(xml, nullptr); - - std::unique_ptr constructors; - RIP_ASSERT_NO_THROW(constructors = std::unique_ptr(new Constructors(xml))); - - nlohmann::json j; - j["something"] = 3.0f; - - std::multimap< std::string, std::shared_ptr > appendage_map; - appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - - std::vector> appendages = mmap_to_vector(appendage_map, "A Type"); - - ASSERT_EQ(constructors->toString(appendages), - "Type type [1] = {\n" - "\tType(3.000000)\n" - "};\n" - ); - } - - TEST(Constructors, BoolArgumentConstructor) - { - tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/bool_argument_constructors.xml", { }), tinyxml2::XML_SUCCESS); - - tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); - ASSERT_NE(xml, nullptr); - - std::unique_ptr constructors; - RIP_ASSERT_NO_THROW(constructors = std::unique_ptr(new Constructors(xml))); - - nlohmann::json j; - j["something"] = true; - - std::multimap< std::string, std::shared_ptr > appendage_map; - appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - - std::vector> appendages = mmap_to_vector(appendage_map, "A Type"); + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); ASSERT_EQ(constructors->toString(appendages), - "Type type [1] = {\n" - "\tType(true)\n" - "};\n" + "" ); } - TEST(Constructors, StringArgumentConstructor) + TEST(Constructors, one_constructor) { tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/string_argument_constructors.xml", { }), tinyxml2::XML_SUCCESS); + ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/one_constructor.xml", { }), tinyxml2::XML_SUCCESS); tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); ASSERT_NE(xml, nullptr); @@ -169,26 +85,25 @@ namespace rip RIP_ASSERT_NO_THROW(constructors = std::unique_ptr(new Constructors(xml))); nlohmann::json j; - j["something"] = "asdf"; + j["something"] = 1; + j["somethingElse"] = false; - std::multimap< std::string, std::shared_ptr > appendage_map; + std::multimap> appendage_map; appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - std::vector> appendages = mmap_to_vector(appendage_map, "A Type"); - - ASSERT_EQ(appendages.size(), 1); + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); ASSERT_EQ(constructors->toString(appendages), "Type type [1] = {\n" - "\tType(\"asdf\")\n" + "\tType(1)\n" "};\n" ); } - TEST(Constructors, SingleAppendageMultipleArgumentConstructor) + TEST(Constructors, two_constructors) { tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/multiple_argument_constructors.xml", { }), tinyxml2::XML_SUCCESS); + ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/two_constructors.xml", { }), tinyxml2::XML_SUCCESS); tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); ASSERT_NE(xml, nullptr); @@ -197,83 +112,21 @@ namespace rip RIP_ASSERT_NO_THROW(constructors = std::unique_ptr(new Constructors(xml))); nlohmann::json j; - j["something_int"] = 1; - j["something_float"] = 16.0f; - j["something_string"] = "four"; - j["something_bool"] = true; + j["something"] = 1; + j["somethingElse"] = false; - std::multimap< std::string, std::shared_ptr > appendage_map; + std::multimap> appendage_map; appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - appendage_map.emplace(std::make_pair("Other Type", std::make_shared(j, appendage_map, "", false))); - appendage_map.emplace(std::make_pair("abc Type", std::make_shared(j, appendage_map, "", false))); - appendage_map.emplace(std::make_pair("one more Type", std::make_shared(j, appendage_map, "", false))); - - std::vector> appendages = mmap_to_vector(appendage_map, "A Type"); - - ASSERT_EQ(appendages.size(), 1); + std::vector> appendages = get_mmap_values_at_index(appendage_map, "A Type"); ASSERT_EQ(constructors->toString(appendages), - "Type type [1] = {\n" - "\tType(1, true, 16.000000, \"four\")\n" + "IntType intType [1] = {\n" + "\tIntType(1)\n" "};\n" - ); - } - - TEST(Constructors, MultipleAppendageMultipleArgumentConstructor) - { - tinyxml2::XMLDocument doc; - ASSERT_EQ(loadXmlFile(doc, "test/data/constructors/multiple_argument_constructors.xml", { }), tinyxml2::XML_SUCCESS); - - tinyxml2::XMLElement* xml = doc.FirstChildElement("constructors"); - ASSERT_NE(xml, nullptr); - - std::unique_ptr constructors; - RIP_ASSERT_NO_THROW(constructors = std::unique_ptr(new Constructors(xml))); - - nlohmann::json j; - j["something_int"] = 1; - j["something_float"] = 16.0f; - j["something_string"] = "four"; - j["something_bool"] = true; - - std::multimap< std::string, std::shared_ptr > appendage_map; - appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - - j["something_int"] = 2; - j["something_float"] = 1290.0f; - j["something_string"] = "three"; - j["something_bool"] = false; - - appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - - j["something_int"] = 3; - j["something_float"] = 2.5f; - j["something_string"] = "two"; - j["something_bool"] = true; - - appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - - j["something_int"] = 4; - j["something_float"] = 1.125f; - j["something_string"] = "one"; - j["something_bool"] = false; - appendage_map.emplace(std::make_pair("A Type", std::make_shared(j, appendage_map, "", false))); - - appendage_map.emplace(std::make_pair("Other Type", std::make_shared(j, appendage_map, "", false))); - appendage_map.emplace(std::make_pair("abc Type", std::make_shared(j, appendage_map, "", false))); - appendage_map.emplace(std::make_pair("one more Type", std::make_shared(j, appendage_map, "", false))); - - std::vector> appendages = mmap_to_vector(appendage_map, "A Type"); - - ASSERT_EQ(appendages.size(), 4); - - ASSERT_EQ(constructors->toString(appendages), - "Type type [4] = {\n" - "\tType(1, true, 16.000000, \"four\"),\n" - "\tType(2, false, 1290.000000, \"three\"),\n" - "\tType(3, true, 2.500000, \"two\"),\n" - "\tType(4, false, 1.125000, \"one\")\n" + "\n" + "BoolType boolType [1] = {\n" + "\tBoolType(false)\n" "};\n" ); } diff --git a/arduino_gen/test/test_includes.cpp b/arduino_gen/test/test_includes.cpp index dcd889d..36c0361 100644 --- a/arduino_gen/test/test_includes.cpp +++ b/arduino_gen/test/test_includes.cpp @@ -1,6 +1,6 @@ -#include "includes.hpp" -#include "exceptions.hpp" -#include "xml_utils.hpp" +#include "arduino_gen/includes.hpp" +#include "arduino_gen/exceptions.hpp" +#include "arduino_gen/xml_utils.hpp" #include #include @@ -32,7 +32,7 @@ namespace rip Includes includes(includesElement); std::vector includes_vec = includes.GetIncludes(); - EXPECT_EQ(includes_vec.size(), 0); + EXPECT_EQ(includes_vec.size(), 0u); } TEST(Includes, SingleInclude) diff --git a/arduino_gen/test/test_loop.cpp b/arduino_gen/test/test_loop.cpp index 3d11669..53c20e6 100644 --- a/arduino_gen/test/test_loop.cpp +++ b/arduino_gen/test/test_loop.cpp @@ -1,7 +1,7 @@ -#include "loop.hpp" -#include "exceptions.hpp" -#include "xml_utils.hpp" -#include "appendage.hpp" +#include "arduino_gen/loop.hpp" +#include "arduino_gen/exceptions.hpp" +#include "arduino_gen/xml_utils.hpp" +#include "arduino_gen/appendage.hpp" #include #include diff --git a/arduino_gen/test/test_parameter.cpp b/arduino_gen/test/test_parameter.cpp index a7440d5..ecabca8 100644 --- a/arduino_gen/test/test_parameter.cpp +++ b/arduino_gen/test/test_parameter.cpp @@ -1,5 +1,5 @@ -#include "parameter.hpp" -#include "exceptions.hpp" +#include "arduino_gen/parameter.hpp" +#include "arduino_gen/exceptions.hpp" #include #include diff --git a/arduino_gen/test/test_return_value.cpp b/arduino_gen/test/test_return_value.cpp index 2449b3c..35dd6b8 100644 --- a/arduino_gen/test/test_return_value.cpp +++ b/arduino_gen/test/test_return_value.cpp @@ -1,5 +1,5 @@ -#include "return_value.hpp" -#include "exceptions.hpp" +#include "arduino_gen/return_value.hpp" +#include "arduino_gen/exceptions.hpp" #include #include @@ -171,7 +171,7 @@ namespace rip ASSERT_EQ(retval->getName(), "var"); ASSERT_EQ(retval->declare(), "\tint var;\n"); - ASSERT_EQ(retval->send(), "\tcmdMessenger.sendBinArg(var);\n"); + ASSERT_EQ(retval->send(), "\tcmdMessenger.sendCmdBinArg(var);\n"); } TEST(ReturnValue_declare_send, char_thing) @@ -189,7 +189,7 @@ namespace rip ASSERT_EQ(retval->getName(), "thing"); ASSERT_EQ(retval->declare(), "\tchar thing;\n"); - ASSERT_EQ(retval->send(), "\tcmdMessenger.sendBinArg(thing);\n"); + ASSERT_EQ(retval->send(), "\tcmdMessenger.sendCmdBinArg(thing);\n"); } } } diff --git a/arduino_gen/test/test_setup.cpp b/arduino_gen/test/test_setup.cpp index 60574a0..1328895 100644 --- a/arduino_gen/test/test_setup.cpp +++ b/arduino_gen/test/test_setup.cpp @@ -1,7 +1,7 @@ -#include "setup.hpp" -#include "exceptions.hpp" -#include "xml_utils.hpp" -#include "appendage.hpp" +#include "arduino_gen/setup.hpp" +#include "arduino_gen/exceptions.hpp" +#include "arduino_gen/xml_utils.hpp" +#include "arduino_gen/appendage.hpp" #include #include diff --git a/build-linux.sh b/build-linux.sh new file mode 100755 index 0000000..f5001d7 --- /dev/null +++ b/build-linux.sh @@ -0,0 +1,33 @@ +#!/bin/bash +pushd "$(dirname "$0")" + +mkdir -p build +pushd build + +cleanup() { + popd;popd; + exit $SIGNAL; +} + +trap 'SIGNAL=$?;cleanup' ERR +trap 'cleanup' SIGINT + +cmake .. \ + -DENABLE_TESTING=on \ + $@ +# + +nprocs="$(nproc --ignore=1)" + +trap '' ERR +make_retval=1 +make -j$nprocs +make_retval=$? +trap 'SIGNAL=$?;cleanup' ERR + +if [ $make_retval -eq 0 ]; then + echo "Make completed." +else + echo -e "\033[0;31m !!! Multithreaded make has failed, building single-threaded for error isolation. \033[0m" + make +fi diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 1f9ae1e..97ff108 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -1,14 +1,8 @@ cmake_minimum_required(VERSION 3.1) message("Building core") -project(core) add_subdirectory(utilities) -add_subdirectory(framework) - -if(GIT_BRANCH MATCHES "navigation/.*" OR GIT_BRANCH MATCHES "navx/.*" OR GIT_BRANCH MATCHES "path_planner/.*") - add_subdirectory(navigation) -else() - add_subdirectory(communication) - add_subdirectory(navigation) -endif() +add_subdirectory(framework) +add_subdirectory(motor_controllers) +add_subdirectory(navigation) diff --git a/core/communication/CMakeLists.txt b/core/communication/CMakeLists.txt deleted file mode 100644 index 87f00dd..0000000 --- a/core/communication/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 3.1) -project(communication) - -# utils built by top level cmake -# appendages built by top level cmake - -# Get all .cpp files except for main.cpp -file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES "src/*.cpp") -file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS "src/*.cpp") - -# Create the core library for RIP -add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) -set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) -if(ENABLE_TESTING) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs - -ftest-coveragcoverage") -endif() -target_link_libraries(${PROJECT_NAME} fmt json spdlog cppfs appendages cmd_messenger) -target_include_directories(${PROJECT_NAME} PUBLIC include) - -if(ENABLE_TESTING) - # Get test source files and test data files - file(GLOB_RECURSE ${PROJECT_NAME}_TEST_SOURCES "test/*.cpp") - file(GLOB_RECURSE ${PROJECT_NAME}_TEST_DATA "test/data/*") - - # Copy test data to outdir/test - set(copiedTestData "") - foreach(TestDataFile ${${PROJECT_NAME}_TEST_DATA}) - file(RELATIVE_PATH name ${CMAKE_CURRENT_SOURCE_DIR} ${TestDataFile}) - add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${name} - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${TestDataFile} ${CMAKE_CURRENT_BINARY_DIR}/${name} - DEPENDS ${TestDataFile} - COMMENT "Copying ${name}" - VERBATIM - ) - list(APPEND copiedTestData ${CMAKE_CURRENT_BINARY_DIR}/${name}) - endforeach() - - # Create arduino_gen_test executable, set to c++11, link arduino_gen library and google test libraries - add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES} ${copiedTestData}) - set_property(TARGET ${PROJECT_NAME}_test PROPERTY CXX_STANDARD 11) - target_link_libraries(${PROJECT_NAME}_test _${PROJECT_NAME}) - target_link_libraries(${PROJECT_NAME}_test gmock) - target_link_libraries(${PROJECT_NAME}_test gtest) - target_link_libraries(${PROJECT_NAME}_test googletest_rip_macros) - - - # In CMakeModules I include code coverage for c++ - set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules) - - if(CMAKE_COMPILER_IS_GNUCXX) - include(CodeCoverage) - setup_target_for_coverage(${PROJECT_NAME}_coverage ${PROJECT_NAME}_test coverage) - endif() -endif() diff --git a/core/communication/include/spine.hpp b/core/communication/include/spine.hpp deleted file mode 100644 index 16a2ff8..0000000 --- a/core/communication/include/spine.hpp +++ /dev/null @@ -1,94 +0,0 @@ -#ifndef SPINE_HPP -#define SPINE_HPP - -#include -#include -#include - -#include -#include - -#include "cmd_messenger.hpp" -#include "appendage.hpp" - -namespace rip -{ - namespace core - { - /** - * @class Spine - * @brief Handles creaing all the appendages from the config files and setting up communication - */ - class Spine - { - public: - /** - * @brief Returns the singleton - * - * @returns The singleton - */ - static std::shared_ptr getInstance(); - - /** - * @brief Attempts to connect to all the devices specified. Defaults to attempting all - * if none are provided. - * - * @param device_names A list of devices to attempt to connect to - * - * @exception DeviceNotFound Thrown if a name is specified and the device is not found - * in dev - * @exception DeviceFolderNotFound Thrown if a name is specified and a folder with the - * configuration cannot be found - */ - void loadDevices(std::vector device_names = {}); - - /** - * @brief Returns the specified appendage - * - * @tparam T The Appendage class type - * - * @param appendage_name The name of the appendage to return - * - * @returns The specified appendage - */ - template - std::shared_ptr get(std::string appendage_name) - { - if (m_appendages.find(appendage_name) == m_appendages.end()) - { - throw AppendageNotFound(fmt::format("Cannot find appendage named {}", appendage_name)); - } - - return std::dynamic_pointer_cast(m_appendages[appendage_name]); - } - - private: - /** - * @brief Loads the config file at a specific path - * - * @param path The location of the config file - * - * @exception FileNotFound Thrown if the config file cannot be found - * @exception IncorrectConfig Thrown if the config has incorrect information in it - */ - void loadConfig(std::string path); - - /** - * @brief Checks if possible to load a device - * - * @param device_name The name of the device - * - * @returns Whether the device can be found in dev and if it has a folder with a config file in it - */ - bool canLoadDevice(std::string device_name); - - static std::shared_ptr m_singleton; - - std::map< std::string, std::shared_ptr > m_devices; - std::map< std::string, std::shared_ptr > m_appendages; - - }; // class Spine - } -} // namespace rip - -#endif // SPINE_HPP diff --git a/core/framework/CMakeLists.txt b/core/framework/CMakeLists.txt index 41fac8c..6a3931d 100644 --- a/core/framework/CMakeLists.txt +++ b/core/framework/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.1) -project(core) +project(framework) # utils built by top level cmake # appendages built by top level cmake @@ -13,49 +13,26 @@ file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS "include/*.hpp") add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) if(ENABLE_TESTING) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs - -ftest-coveragcoverage") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") endif() target_link_libraries(${PROJECT_NAME} fmt json spdlog cppfs units cmd_messenger appendages) target_include_directories(${PROJECT_NAME} PUBLIC include) - - if(ENABLE_TESTING) # Get test source files and test data files file(GLOB_RECURSE ${PROJECT_NAME}_TEST_SOURCES "test/*.cpp") file(GLOB_RECURSE ${PROJECT_NAME}_TEST_DATA "test/data/*") + include(FileOutputs) + # Copy test data to outdir/test - set(copiedTestData "") - foreach(TestDataFile ${${PROJECT_NAME}_TEST_DATA}) - file(RELATIVE_PATH name ${CMAKE_CURRENT_SOURCE_DIR} ${TestDataFile}) - add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${name} - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${TestDataFile} ${CMAKE_CURRENT_BINARY_DIR}/${name} - DEPENDS ${TestDataFile} - COMMENT "Copying ${name}" - VERBATIM - ) - list(APPEND copiedTestData ${CMAKE_CURRENT_BINARY_DIR}/${name}) - endforeach() + make_outputs(${CMAKE_CURRENT_SOURCE_DIR} "${${PROJECT_NAME}_TEST_DATA}" ${CMAKE_CURRENT_BINARY_DIR} testDataOutputs) # Create arduino_gen_test executable, set to c++11, link arduino_gen library and google test libraries - add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES} ${copiedTestData}) + add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES} ${testDataOutputs}) set_property(TARGET ${PROJECT_NAME}_test PROPERTY CXX_STANDARD 11) - target_link_libraries(${PROJECT_NAME}_test _${PROJECT_NAME}) - target_link_libraries(${PROJECT_NAME}_test gmock) - target_link_libraries(${PROJECT_NAME}_test gtest) - target_link_libraries(${PROJECT_NAME}_test googletest_rip_macros) - - - # In CMakeModules I include code coverage for c++ - set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules) - - if(CMAKE_COMPILER_IS_GNUCXX) - include(CodeCoverage) - setup_target_for_coverage(${PROJECT_NAME}_coverage ${PROJECT_NAME}_test coverage) - endif() + target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) + target_link_libraries(${PROJECT_NAME}_test gmock gtest googletest_rip_macros) endif() diff --git a/core/framework/README.md b/core/framework/README.md index ef587bf..f9e7ddc 100644 --- a/core/framework/README.md +++ b/core/framework/README.md @@ -1 +1,6 @@ -# communication_cpp \ No newline at end of file +# Core Framework + +The core framework of RIP includes: + - Action Scheduling System + - Robot Subsystems + - RIP Spine diff --git a/core/framework/include/action.hpp b/core/framework/include/action.hpp deleted file mode 100644 index 7db1566..0000000 --- a/core/framework/include/action.hpp +++ /dev/null @@ -1,65 +0,0 @@ -/* - * The RIP License (Revision 0.3): - * This software is available without warranty and without support. - * Use at your own risk. Literally. It might delete your filesystem or - * eat your cat. As long as you retain this notice, you can do whatever - * you want with this. If we meet some day, you owe me a beer. - * - * Go Vols! - * - * __ __ ________ __ __ _______ ______ _______ - * | \ | \| \| \ / \ | \ | \| \ - * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ - * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ - * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ - * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ - * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ - * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ - * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ - */ -#ifndef ACTION_HPP -#define ACTION_HPP - -#include - -namespace rip -{ - /** - * Abstract Action Base class - */ - class Action - { - public: - /** - * Returns whether or not the action has finished execution. - */ - virtual bool isFinished() = 0; - - /** - * Iteratively called until {@see Action#isFinished()} returns true - */ - virtual void update() = 0; - - /** - * Run once before the main code - */ - virtual void setup() = 0; - - /** - * Run once after finished - */ - virtual void teardown() = 0; - - /** - * Returns the state which can be used in case of a crash - */ - virtual nlohmann::json save() const = 0; - - /** - * Restores the action to state - */ - virtual void restore(const nlohmann::json& state) = 0; - }; -} - -#endif // ACTION_HPP diff --git a/core/framework/include/framework/action.hpp b/core/framework/include/framework/action.hpp new file mode 100644 index 0000000..d41b6a3 --- /dev/null +++ b/core/framework/include/framework/action.hpp @@ -0,0 +1,74 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ +#ifndef ACTION_HPP +#define ACTION_HPP + +#include + +namespace rip +{ + namespace framework + { + /** + * Abstract Action Base class + */ + class Action + { + public: + Action(const std::string& name) + : m_name(name) + {} + + std::string name() const + { + return m_name; + } + + void setName(const std::string& name) + { + m_name = name; + } + + /** + * Returns whether or not the action has finished execution. + */ + virtual bool isFinished() = 0; + + /** + * Iteratively called until {@see Action#isFinished()} returns true + */ + virtual void update(nlohmann::json& state) = 0; + + /** + * Run once before the main code + */ + virtual void setup(nlohmann::json& state) = 0; + + /** + * Run once after finished + */ + virtual void teardown(nlohmann::json& state) = 0; + protected: + std::string m_name; + }; + } +} + +#endif // ACTION_HPP diff --git a/core/framework/include/core_exceptions.hpp b/core/framework/include/framework/exceptions.hpp similarity index 83% rename from core/framework/include/core_exceptions.hpp rename to core/framework/include/framework/exceptions.hpp index e3d7161..209cddc 100644 --- a/core/framework/include/core_exceptions.hpp +++ b/core/framework/include/framework/exceptions.hpp @@ -19,13 +19,16 @@ */ #ifndef EXCEPTIONS_HPP #define EXCEPTIONS_HPP -#include +#include namespace rip { - NEW_EX(AppendageNotFound) - NEW_EX(FileNotFound) - NEW_EX(AppendageWithoutType) - NEW_EX(CannotLoadDevice) + namespace framework + { + NEW_EX(AppendageNotFound) + NEW_EX(FileNotFound) + NEW_EX(AppendageWithoutType) + NEW_EX(CannotLoadDevice) + } } #endif // EXCEPTIONS_HPP diff --git a/core/roboclaw/include/exceptions.hpp b/core/framework/include/framework/instant_action.hpp similarity index 52% rename from core/roboclaw/include/exceptions.hpp rename to core/framework/include/framework/instant_action.hpp index bf28510..110d51e 100644 --- a/core/roboclaw/include/exceptions.hpp +++ b/core/framework/include/framework/instant_action.hpp @@ -17,40 +17,52 @@ * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ */ -#ifndef ROBOCLAW_EXCEPTIONS_HPP -#define ROBOCLAW_EXCEPTIONS_HPP -#include +#ifndef RUN_ONCE_ACTION_HPP +#define RUN_ONCE_ACTION_HPP + +#include "action.hpp" namespace rip { - namespace utilities + namespace framework { - namespace roboclaw + /** + * Base action for something that only needs to be done + * once + */ + class InstantAction : public Action { + public: + InstantAction(const std::string& name); + + /** + * Returns whether the action is finished + * + * @note Is always true + */ + virtual bool isFinished() override; + /** - * @class CommandFailure - * @brief An exception for when the command fails + * Action has already happened, so nothing */ - NEW_EX(CommandFailure); + virtual void update(nlohmann::json& state) override; /** - * @class ReadFailure - * @brief An exception for when the response for a read command fails + * Where the action actually does stuff */ - NEW_EX(ReadFailure); + virtual void setup(nlohmann::json& state) override; /** - * @class OutOfRange - * @brief An exception for when something falls out of range + * Never happens */ - NEW_EX(OutOfRange); + virtual void teardown(nlohmann::json& state) override; /** - * @class UnknownDType - * @brief Unknown combination of motor dynamic parameters + * This is where the magic happens for this action */ - NEW_EX(UnknownDType); - } + virtual void runOnce() = 0; + }; } } -#endif //ROBOCLAW_EXCEPTIONS_HPP + +#endif // RUN_ONCE_ACTION_HPP diff --git a/core/framework/include/framework/parallel_action.hpp b/core/framework/include/framework/parallel_action.hpp new file mode 100644 index 0000000..43f61e8 --- /dev/null +++ b/core/framework/include/framework/parallel_action.hpp @@ -0,0 +1,74 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ +#ifndef PARALLEL_ACTION_HPP +#define PARALLEL_ACTION_HPP + +#include +#include + +#include "action.hpp" + +namespace rip +{ + namespace framework + { + /** + * Composite action + */ + class ParallelAction : public Action + { + public: + /** + * Constructor + * + * @param name Name of the action + * @param actions A list of actions to be completed at the same time + */ + ParallelAction(const std::string& name, const std::vector< std::shared_ptr >& actions); + + /** + * Returns whether or not this action is finished + * + * @note Finishes when ALL internal actions are finished + */ + virtual bool isFinished() override; + + /** + * Updates all the internal actions + */ + virtual void update(nlohmann::json& state) override; + + /** + * Sets up all the internal actions + */ + virtual void setup(nlohmann::json& state) override; + + /** + * Tears down all the internal actions + */ + virtual void teardown(nlohmann::json& state) override; + + private: + std::vector< std::shared_ptr > m_actions; + }; + } +} + +#endif // PARALLEL_ACTION_HPP diff --git a/core/framework/include/framework/robot_base.hpp b/core/framework/include/framework/robot_base.hpp new file mode 100644 index 0000000..b8c8d0c --- /dev/null +++ b/core/framework/include/framework/robot_base.hpp @@ -0,0 +1,117 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ +#ifndef ROBOT_BASE_HPP +#define ROBOT_BASE_HPP + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include "action.hpp" +#include "spine.hpp" +#include "subsystem.hpp" + +namespace rip +{ + namespace diag + { + class Diag; + } + + namespace framework + { + void handleSignal(int signum); + + /** + * Main robot base class + */ + class RobotBase + { + public: + /** + * Constructor + * + * @param config_path The path to the config file for this robot + */ + RobotBase(const std::string& config_path); + + /** + * Destructor + */ + virtual ~RobotBase(); + + /** + * Initalize robot + */ + void init(); + + /** + * Builds the routine to run for the robot + */ + virtual void createRoutine() = 0; + + /** + * Create the subsystem controllers for the robot + */ + virtual void createSubsystems(const nlohmann::json& config) = 0; + + /** + * Start the routine + */ + void start(); + + /** + * Stop the routine + */ + void stop(); + + protected: + /** + * Main loop for the routine + */ + void run(); + + bool m_running; + units::Time m_update_time; + std::unique_ptr m_thread; + std::unique_ptr m_state_file; + + std::vector< std::shared_ptr > m_routine; + + std::unique_ptr m_spine; + std::map > m_subsystems; + + std::string m_config_path; + + friend class Diag; + }; + } +} + +#endif // ROBOT_BASE_HPP diff --git a/core/framework/include/framework/series_action.hpp b/core/framework/include/framework/series_action.hpp new file mode 100644 index 0000000..1304d03 --- /dev/null +++ b/core/framework/include/framework/series_action.hpp @@ -0,0 +1,76 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ +#ifndef SERIES_ACTION +#define SERIES_ACTION + +#include +#include + +#include "action.hpp" + +namespace rip +{ + namespace framework + { + /** + * Executes one action at a time. + * + * @note Useful as a member of {@link ParallelAction} + */ + class SeriesAction : public Action + { + public: + /** + * Constructor + * + * @param name Name of the action + * @param action A list of actions to complete in sequential order + */ + SeriesAction(const std::string& name, const std::vector< std::shared_ptr >& actions); + + /** + * Returns whether this action is finished + */ + virtual bool isFinished() override; + + /** + * Sets up the action + */ + virtual void setup(nlohmann::json& state) override; + + /** + * Updates the action + */ + virtual void update(nlohmann::json& state) override; + + /** + * Tears down the action + */ + virtual void teardown(nlohmann::json& state) override; + + private: + std::vector< std::shared_ptr > m_actions; + int m_current; + int m_previous; + }; + } +} + +#endif // SERIES_ACTION diff --git a/core/framework/include/spine.hpp b/core/framework/include/framework/spine.hpp similarity index 96% rename from core/framework/include/spine.hpp rename to core/framework/include/framework/spine.hpp index ead8a05..1f2c3df 100644 --- a/core/framework/include/spine.hpp +++ b/core/framework/include/framework/spine.hpp @@ -26,13 +26,16 @@ #include -#include -#include +#include +#include -#include "core_exceptions.hpp" +#include "framework/exceptions.hpp" namespace rip { + + namespace framework + { /** * @class Spine * @brief Handles creating all the appendages from the config files and setting up communication @@ -42,6 +45,8 @@ namespace rip public: Spine() = default; + ~Spine(); + /** * @brief Attempts to connect to all the devices specified. Defaults to attempting all * if none are provided. @@ -109,6 +114,7 @@ namespace rip std::map< std::string, std::shared_ptr > m_appendages; }; // class Spine + } } // namespace rip #endif // SPINE_HPP diff --git a/core/utilities/misc/include/exception_base.hpp b/core/framework/include/framework/subsystem.hpp similarity index 55% rename from core/utilities/misc/include/exception_base.hpp rename to core/framework/include/framework/subsystem.hpp index b1838c8..9a01a15 100644 --- a/core/utilities/misc/include/exception_base.hpp +++ b/core/framework/include/framework/subsystem.hpp @@ -17,33 +17,46 @@ * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ */ -#ifndef EXCEPTION_BASE_HPP -#define EXCEPTION_BASE_HPP -#include +#ifndef SUBSYSTEM_HPP +#define SUBSYSTEM_HPP -#define NEW_EX(name) \ -class name : public rip::utilities::ExceptionBase \ -{ \ -public: \ - name(std::string message = "") : rip::utilities::ExceptionBase(#name ": " + message) {} \ -}; +#include +#include +#include namespace rip { - namespace utilities + namespace framework { - class ExceptionBase : public std::exception + class Subsystem { - protected: - std::string m_message; - public: - ExceptionBase(std::string message = "") : m_message(message) {} - const char* what() const throw() - { - return m_message.c_str(); - } + Subsystem(const std::string& name); + + virtual ~Subsystem(); + + void setName(const std::string& name); + + std::string name() const; + + virtual bool diagnostic() = 0; + virtual void stop() = 0; + + /* + void setDiagOut(std::ostream output) + { + m_diag_output = output; + } + + void setDiagIn(std::istream input) + { + m_diag_input = input; + } + */ + private: + std::string m_name; }; } } -#endif // EXCEPTION_BASE_HPP + +#endif // SUBSYSTEM_HPP diff --git a/core/framework/include/framework/wait_action.hpp b/core/framework/include/framework/wait_action.hpp new file mode 100644 index 0000000..8155cc0 --- /dev/null +++ b/core/framework/include/framework/wait_action.hpp @@ -0,0 +1,75 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ +#ifndef WAIT_ACTION_HPP +#define WAIT_ACTION_HPP + +#include +#include + +#include + +#include "action.hpp" + +namespace rip +{ + namespace framework + { + /** + * Action to wait for a given amount of time + */ + class WaitAction : public Action + { + public: + /** + * Constructor + * + * @param name Name of the action + * @param wait_time The amount of time for this action to wait + */ + WaitAction(const std::string& name, const units::Time& wait_time); + + /** + * Finishes when the elapsed time is more than the wait time + */ + virtual bool isFinished() override; + + /** + * [setup description] + */ + virtual void setup(nlohmann::json& state) override; + + /** + * [update description] + */ + virtual void update(nlohmann::json& state) override; + + /** + * [teardown description] + */ + virtual void teardown(nlohmann::json& state) override; + + private: + units::Time m_wait_time; + std::chrono::time_point m_start_time; + }; + } +} + +#endif // WAIT_ACTION_HPP diff --git a/core/framework/include/parallel_action.hpp b/core/framework/include/parallel_action.hpp deleted file mode 100644 index b89705a..0000000 --- a/core/framework/include/parallel_action.hpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * The RIP License (Revision 0.3): - * This software is available without warranty and without support. - * Use at your own risk. Literally. It might delete your filesystem or - * eat your cat. As long as you retain this notice, you can do whatever - * you want with this. If we meet some day, you owe me a beer. - * - * Go Vols! - * - * __ __ ________ __ __ _______ ______ _______ - * | \ | \| \| \ / \ | \ | \| \ - * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ - * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ - * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ - * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ - * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ - * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ - * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ - */ -#ifndef PARALLEL_ACTION_HPP -#define PARALLEL_ACTION_HPP - -#include -#include - -#include "action.hpp" - -namespace rip -{ - /** - * Composite action - */ - class ParallelAction : public Action - { - public: - /** - * Constructor - * - * @param actions A list of actions to be completed at the same time - */ - ParallelAction(const std::vector< std::shared_ptr >& actions); - - /** - * Returns whether or not this action is finished - * - * @note Finishes when ALL internal actions are finished - */ - virtual bool isFinished() override; - - /** - * Updates all the internal actions - */ - virtual void update() override; - - /** - * Sets up all the internal actions - */ - virtual void setup() override; - - /** - * Tears down all the internal actions - */ - virtual void teardown() override; - - /** - * Returns the saved state of each of the internal actions - * @return [description] - */ - virtual nlohmann::json save() const override; - - /** - * Restores each of the internal actions - */ - virtual void restore(const nlohmann::json& state) override; - - private: - std::vector< std::shared_ptr > m_actions; - }; -} - -#endif // PARALLEL_ACTION_HPP diff --git a/core/framework/include/robot_base.hpp b/core/framework/include/robot_base.hpp deleted file mode 100644 index a0ddcca..0000000 --- a/core/framework/include/robot_base.hpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * The RIP License (Revision 0.3): - * This software is available without warranty and without support. - * Use at your own risk. Literally. It might delete your filesystem or - * eat your cat. As long as you retain this notice, you can do whatever - * you want with this. If we meet some day, you owe me a beer. - * - * Go Vols! - * - * __ __ ________ __ __ _______ ______ _______ - * | \ | \| \| \ / \ | \ | \| \ - * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ - * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ - * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ - * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ - * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ - * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ - * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ - */ -#ifndef ROBOT_BASE_HPP -#define ROBOT_BASE_HPP - -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include "action.hpp" -#include "spine.hpp" -#include "subsystem.hpp" - -namespace rip -{ - /** - * Main robot base class - */ - class RobotBase - { - public: - /** - * Constructor - * - * @param config_path The path to the config file for this robot - */ - RobotBase(const std::string& config_path); - - /** - * Destructor - */ - ~RobotBase(); - - /** - * Initalize robot - */ - void init(); - - /** - * Builds the routine to run for the robot - */ - virtual void createRoutine() = 0; - - /** - * Create the subsystem controllers for the robot - */ - virtual void createSubsystems(const nlohmann::json& config) = 0; - - /** - * Start the routine - */ - void start(); - - /** - * Stop the routine - */ - void stop(); - - /** - * Run the diagnostics for all of the subsystems and appendages - */ - void diagnostic(); - - protected: - /** - * Main loop for the routine - */ - void run(); - - protected: - bool m_running; - units::Time m_update_time; - std::unique_ptr m_thread; - std::unique_ptr m_state_file; - - std::vector< std::shared_ptr > m_routine; - - std::unique_ptr m_spine; - std::map > m_subsystems; - - std::string m_config_path; - }; -} - -#endif // ROBOT_BASE_HPP diff --git a/core/framework/include/series_action.hpp b/core/framework/include/series_action.hpp deleted file mode 100644 index 565bb56..0000000 --- a/core/framework/include/series_action.hpp +++ /dev/null @@ -1,82 +0,0 @@ -/* - * The RIP License (Revision 0.3): - * This software is available without warranty and without support. - * Use at your own risk. Literally. It might delete your filesystem or - * eat your cat. As long as you retain this notice, you can do whatever - * you want with this. If we meet some day, you owe me a beer. - * - * Go Vols! - * - * __ __ ________ __ __ _______ ______ _______ - * | \ | \| \| \ / \ | \ | \| \ - * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ - * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ - * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ - * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ - * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ - * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ - * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ - */ -#ifndef SERIES_ACTION -#define SERIES_ACTION - -#include -#include - -#include "action.hpp" - -namespace rip -{ - /** - * Executes one action at a time. - * - * @note Useful as a member of {@link ParallelAction} - */ - class SeriesAction : public Action - { - public: - /** - * Constructor - * - * @param action A list of actions to complete in sequential order - */ - SeriesAction(const std::vector< std::shared_ptr >& actions); - - /** - * Returns whether this action is finished - */ - virtual bool isFinished() override; - - /** - * Sets up the action - */ - virtual void setup() override; - - /** - * Updates the action - */ - virtual void update() override; - - /** - * Tears down the action - */ - virtual void teardown() override; - - /** - * Saves the state of this action - */ - virtual nlohmann::json save() const override; - - /** - * Restores the state of this action - */ - virtual void restore(const nlohmann::json& state) override; - - private: - std::vector< std::shared_ptr > m_actions; - int m_current; - int m_previous; - }; -} - -#endif // SERIES_ACTION diff --git a/core/framework/include/wait_action.hpp b/core/framework/include/wait_action.hpp deleted file mode 100644 index e1f9e03..0000000 --- a/core/framework/include/wait_action.hpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * The RIP License (Revision 0.3): - * This software is available without warranty and without support. - * Use at your own risk. Literally. It might delete your filesystem or - * eat your cat. As long as you retain this notice, you can do whatever - * you want with this. If we meet some day, you owe me a beer. - * - * Go Vols! - * - * __ __ ________ __ __ _______ ______ _______ - * | \ | \| \| \ / \ | \ | \| \ - * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ - * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ - * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ - * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ - * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ - * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ - * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ - */ -#ifndef WAIT_ACTION_HPP -#define WAIT_ACTION_HPP - -#include -#include - -#include - -#include "action.hpp" - -namespace rip -{ - /** - * Action to wait for a given amount of time - */ - class WaitAction : public Action - { - public: - /** - * Constructor - * - * @param wait_time The amount of time for this action to wait - */ - WaitAction(const units::Time& wait_time); - - /** - * Finishes when the elapsed time is more than the wait time - */ - virtual bool isFinished() override; - - /** - * [setup description] - */ - virtual void setup() override; - - /** - * [update description] - */ - virtual void update() override; - - /** - * [teardown description] - */ - virtual void teardown() override; - - /** - * Saves the elapsed time - */ - virtual nlohmann::json save() const override; - - /** - * Restores the action - */ - virtual void restore(const nlohmann::json& state) override; - - private: - units::Time m_wait_time; - std::chrono::time_point m_start_time; - }; -} - -#endif // WAIT_ACTION_HPP diff --git a/core/framework/src/instant_action.cpp b/core/framework/src/instant_action.cpp new file mode 100644 index 0000000..9c664d5 --- /dev/null +++ b/core/framework/src/instant_action.cpp @@ -0,0 +1,26 @@ +#include "framework/instant_action.hpp" + +namespace rip +{ + + namespace framework + { + InstantAction::InstantAction(const std::string& name) + : Action(name) + {} + + bool InstantAction::isFinished() + { + return true; + } + + void InstantAction::update(nlohmann::json& state) {} + + void InstantAction::setup(nlohmann::json& state) + { + runOnce(); + } + + void InstantAction::teardown(nlohmann::json& state) {} + } +} diff --git a/core/framework/src/parallel_action.cpp b/core/framework/src/parallel_action.cpp index 609cccb..b92c204 100644 --- a/core/framework/src/parallel_action.cpp +++ b/core/framework/src/parallel_action.cpp @@ -1,63 +1,48 @@ -#include "parallel_action.hpp" +#include "framework/parallel_action.hpp" namespace rip { - - ParallelAction::ParallelAction(const std::vector >& actions) - : m_actions(actions) - {} - - bool ParallelAction::isFinished() + namespace framework { - for (std::shared_ptr action : m_actions) + ParallelAction::ParallelAction(const std::string& name, const std::vector >& actions) + : Action(name) + , m_actions(actions) + {} + + bool ParallelAction::isFinished() { - if (!action->isFinished()) + for (std::shared_ptr action : m_actions) { - return false; + if (!action->isFinished()) + { + return false; + } } + return true; } - return true; - } - void ParallelAction::update() - { - for (std::shared_ptr action : m_actions) + void ParallelAction::update(nlohmann::json& state) { - action->update(); - } - } - - void ParallelAction::setup() - { - for (std::shared_ptr action : m_actions) - { - action->setup(); - } - } - - void ParallelAction::teardown() - { - for (std::shared_ptr action : m_actions) - { - action->teardown(); + for (std::shared_ptr action : m_actions) + { + action->update(state); + } } - } - nlohmann::json ParallelAction::save() const - { - nlohmann::json j; - for (std::shared_ptr action : m_actions) + void ParallelAction::setup(nlohmann::json& state) { - j.push_back(action->save()); + for (std::shared_ptr action : m_actions) + { + action->setup(state); + } } - return j; - } - void ParallelAction::restore(const nlohmann::json& state) - { - for (int i = 0, end = m_actions.size(); i < end; i++) + void ParallelAction::teardown(nlohmann::json& state) { - m_actions[i]->restore(state[i]); + for (std::shared_ptr action : m_actions) + { + action->teardown(state); + } } } diff --git a/core/framework/src/robot_base.cpp b/core/framework/src/robot_base.cpp index a3d5e92..5186cba 100644 --- a/core/framework/src/robot_base.cpp +++ b/core/framework/src/robot_base.cpp @@ -1,121 +1,152 @@ -#include "robot_base.hpp" +#include "framework/robot_base.hpp" #include #include #include #include +#include +#include +#include namespace rip { - - RobotBase::RobotBase(const std::string& config) - : m_running(false) - , m_config_path(config) - { - } - - RobotBase::~RobotBase() + namespace framework { - m_running = false; - - for (auto iter : m_subsystems) + RobotBase::RobotBase(const std::string& config) + : m_running(false) + , m_config_path(config) { - iter.second->stop(); } - m_spine->stop(); - } - - void RobotBase::init() - { - cppfs::FileHandle config_file = cppfs::fs::open(m_config_path); - if(!config_file.exists()) + RobotBase::~RobotBase() { - throw FileNotFound(); + m_running = false; } - std::unique_ptr in = config_file.createInputStream(); - nlohmann::json j; - (*in) >> j; - std::vector devices; - for(nlohmann::json d : j["devices"]) + void RobotBase::init() { - std::string device_name = d; - devices.push_back(device_name); - } + misc::Logger::getInstance()->debug("Robot is initializing..."); + cppfs::FileHandle config_file = cppfs::fs::open(m_config_path); + if (!config_file.exists()) + { + throw FileNotFound(); + } - m_spine = std::unique_ptr(new Spine); - m_spine->loadDevices(j["arduino_gen_home"], devices); + std::unique_ptr in = config_file.createInputStream(); + nlohmann::json j; + (*in) >> j; + std::vector devices; - createSubsystems(j); + if(j.find("devices") != j.end()) + { + for (nlohmann::json d : j["devices"]) + { + std::string device_name = d; + devices.push_back(device_name); + } + } + m_spine = std::unique_ptr(new Spine); - createRoutine(); - m_state_file = cppfs::fs::open(j["state_file"]).createOutputStream(); - } + if(j.find("arduino_gen_home") != j.end()) + { + m_spine->loadDevices(j["arduino_gen_home"], devices); + } - void RobotBase::start() - { - m_running = true; - m_thread = std::unique_ptr(new std::thread(&RobotBase::run, this)); - } + if(j.find("subsystems") != j.end()) + { + createSubsystems(j["subsystems"]); + } + createRoutine(); - void RobotBase::stop() - { - m_running = false; - } + if(j.find("state_file") != j.end()) + { + m_state_file = cppfs::fs::open(j["state_file"]).createOutputStream(); + } + } - void RobotBase::diagnostic() - { - if(!m_spine->diagnostic()) + void RobotBase::start() { - return; + m_running = true; + misc::Logger::getInstance()->debug("Starting the robot..."); + run(); } - for (auto iter : m_subsystems) + void RobotBase::stop() { - if (!iter.second->diagnostic()) - { - // Diagnostic - return; - } + misc::Logger::getInstance()->debug("Stoping the robot..."); + m_running = false; } - } - void RobotBase::run() - { - nlohmann::json state; - for (std::shared_ptr action : m_routine) + void RobotBase::run() { - if (!m_running) - { - break; - } - - action->setup(); - - if (!m_running) - { - break; - } + nlohmann::json state; + misc::Logger::getInstance()->debug("run"); - while (m_running && !action->isFinished()) + // Loop through the routine + for (std::shared_ptr action : m_routine) { - action->update(); - state = action->save(); - (*m_state_file) << state; - m_state_file->flush(); - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(m_update_time.to(units::ms)))); + misc::Logger::getInstance()->debug("loop"); + + // If ever interrupted then stop + if (!m_running) + { + break; + } + + // Setup the action + misc::Logger::getInstance()->debug("Setting up action: {}", action->name()); + action->setup(state); + + // Reset the state file + if(m_state_file != nullptr) + { + m_state_file->clear(); + (*m_state_file) << state; + m_state_file->flush(); + } + + // If ever interrupted then stop + if (!m_running) + { + break; + } + + // Update until the action is finished + while (m_running && !action->isFinished()) + { + action->update(state); + + // Reset the state file + if(m_state_file != nullptr) + { + m_state_file->clear(); + (*m_state_file) << state; + m_state_file->flush(); + } + + // std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(m_update_time.to(units::ms)))); + } + + // If ever interrupted then stop + if (!m_running) + { + break; + } + + // Teardown the action + misc::Logger::getInstance()->debug("Tearing down action: {}", action->name()); + action->teardown(state); + + // Reset the state file + if(m_state_file != nullptr) + { + m_state_file->clear(); + (*m_state_file) << state; + m_state_file->flush(); + } } - - if (!m_running) - { - break; - } - - action->teardown(); + m_running = false; } - m_running = false; } - } diff --git a/core/framework/src/run_once_action.cpp b/core/framework/src/run_once_action.cpp deleted file mode 100644 index 9d65a8d..0000000 --- a/core/framework/src/run_once_action.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "run_once_action.hpp" - -namespace rip -{ - - bool RunOnceAction::isFinished() - { - return true; - } - - void RunOnceAction::update() {} - - void RunOnceAction::setup() - { - runOnce(); - } - - void RunOnceAction::teardown() {} - - nlohmann::json RunOnceAction::save() const - { - return nlohmann::json(); - } - - void RunOnceAction::restore(const nlohmann::json& state) {} - -} diff --git a/core/framework/src/series_action.cpp b/core/framework/src/series_action.cpp index dbfd0a3..961cae6 100644 --- a/core/framework/src/series_action.cpp +++ b/core/framework/src/series_action.cpp @@ -1,52 +1,43 @@ -#include "series_action.hpp" +#include "framework/series_action.hpp" namespace rip { - - SeriesAction::SeriesAction(const std::vector >& actions) - : m_actions(actions) - , m_current(0) - , m_previous(-1) - {} - - bool SeriesAction::isFinished() - { - return m_current == m_actions.size(); - } - - void SeriesAction::setup() {} - - void SeriesAction::update() + namespace framework { - if (m_current != m_previous) - { - m_actions[m_current]->setup(); - m_previous = m_current; - } - m_actions[m_current]->update(); - if (m_actions[m_current]->isFinished()) + SeriesAction::SeriesAction(const std::string& name, const std::vector >& actions) + : Action(name) + , m_actions(actions) + , m_current(0) + , m_previous(-1) + {} + + bool SeriesAction::isFinished() { - m_actions[m_current]->teardown(); - m_current++; + return m_current == static_cast(m_actions.size()); } - } - void SeriesAction::teardown() {} + void SeriesAction::setup(nlohmann::json& state) {} - nlohmann::json SeriesAction::save() const - { - nlohmann::json j; - j["previous"] = m_previous; - j["current"] = m_current; - j["substate"] = m_actions[m_current]->save(); - return j; - } + void SeriesAction::update(nlohmann::json& state) + { + // If new sub-action then set it up + if (m_current != m_previous) + { + m_actions[m_current]->setup(state); + m_previous = m_current; + } + + // Update + m_actions[m_current]->update(state); + + // If action is done then tear it down + if (m_actions[m_current]->isFinished()) + { + m_actions[m_current]->teardown(state); + m_current++; + } + } - void SeriesAction::restore(const nlohmann::json& state) - { - m_previous = state["previous"]; - m_current = state["current"]; - m_actions[m_current]->restore(state["substate"]); + void SeriesAction::teardown(nlohmann::json& state) {} } - } diff --git a/core/framework/src/spine.cpp b/core/framework/src/spine.cpp index 896462f..d18c856 100644 --- a/core/framework/src/spine.cpp +++ b/core/framework/src/spine.cpp @@ -1,110 +1,116 @@ -#include "spine.hpp" +#include "framework/spine.hpp" #include #include #include -#include - -#include "core_exceptions.hpp" +#include namespace rip { - void Spine::loadDevices(const std::string& arduino_gen_folder, std::vector< std::string >& device_names) + namespace framework { - // device names have not been specified - if (device_names.empty()) + Spine::~Spine() { - cppfs::FileHandle arduino_gen_home = cppfs::fs::open(arduino_gen_folder); - for(const std::string& child: arduino_gen_home.listFiles()) + stop(); + } + + void Spine::loadDevices(const std::string& arduino_gen_folder, std::vector< std::string >& device_names) + { + // device names have not been specified + if (device_names.empty()) { - cppfs::FileHandle fh = cppfs::fs::open(child); - if(fh.isDirectory() && canLoadDevice(arduino_gen_folder, child)) + cppfs::FileHandle arduino_gen_home = cppfs::fs::open(arduino_gen_folder); + for (const std::string& child : arduino_gen_home.listFiles()) { - device_names.push_back(child); + cppfs::FileHandle fh = cppfs::fs::open(child); + if (fh.isDirectory() && canLoadDevice(arduino_gen_folder, child)) + { + device_names.push_back(child); + } } } - } - else - { - // Check if all of the specified devices can be loaded - for (const std::string& device_name : device_names) + else { - if (!canLoadDevice(arduino_gen_folder, device_name)) + // Check if all of the specified devices can be loaded + for (const std::string& device_name : device_names) { - throw CannotLoadDevice(fmt::format("Cannot load {}", device_name)); + if (!canLoadDevice(arduino_gen_folder, device_name)) + { + throw CannotLoadDevice(fmt::format("Cannot load {}", device_name)); + } } - } - } + } - // Create devices - for (const std::string& device_name : device_names) - { - m_devices[device_name] = std::make_shared(fmt::format("/dev/{}", device_name)); - loadConfig(m_devices[device_name], fmt::format("{}/{}/core.json", arduino_gen_folder, device_name)); + // Create devices + for (const std::string& device_name : device_names) + { + m_devices[device_name] = std::make_shared(fmt::format("/dev/{}", device_name)); + loadConfig(m_devices[device_name], fmt::format("{}/{}/core.json", arduino_gen_folder, device_name)); + } } - } - void Spine::stop() - { - for(auto iter : m_appendages) + void Spine::stop() { - iter.second->stop(); + for (auto iter : m_appendages) + { + iter.second->stop(); + } } - } - bool Spine::diagnostic() - { - for(auto iter : m_appendages) + bool Spine::diagnostic() { - if(!iter.second->diagnostic()) + for (auto iter : m_appendages) { - return false; + if (!iter.second->diagnostic()) + { + return false; + } } - } - - return true; - } - bool Spine::canLoadDevice(const std::string& arduino_gen_folder, const std::string& device_name) const - { - cppfs::FileHandle dev = cppfs::fs::open(fmt::format("/dev/{}", device_name)); - cppfs::FileHandle config = cppfs::fs::open(fmt::format("{}/{}/core.json", arduino_gen_folder, device_name)); - return dev.exists() && config.exists(); - } + return true; + } - void Spine::loadConfig(std::shared_ptr device, const std::string& path) - { - cppfs::FileHandle config_file = cppfs::fs::open(path); - if (!config_file.exists()) + bool Spine::canLoadDevice(const std::string& arduino_gen_folder, const std::string& device_name) const { - throw FileNotFound(fmt::format("Cannot find {}", path)); + cppfs::FileHandle dev = cppfs::fs::open(fmt::format("/dev/{}", device_name)); + cppfs::FileHandle config = cppfs::fs::open(fmt::format("{}/{}/core.json", arduino_gen_folder, device_name)); + return dev.exists() && config.exists(); } - std::unique_ptr in = config_file.createInputStream(); + void Spine::loadConfig(std::shared_ptr device, const std::string& path) + { + cppfs::FileHandle config_file = cppfs::fs::open(path); + if (!config_file.exists()) + { + throw FileNotFound(fmt::format("Cannot find {}", path)); + } - nlohmann::json config; - (*in) >> config; + std::unique_ptr in = config_file.createInputStream(); - nlohmann::json commands = config["commands"]; + nlohmann::json config; + (*in) >> config; - std::map< std::string, int> command_map; - for (nlohmann::json::iterator iter = commands.begin(); iter != commands.end(); iter++) - { - command_map[iter.key()] = iter.value(); - } + nlohmann::json commands = config["commands"]; - std::shared_ptr factory = appendages::AppendageFactory::getInstance(); - nlohmann::json appendages; - for (nlohmann::json appendage : appendages) - { - if (appendage.find("type") == appendage.end()) + std::map< std::string, int> command_map; + for (nlohmann::json::iterator iter = commands.begin(); iter != commands.end(); iter++) { - throw AppendageWithoutType(fmt::format("appendage missing type")); + command_map[iter.key()] = iter.value(); } - m_appendages[appendage["type"]] = factory->makeAppendage(appendage, command_map, device); + std::shared_ptr factory = appendages::AppendageFactory::getInstance(); + nlohmann::json appendages; + for (nlohmann::json appendage : appendages) + { + if (appendage.find("type") == appendage.end()) + { + throw AppendageWithoutType(fmt::format("appendage missing type")); + } + + m_appendages[appendage["type"]] = factory->makeAppendage(appendage, command_map, device); + } } } } // rip diff --git a/core/framework/src/subsystem.cpp b/core/framework/src/subsystem.cpp new file mode 100644 index 0000000..ef34bb1 --- /dev/null +++ b/core/framework/src/subsystem.cpp @@ -0,0 +1,27 @@ +#include "framework/subsystem.hpp" + +namespace rip +{ + namespace framework + { + + Subsystem::Subsystem(const std::string& name) + : m_name(name) + {} + + Subsystem::~Subsystem() + { + } + + void Subsystem::setName(const std::string& name) + { + m_name = name; + } + + std::string Subsystem::name() const + { + return m_name; + } + + } +} diff --git a/core/framework/src/subsystem.hpp b/core/framework/src/subsystem.hpp deleted file mode 100644 index 78dda01..0000000 --- a/core/framework/src/subsystem.hpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "subsystem.hpp" - -namespace rip -{ - -} \ No newline at end of file diff --git a/core/framework/src/wait_action.cpp b/core/framework/src/wait_action.cpp index 3caf81d..65c698b 100644 --- a/core/framework/src/wait_action.cpp +++ b/core/framework/src/wait_action.cpp @@ -1,39 +1,27 @@ -#include "wait_action.hpp" +#include "framework/wait_action.hpp" namespace rip { - - WaitAction::WaitAction(const units::Time& wait_time) - : m_wait_time(wait_time) - {} - - bool WaitAction::isFinished() - { - std::chrono::time_point now = std::chrono::system_clock::now(); - return std::chrono::duration_cast(now - m_start_time).count() >= m_wait_time.to(units::ms); - } - - void WaitAction::setup() + namespace framework { - m_start_time = std::chrono::system_clock::now(); - } + WaitAction::WaitAction(const std::string& name, const units::Time& wait_time) + : Action(name) + , m_wait_time(wait_time) + {} - void WaitAction::update() {} + bool WaitAction::isFinished() + { + std::chrono::time_point now = std::chrono::system_clock::now(); + return std::chrono::duration_cast(now - m_start_time).count() >= m_wait_time.to(units::ms); + } - void WaitAction::teardown() {} + void WaitAction::setup(nlohmann::json& state) + { + m_start_time = std::chrono::system_clock::now(); + } - nlohmann::json WaitAction::save() const - { - nlohmann::json j; - std::chrono::time_point now = std::chrono::system_clock::now(); - j["elapsed_time"] = std::chrono::duration_cast(now - m_start_time).count() * units::ms; - return j; - } + void WaitAction::update(nlohmann::json& state) {} - void WaitAction::restore(const nlohmann::json& state) - { - std::chrono::time_point now = std::chrono::system_clock::now(); - m_start_time = now - std::chrono::milliseconds(static_cast(state["elapsed_time"].get().to(units::ms))); + void WaitAction::teardown(nlohmann::json& state) {} } - } diff --git a/core/framework/test/test_main.cpp b/core/framework/test/test_main.cpp new file mode 100644 index 0000000..08fb839 --- /dev/null +++ b/core/framework/test/test_main.cpp @@ -0,0 +1,7 @@ +#include "gtest/gtest.h" + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/core/roboclaw/Astyle b/core/motor_controllers/Astyle similarity index 100% rename from core/roboclaw/Astyle rename to core/motor_controllers/Astyle diff --git a/core/motor_controllers/CMakeLists.txt b/core/motor_controllers/CMakeLists.txt new file mode 100644 index 0000000..8f8d606 --- /dev/null +++ b/core/motor_controllers/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.1) +project(motor_controllers) +message("Building Motor Controllers") + +file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/**/**.hpp) +file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cpp) +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) +target_include_directories(${PROJECT_NAME} PUBLIC include) +target_link_libraries(${PROJECT_NAME} framework periphery json units fmt misc) + + +if(ENABLE_TESTING) + file(GLOB_RECURSE ${PROJECT_NAME}_DIAG_SOURCES "diag/*.cpp") + file(GLOB_RECURSE ${PROJECT_NAME}_DIAG_HEADERS "diag/*.hpp") + add_executable(${PROJECT_NAME}_diag ${${PROJECT_NAME}_DIAG_SOURCES}) + set_property(TARGET ${PROJECT_NAME}_diag PROPERTY CXX_STANDARD 11) + target_link_libraries(${PROJECT_NAME}_diag ${PROJECT_NAME}) + target_link_libraries(${PROJECT_NAME}_diag motor_controllers periphery units) + target_include_directories(${PROJECT_NAME}_diag PUBLIC diag) + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") + + # Get test source files and test data files + file(GLOB_RECURSE ${PROJECT_NAME}_TEST_SOURCES "test/*.cpp") + file(GLOB_RECURSE ${PROJECT_NAME}_TEST_HEADERS "test/*.hpp") + file(GLOB_RECURSE ${PROJECT_NAME}_TEST_DATA "test/data/*") + + # Copy test data to outdir/test + include(FileOutputs) + make_outputs(${CMAKE_CURRENT_SOURCE_DIR} "${${PROJECT_NAME}_TEST_DATA}" ${CMAKE_CURRENT_BINARY_DIR} testDataOutputs) + + # Create cmd_messneger_test executable, set to c++11, link cmd_messenger library and google test libraries + add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES} ${testDataOutputs}) + set_property(TARGET ${PROJECT_NAME}_test PROPERTY CXX_STANDARD 11) + target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) + target_link_libraries(${PROJECT_NAME}_test gmock gtest) + target_include_directories(${PROJECT_NAME}_test PUBLIC include test) + + #if(CMAKE_COMPILER_IS_GNUCXX) + # include(CodeCoverage) + # setup_target_for_coverage(${PROJECT_NAME}_coverage ${PROJECT_NAME}_test coverage) + #endif() +endif() diff --git a/core/roboclaw/Doxyfile b/core/motor_controllers/Doxyfile similarity index 100% rename from core/roboclaw/Doxyfile rename to core/motor_controllers/Doxyfile diff --git a/core/roboclaw/LICENSE b/core/motor_controllers/LICENSE similarity index 100% rename from core/roboclaw/LICENSE rename to core/motor_controllers/LICENSE diff --git a/core/motor_controllers/README.md b/core/motor_controllers/README.md new file mode 100644 index 0000000..0b9a20e --- /dev/null +++ b/core/motor_controllers/README.md @@ -0,0 +1,3 @@ +# Roboclaw + +The [Roboclaw](http://www.ionmc.com/Roboclaw-2x7A-Motor-Controller_p_13.html) motor controller is a fully featured motor controller with open or close loop DC motor control. This module implements an interface from a SBC like the Pi 3 to the Roboclaw unit. diff --git a/core/motor_controllers/diag/diag_roboclaw.cpp b/core/motor_controllers/diag/diag_roboclaw.cpp new file mode 100644 index 0000000..7bd09cc --- /dev/null +++ b/core/motor_controllers/diag/diag_roboclaw.cpp @@ -0,0 +1,1902 @@ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#pragma GCC diagnostic ignored "-Wswitch-default" +#pragma GCC diagnostic ignored "-Wsign-compare" +#pragma GCC diagnostic ignored "-Wimplicit-fallthrough=" + +#include "diag_roboclaw.hpp" + +namespace rip +{ + namespace motorcontrollers + { + namespace roboclaw + { + namespace diag + { + + Diag::Diag(std::vector roboclaws) + { + if (roboclaws.size() > 8) + { + std::cout << std::endl << "Warning: Multi unit mode supports a maximum of 8 roboclaws per serial device" << std::endl << std::endl; + } + m_roboclaws = roboclaws; + m_roboclaws_original = roboclaws; + } +//To do: motor select, add motor back + void Diag::mainMenu() + { + int choice, n; + + do + { + std::cout << std::endl << std::endl << "Roboclaw Diag Main Menu" << std::endl; + std::cout << "0| exit" << std::endl; + std::cout << "1| readVersion" << std::endl; + std::cout << "2| readStatus" << std::endl; + std::cout << "3| voltage diagnostics" << std::endl; + std::cout << "4| current diagnostics" << std::endl; + std::cout << "5| basic (duty) driving diagnostics" << std::endl; + std::cout << "6| advanced driving/dynamics diagnostics" << std::endl; + std::cout << "7| encoder diagnostics" << std::endl; + std::cout << "8| PID diagnostics" << std::endl; + std::cout << "9| miscellaneous" << std::endl; + std::cout << "10| remove a roboclaw from the list" << std::endl; + std::cout << "11| reset list back to original (as provided in Constructor)" << std::endl; + std::cout << std::endl << std::endl << "Enter a choice" << std::endl; + std::cin.clear(); + std::cin >> choice; + switch (choice) + { + case 0: + { + std::cout << "Exiting" << std::endl; + break; + } + case 1: + { + clawVersion(); + break; + } + case 2: + { + clawStatus(); + break; + } + case 3: + { + voltageMenu(); + break; + } + case 4: + { + currentMenu(); + break; + } + case 5: + { + simpleDrive(); + break; + } + case 6: + { + driveMenu(); + break; + } + case 7: + { + encoderMenu(); + break; + } + case 8: + { + pidMenu(); + break; + } + case 9: + { + miscMenu(); + break; + } + case 10: + { + std::cout << "Enter the index of the claw to remove" << std::endl; + std::cin.clear(); + std::cin >> n; + removeClaw(n); + break; + } + case 11: + { + std::cout << "Resetting roboclaw list back to default" << std::endl; + recoverClaws(); + } + default: + { + std::cout << "invalid choice" << std::endl; + } + } + } + while (choice != 0); + } + + void Diag::voltageMenu() + { + int choice, n; + uint32_t c; + std::vector > defaultMain; + std::vector > defaultLogic; + std::array minmax; + units::Voltage v; + bool defaultsSet = 1; + + std::cout << std::endl << "NOTE: Damage can result from improper use. So don't do that" << std::endl; + std::cout << "Reading the current voltage values" << std::endl; + + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + defaultMain.push_back(m_roboclaws[i]->readMinMaxMainVoltages()); + defaultLogic.push_back(m_roboclaws[i]->readMinMaxLogicVoltages()); + } + catch (const std::exception& e) + { + std::cout << "Issue reading claw " << i << " min/max voltages, " << e.what() << std::endl; + std::cout << "Disabling revert option (option 7) " << std::endl; + defaultsSet = 0; + } + } + do + { + std::cout << std::endl << std::endl << "Roboclaw voltage Menu" << std::endl; + std::cout << "0| exit" << std::endl; + std::cout << "Getters (caution)" << std::endl; + std::cout << "1| read main battery voltage" << std::endl; + std::cout << "2| read logic battery voltage" << std::endl; + std::cout << "3| read min/max main voltage" << std::endl; + std::cout << "4| read min/max logic voltage" << std::endl; + std::cout << "Setters (caution)" << std::endl; + std::cout << "5| set main battery voltage min/max's" << std::endl; + std::cout << "6| set logic battery voltage min/max's" << std::endl; + std::cout << "7| set voltages back to initial values" << std::endl; + + std::cout << std::endl << std::endl << "Enter a choice" << std::endl; + std::cin.clear(); + std::cin >> choice; + switch (choice) + { + case 0: + { + std::cout << "Exiting" << std::endl; + break; + } + case 1: + { + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + v = m_roboclaws[i]->readMainBatteryVoltage(); + std::cout << "Claw " << i << " Main battery voltage: " << v() << std::endl; + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + break; + } + case 2: + { + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + v = m_roboclaws[i]->readLogicBatteryVoltage(); + std::cout << "Claw " << i << " Logic battery voltage: " << v() << std::endl; + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + break; + } + case 3: + { + //min max main + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + minmax = m_roboclaws[i]->readMinMaxMainVoltages(); + std::cout << "Claw " << i << " Main battery min voltage: " << minmax[0].to(units::V) << std::endl; + std::cout << "Claw " << i << " Main battery max voltage: " << minmax[1].to(units::V) << std::endl; + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + break; + } + case 4: + { + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + minmax = m_roboclaws[i]->readMinMaxLogicVoltages(); + std::cout << "Claw " << i << " logic battery min voltage: " << minmax[0].to(units::V) << std::endl; + std::cout << "Claw " << i << " logic battery max voltage: " << minmax[1].to(units::V) << std::endl; + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + break; + } + case 5: + { + //set min max main + std::cout << "Enter a value in volts for the minimum main battery voltage (min 6, max 34)" << std::endl; + std::cin.clear(); + std::cin >> c; + minmax[0] = c * units::V; + std::cout << "Enter a value in volts for the maximum main battery voltage (min 6, max 34)" << std::endl; + std::cin.clear(); + std::cin >> c; + minmax[1] = c * units::V; + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setMainVoltages(minmax[0], minmax[1]); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + break; + } + case 6: + { + std::cout << "Enter a value in volts for the minimum logic battery voltage (min 6, max 34)" << std::endl; + std::cin.clear(); + std::cin >> c; + minmax[0] = c * units::V; + std::cout << "Enter a value in volts for the maximum logic battery voltage (min 6, max 34)" << std::endl; + std::cin.clear(); + std::cin >> c; + minmax[1] = c * units::V; + + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setLogicVoltages(minmax[0], minmax[1]); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + break; + } + case 7: + { + if (defaultsSet) + { + std::cout << "setting roboclaw's back to their prior values (upon entering voltage menu)" << std::endl; + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setMainVoltages(defaultMain[i][0], defaultMain[i][1]); + m_roboclaws[i]->setLogicVoltages(defaultLogic[i][0], defaultLogic[i][1]); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + } + else + { + std::cout << "Defaults not available" << std::endl; + } + break; + } + default: + std::cout << "invalid choice" << std::endl; + } + std::cout << std::endl << "Enter any number to continue..." << std::endl; + std::cin.clear(); + std::cin >> n; + } + while (choice != 0); + } + + void Diag::clawStatus() + { + std::array status; + int n; + std::string states[] = {"Normal", "M1 OverCurrent Warning", "M2 OverCurrent Warning", + "E-Stop", "Temperature Error", "Temperature2 Error", "Main Battery High Error", + "Logic Battery High Error", "Logic Battery Low Error", "M1 Driver Fault", "M2 Driver Fault", + "Main Battery High Warning", "Main Battery Low Warning", "Temperature Warning", + "Temperature2 Warning", "M1 Home", "M2 Home" + }; + + std::cout << "Reading status" << std::endl; + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + status = m_roboclaws[i]->readStatus(); + for (int j = 0; j < 17; j++) + { + if (status[j]) + { + std::cout << states[j] << std::endl; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << std::endl << "Enter any number to continue..." << std::endl; + std::cin.clear(); + std::cin >> n; + } + + void Diag::clawVersion() + { + int n; + std::cout << "RoboClaw readversion" << std::endl; + std::cout << "Output should resemble: " << std::endl; + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + std::cout << "claw " << i << " version: " << m_roboclaws[i]->readVersion() << std::endl; + } + catch (const std::exception& e) + { + std::cout << "claw: " << i << "| " << e.what() << std::endl; + } + } + std::cout << std::endl << "Enter any number to continue..." << std::endl; + std::cin.clear(); + std::cin >> n; + } + + void Diag::simpleDrive() + { + int16_t duty; + std::cout << "Enter a duty (-32767 - 32767, where 32767 is max duty)" << std::endl; + std::cout << "Enter 0 to stop the motor(s)" << std::endl; + std::cin.clear(); + std::cin >> duty; + + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->drive(duty); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + } + + void Diag::driveMenu() + { + int choice, n, mag = 0; + uint32_t mag2 = 0; + std::array s1, s2, v1, v2; + double d, acc; + MotorDynamics dynamics, stop; + units::Velocity v; + units::Distance dist; + units::Acceleration accel; + units::Time t; + stop.setSpeed(0); + stop.setDistance(0); + std::cout << std::endl << "Note that the drive functions will throw if the respective dynamics have not been set." << std::endl; + do + { + std::cout << std::endl << std::endl << "Roboclaw Dynamics Menu" << std::endl; + std::cout << "0| exit" << std::endl; + std::cout << "1| drive w/ velocity" << std::endl; + std::cout << "2| drive w/ velocity, acceleration" << std::endl; + std::cout << "3| drive w/ velocity, distance " << std::endl; + std::cout << "4| drive w/ velocity, distance, acceleration " << std::endl; + std::cout << "5| drive w/ velocity, distance, acceleration, deceleration " << std::endl; + std::cout << "6| set velocity" << std::endl; + std::cout << "7| set distance" << std::endl; + std::cout << "8| set acceleration" << std::endl; + std::cout << "9| set deceleration" << std::endl; + std::cout << "10| accel test" << std::endl; + std::cout << std::endl << std::endl << "Enter a choice" << std::endl; + std::cin.clear(); + std::cin >> choice; + + switch (choice) + { + case 0: + { + std::cout << "Exiting" << std::endl; + break; + } + case 3: + case 4: + case 5: + { + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setDynamics(dynamics, 0); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + std::cout << "Ensure that the dynamics are properly set." << std::endl; + } + } + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setDynamics(stop, 0); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + std::cout << "Ensure that the dynamics are properly set." << std::endl; + } + } + break; + } + case 1: + case 2: + { + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setDynamics(dynamics, 0); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + std::cout << "Ensure that the dynamics are properly set." << std::endl; + } + } + break; + } + + case 6: + { + //velocity + m_velocity = velocityPrompt(); + dynamics.setSpeed(m_velocity); + break; + } + case 7: + { + //dist + std::cout << "Distance must be positive" << std::endl; + m_dist = distancePrompt(); + if (dist() < 0) + { + std::cout << "Distance must be positive, changing sign of distance" << std::endl; + m_dist *= -1; + } + dynamics.setDistance(m_dist); + break; + } + case 8: + { + + //acceleration + std::cout << "acceleration = dist / time^2" << std::endl; + + std::cout << "Choose a distance unit" << std::endl; + std::cout << "1: meter, 2: ft, 3: cm , 4: in, 5: mm" << std::endl; + std::cin.clear(); + std::cin >> n; + switch (n) + { + case 1: + { + dist = units::m; + break; + } + case 2: + { + dist = units::ft; + break; + } + case 3: + { + dist = units::cm; + break; + } + case 4: + { + dist = units::in; + break; + } + case 5: + { + dist = units::mm; + break; + } + default: + { + std::cout << "Enter a valid selection, defaulting to feet" << std::endl; + dist = units::ft; + } + } + + std::cout << "Select a time unit" << std::endl; + std::cout << "1: seconds, 2: ms, 3: minute, 4: hour" << std::endl; + std::cin.clear(); + std::cin >> n; + switch (n) + { + case 1: + { + t = units::s; + break; + } + case 2: + { + t = units::ms; + break; + } + case 3: + { + t = units::minute; + break; + } + case 4: + { + t = units::hr; + break; + } + default: + { + std::cout << "Enter a valid selection, defaulting to seconds" << std::endl; + t = units::s; + } + } + + std::cout << "Enter a positive magnitude for acceleration" << std::endl; + std::cin.clear(); + std::cin >> d; + + accel = dist / (t * t); + accel *= d; + + std::cout << "debugging 1" << std::endl; + m_accel = accel; + dynamics.setAcceleration(m_accel); + + break; + } + case 9: + { + //acceleration + std::cout << "acceleration = dist / time^2" << std::endl; + + std::cout << "Choose a distance unit" << std::endl; + std::cout << "1: meter, 2: ft, 3: cm , 4: in, 5: mm" << std::endl; + std::cin.clear(); + std::cin >> n; + switch (n) + { + case 1: + { + dist = units::m; + break; + } + case 2: + { + dist = units::ft; + break; + } + case 3: + { + dist = units::cm; + break; + } + case 4: + { + dist = units::in; + break; + } + case 5: + { + dist = units::mm; + break; + } + default: + { + std::cout << "Enter a valid selection, defaulting to feet" << std::endl; + dist = units::ft; + } + } + + + std::cout << "Select a time unit" << std::endl; + std::cout << "1: seconds, 2: ms, 3: minute, 4: hour" << std::endl; + std::cin.clear(); + std::cin >> n; + switch (n) + { + case 1: + { + t = units::s; + break; + } + case 2: + { + t = units::ms; + break; + } + case 3: + { + t = units::minute; + break; + } + case 4: + { + t = units::hr; + break; + } + default: + { + std::cout << "Enter a valid selection, defaulting to seconds" << std::endl; + t = units::s; + } + } + + std::cout << "Enter a positive magnitude for acceleration" << std::endl; + std::cin.clear(); + std::cin >> d; + + accel = dist / (t * t); + accel *= d; + + m_decel = accel; + dynamics.setDeceleration(m_decel); + + break; + } + case 10: + { + //test acceleration + int numb = 0, dumb_iterator = 0, j; + std::vector averagesM1, averagesM2; + std::cout << "Enter number of iterations to do before prompting" << std::endl; + std::cin.clear(); + std::cin >> j; + if (dynamics.getDType() != MotorDynamics::DType::kSpeedAccel) + { + std::cout << " Speed/acceleration dynamic type required" << std::endl; + break; + } + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->resetEncoders(); + m_roboclaws[i]->setDynamics(dynamics, 0); + averagesM1.push_back(0); + averagesM2.push_back(0); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + std::cout << "Ensure that the dynamics are properly set." << std::endl; + } + } + s1[0] = m_roboclaws[0]->readEncoderRaw(Roboclaw::Motor::kM1); + v1[0] = m_roboclaws[0]->readEncoderVelocityRaw(Roboclaw::Motor::kM1); + + s1[1] = m_roboclaws[0]->readEncoderRaw(Roboclaw::Motor::kM2); + v1[1] = m_roboclaws[0]->readEncoderVelocityRaw(Roboclaw::Motor::kM2); + + do + { + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + s2[0] = m_roboclaws[i]->readEncoderRaw(Roboclaw::Motor::kM1); + v2[0] = m_roboclaws[i]->readEncoderVelocityRaw(Roboclaw::Motor::kM1); + acc = (pow(v2[0], 2) - pow(v1[0], 2)) / (2.0 * (s2[0] - (s1[0] + 1))); + if ((dumb_iterator % 10) == 0) + { + s1[0] = s2[0]; + v1[0] = v2[0]; + } + std::cout << "Claw " << i << " , motor M1&M2, accel(M1): "; + std::cout << acc << std::endl; + if (acc > 10000 || acc < -10000) + { + acc = 0; + } + averagesM1[i] += acc; + + s2[1] = m_roboclaws[i]->readEncoderRaw(Roboclaw::Motor::kM2); + v2[1] = m_roboclaws[i]->readEncoderVelocityRaw(Roboclaw::Motor::kM2); + acc = (pow(v2[1], 2) - pow(v1[1], 2)) / (2.0 * (s2[1] - (s1[1] + 1))); + + if ((dumb_iterator % 10) == 0) + { + s1[1] = s2[1]; + v1[1] = v2[1]; + } + std::cout << "Claw " << i << " , motor M1&M2, accel(M2): "; + std::cout << acc << std::endl; + if (acc > 10000 || acc < -10000) + { + acc = 0; + } + averagesM2[i] += acc; + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + std::cout << "Ensure that the dynamics are properly set." << std::endl; + } + + } + if (dumb_iterator > j) + { + std::cout << std::endl << "Enter any number to continue, -1 to stop" << std::endl; + std::cin.clear(); + std::cin >> numb; + } + dumb_iterator++; + } + while (numb != -1); + for (size_t i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setDynamics(stop, 0); + averagesM1[i] /= dumb_iterator; + averagesM2[i] /= dumb_iterator; + std::cout << "Claw " << i << " , motor M1&M2, avg accel(M1): "; + std::cout << std::dec << averagesM1[i] << std::endl; + std::cout << "Claw " << i << " , motor M1&M2, avg accel(M2): "; + std::cout << std::dec << averagesM2[i] << std::endl; + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + std::cout << "Ensure that the dynamics are properly set." << std::endl; + } + } + + std::cout << "fin" << std::endl; + break; + } + default: + { + std::cout << "invalid choice" << std::endl; + } + } + std::cout << std::endl << "Enter any number to continue..." << std::endl; + std::cin.clear(); + std::cin >> n; + } + while (choice != 0); + } + + void Diag::encoderMenu() + { + int choice, n, motors = 2, i = 0, ticks = 0; + units::Distance dist; + units::Velocity velocity; + /* + Motors: + 0 -> m1 + 1 -> m2 + 2 -> m1&m2 -> default + */ + do + { + std::cout << std::endl << std::endl << "Roboclaw Encoder Menu" << std::endl; + + std::cout << "0| exit" << std::endl; + std::cout << "1| read encoder raw tick count" << std::endl; + std::cout << "2| read encoder (distance)" << std::endl; + std::cout << "3| set encoder raw tick count" << std::endl; + std::cout << "4| set encoder via distance" << std::endl; + std::cout << "5| read encoder velocity (raw)" << std::endl; + std::cout << "6| read encoder velocity" << std::endl; + std::cout << "7| reset encoders to 0" << std::endl; + std::cout << "8| Motor selection (1,2, or both. default is both)" << std::endl; + + + std::cout << std::endl << std::endl << "Enter a choice" << std::endl; + std::cin.clear(); + std::cin >> choice; + switch (choice) + { + case 0: + { + std::cout << "Exiting" << std::endl; + break; + } + case 1: + { + //raw encoder tick count + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + std::cout << "Claw " << i << " , motor kM1, raw encoder value: "; + std::cout << m_roboclaws[i]->readEncoderRaw(Roboclaw::Motor::kM1) << std::endl; + break; + } + case 1: + { + std::cout << "Claw " << i << " , motor kM2, raw encoder value: "; + std::cout << m_roboclaws[i]->readEncoderRaw(Roboclaw::Motor::kM2) << std::endl; + break; + } + case 2: + { + std::cout << "Claw " << i << " , motor M1&M2, raw encoder value(1): "; + std::cout << m_roboclaws[i]->readEncodersRaw()[0] << std::endl; + std::cout << "Raw encoder value(2) " << m_roboclaws[i]->readEncodersRaw()[1] << std::endl; + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully read " << i << " roboclaw's encoder values" << std::endl; + break; + } + case 2: + { + //read encoder (distance) + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + std::cout << "Claw " << i << " , motor kM1, encoder value: "; + std::cout << m_roboclaws[i]->readEncoder(Roboclaw::Motor::kM1) << std::endl; + break; + } + case 1: + { + std::cout << "Claw " << i << " , motor kM2, encoder value: "; + std::cout << m_roboclaws[i]->readEncoder(Roboclaw::Motor::kM2) << std::endl; + break; + } + case 2: + { + std::cout << "Claw " << i << " , motor M1&M2, encoder value(1): "; + std::cout << m_roboclaws[i]->readEncodersRaw()[0] << std::endl; + std::cout << "Encoder value(2) " << m_roboclaws[i]->readEncodersRaw()[1] << std::endl; + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully read " << i << " roboclaw's encoder values" << std::endl; + break; + } + case 3: + { + //set encoder raw tick count + std::cout << "Enter amount of encoder ticks to set to" << std::endl; + std::cin.clear(); + std::cin >> ticks; + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + m_roboclaws[i]->setEncoderRaw(Roboclaw::Motor::kM1, ticks); + break; + } + case 2: + { + m_roboclaws[i]->setEncoderRaw(Roboclaw::Motor::kM1, ticks); + } + case 1: + { + m_roboclaws[i]->setEncoderRaw(Roboclaw::Motor::kM2, ticks); + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully set " << i << " roboclaw's encoder values" << std::endl; + break; + } + case 4: + { + //set encoder via distance + dist = distancePrompt(); + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + m_roboclaws[i]->setEncoder(Roboclaw::Motor::kM1, dist); + break; + } + case 2: + { + m_roboclaws[i]->setEncoder(Roboclaw::Motor::kM1, dist); + } + case 1: + { + m_roboclaws[i]->setEncoder(Roboclaw::Motor::kM2, dist); + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully set " << i << " roboclaw's encoder values" << std::endl; + break; + } + case 5: + { + //read encoder velocity (raw) + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + std::cout << "Claw " << i << " , motor M1, raw encoder velocity value: "; + std::cout << m_roboclaws[i]->readEncoderVelocityRaw(Roboclaw::Motor::kM1) << std::endl; + break; + } + case 1: + { + std::cout << "Claw " << i << " , motor M2, raw encoder velocity value: "; + std::cout << m_roboclaws[i]->readEncoderVelocityRaw(Roboclaw::Motor::kM2) << std::endl; + break; + } + case 2: + { + std::cout << "Claw " << i << " , motor M1&M2, raw encoder velocity value(1): "; + std::cout << m_roboclaws[i]->readEncodersVelocityRaw()[0] << std::endl; + std::cout << "raw Encoder velocity value(2) " << m_roboclaws[i]->readEncodersVelocityRaw()[1] << std::endl; + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + std::cout << "Successfully read " << i << " roboclaw's encoder values" << std::endl; + } + break; + } + case 6: + { + //read encoder velocity + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + std::cout << "Claw " << i << " , motor M1, encoder velocity value: "; + std::cout << m_roboclaws[i]->readEncoderVelocity(Roboclaw::Motor::kM1) << std::endl; + break; + } + case 1: + { + std::cout << "Claw " << i << " , motor M2, encoder velocity value: "; + std::cout << m_roboclaws[i]->readEncoderVelocity(Roboclaw::Motor::kM2) << std::endl; + break; + } + case 2: + { + std::cout << "Claw " << i << " , motor M1&M2, encoder velocity value(1): "; + std::cout << m_roboclaws[i]->readEncodersVelocity()[0] << std::endl; + std::cout << "Encoder velocity value(2) " << m_roboclaws[i]->readEncodersVelocityRaw()[1] << std::endl; + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + std::cout << "Successfully read " << i << " roboclaw's encoder values" << std::endl; + } + break; + } + case 7: + { + //reset encoders + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->resetEncoders(); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully reset " << i << " roboclaw's encoder values" << std::endl; + break; + } + case 8: + { + std::cout << "Enter a number between 0 and 2" << std::endl; + std::cout << "0 -> M1" << std::endl; + std::cout << "1 -> M2" << std::endl; + std::cout << "2 -> M1&M2 (default)" << std::endl; + std::cin.clear(); + std::cin >> motors; + if ((motors < 0 ) || (motors > 2)) + { + std::cout << "Invalid entry, resetting to default" << std::endl; + motors = 2; + } + break; + } + default: + std::cout << "invalid choice" << std::endl; + } + std::cout << std::endl << "Enter any number to continue..." << std::endl; + std::cin.clear(); + std::cin >> n; + } + while (choice != 0); + } + + void Diag::currentMenu() + { + int choice, n, motors = 2, i = 0; + bool defaultsSet = 1; + uint32_t temp; + std::vector defaultMaxM1; + std::vector defaultMaxM2; + units::Current current; + + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + defaultMaxM1.push_back(m_roboclaws[i]->readMaxCurrent(Roboclaw::Motor::kM1)); + defaultMaxM2.push_back(m_roboclaws[i]->readMaxCurrent(Roboclaw::Motor::kM2)); + } + catch (const std::exception& e) + { + std::cout << "Issue reading claw " << i << " max current, " << e.what() << std::endl; + std::cout << "Disabling revert option (option 7) " << std::endl; + defaultsSet = 0; + } + } + + do + { + std::cout << std::endl << std::endl << "Roboclaw Current Menu" << std::endl; + std::cout << "0| exit" << std::endl; + std::cout << "1| read current" << std::endl; + std::cout << "2| set max current" << std::endl; + std::cout << "3| read max current" << std::endl; + std::cout << "4| revert to original max current (upon entering current menu)" << std::endl; + std::cout << "5| Motor selection (1,2, or both. default is both)" << std::endl; + std::cout << std::endl << std::endl << "Enter a choice" << std::endl; + std::cin.clear(); + std::cin >> choice; + + switch (choice) + { + case 0: + std::cout << "Exiting" << std::endl; + break; + case 1: + { + //read current + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + std::cout << "Claw " << i << " , motor M1, current value: "; + std::cout << m_roboclaws[i]->readCurrent(Roboclaw::Motor::kM1) << std::endl; + break; + } + case 1: + { + std::cout << "Claw " << i << " , motor M2, current value: "; + std::cout << m_roboclaws[i]->readCurrent(Roboclaw::Motor::kM2) << std::endl; + break; + } + case 2: + { + std::cout << "Claw " << i << " , motor M1&M2, current value(1): "; + std::cout << m_roboclaws[i]->readCurrents()[0] << std::endl; + std::cout << "Current value(2) " << m_roboclaws[i]->readCurrents()[1] << std::endl; + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully read " << i << " roboclaw's current values" << std::endl; + break; + } + case 2: + { + //setMaxCurrent + std::cout << "Enter maximum current (amps)" << std::endl; + std::cin.clear(); + std::cin >> temp; + current = units::A * temp; + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + m_roboclaws[i]->setMaxCurrent(Roboclaw::Motor::kM1, current); + break; + } + case 2: + { + m_roboclaws[i]->setMaxCurrent(Roboclaw::Motor::kM1, current); + } + case 1: + { + m_roboclaws[i]->setMaxCurrent(Roboclaw::Motor::kM2, current); + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully set " << i << " roboclaw's max current values" << std::endl; + break; + } + case 3: + { + //readMaxCurrent + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + std::cout << "Claw " << i << " , motor M1, max current value: "; + std::cout << m_roboclaws[i]->readMaxCurrent(Roboclaw::Motor::kM1) << std::endl; + break; + } + case 1: + { + std::cout << "Claw " << i << " , motor M2, max current value: "; + std::cout << m_roboclaws[i]->readMaxCurrent(Roboclaw::Motor::kM2) << std::endl; + break; + } + case 2: + { + std::cout << "Claw " << i << " , motor M1&M2, max current value(1): "; + std::cout << m_roboclaws[i]->readMaxCurrent(Roboclaw::Motor::kM2) << std::endl; + std::cout << "max current value(2) " << m_roboclaws[i]->readMaxCurrent(Roboclaw::Motor::kM2) << std::endl; + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully read " << i << " roboclaw's max current values" << std::endl; + break; + } + case 4: + { + //revert current changes + if (defaultsSet) + { + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setMaxCurrent(Roboclaw::Motor::kM1, defaultMaxM1[i]); + m_roboclaws[i]->setMaxCurrent(Roboclaw::Motor::kM2, defaultMaxM2[i]); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + } + else + { + std::cout << "Defaults are not available." << std::endl; + } + break; + } + case 5: + { + + std::cout << "Enter a number between 0 and 2" << std::endl; + std::cout << "0 -> M1" << std::endl; + std::cout << "1 -> M2" << std::endl; + std::cout << "2 -> M1&M2 (default)" << std::endl; + std::cin.clear(); + std::cin >> motors; + if ((motors < 0 ) || (motors > 2)) + { + std::cout << "Invalid entry, resetting to default" << std::endl; + motors = 2; + } + break; + } + default: + { + std::cout << "invalid choice" << std::endl; + } + } + std::cout << std::endl << "Enter any number to continue..." << std::endl; + std::cin.clear(); + std::cin >> n; + } + while (choice != 0); + } + + void Diag::pidMenu() + { + int choice, n, i = 0, motors = 2, j = 0; + float pid = 0; + uint32_t qpps; + VelocityPIDParameters vparams; + PositionPIDParameters pparams; + + do + { + std::cout << std::endl << std::endl << "Roboclaw {} Menu" << std::endl; + std::cout << "0| exit" << std::endl; + std::cout << "1| read velocity PID" << std::endl; + std::cout << "2| read position PID" << std::endl; + std::cout << "3| set velocity PID" << std::endl; + std::cout << "4| set position PID" << std::endl; + std::cout << "5| set velocity PID parameters" << std::endl; + std::cout << "6| set position PID parameters" << std::endl; + std::cout << "7| Motor selection (1,2, or both. default is both)" << std::endl; + + + std::cout << std::endl << std::endl << "Enter a choice" << std::endl; + std::cin.clear(); + std::cin >> choice; + switch (choice) + { + case 0: + std::cout << "Exiting" << std::endl; + break; + case 1: + { + //read velocity PID + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + case 2: + case 0: + { + vparams = m_roboclaws[i]->readVelocityPID(Roboclaw::Motor::kM1); + std::cout << "Claw " << i << " , motor M1, velocity, P: " << vparams.kp << " I: "; + std::cout << vparams.ki << " D: " << vparams.kd << " QPPS: " << vparams.qpps; + if (motors != 2) + { + break; + } + } + case 1: + { + vparams = m_roboclaws[i]->readVelocityPID(Roboclaw::Motor::kM2); + std::cout << "Claw " << i << " , motor M1, velocity, P: " << vparams.kp << " I: "; + std::cout << vparams.ki << " D: " << vparams.kd << " QPPS: " << vparams.qpps << std::endl; + + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully read " << i << " roboclaw's velocity PID values" << std::endl; + break; + } + case 2: + { + //read position PID + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + case 2: + case 0: + { + pparams = m_roboclaws[i]->readPositionPID(Roboclaw::Motor::kM1); + std::cout << "Claw " << i << " , motor M1, position, P: " << pparams.kp << " I: "; + std::cout << pparams.ki << " D: " << pparams.kd << std::endl; + std::cout << "kiMax: " << pparams.kiMax << " deadzone: " << pparams.deadzone << " min: "; + std::cout << pparams.min << " max: " << pparams.max << std::endl; + if (motors != 2) + { + break; + } + } + case 1: + { + pparams = m_roboclaws[i]->readPositionPID(Roboclaw::Motor::kM2); + std::cout << "Claw " << i << " , motor M2, position, P: " << pparams.kp << " I: "; + std::cout << pparams.ki << " D: " << pparams.kd << std::endl; + std::cout << "kiMax: " << pparams.kiMax << " deadzone: " << pparams.deadzone << " min: "; + std::cout << pparams.min << " max: " << pparams.max << std::endl; + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully read " << i << " roboclaw's position PID values" << std::endl; + break; + } + case 3: + { + //set velocity PID + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + case 2: + case 0: + { + m_roboclaws[i]->setVelocityPID(Roboclaw::Motor::kM1, vparams); + if (motors != 2) + { + break; + } + } + case 1: + { + m_roboclaws[i]->setVelocityPID(Roboclaw::Motor::kM2, vparams); + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully set " << i << " roboclaw's velocity PID values" << std::endl; + break; + } + case 4: + { + //set position PID + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + case 2: + case 0: + { + m_roboclaws[i]->setPositionPID(Roboclaw::Motor::kM1, pparams); + if (motors != 2) + { + break; + } + } + case 1: + { + m_roboclaws[i]->setPositionPID(Roboclaw::Motor::kM2, pparams); + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully set " << i << " roboclaw's velocity PID values" << std::endl; + break; + } + case 5: + { + std::cout << "Enter a number for proportional gain" << std::endl; + std::cin.clear(); + std::cin >> pid; + vparams.kp = pid; + std::cout << "Enter a number for integral gain" << std::endl; + std::cin.clear(); + std::cin >> pid; + vparams.ki = pid; + std::cout << "Enter a number for derivative gain" << std::endl; + std::cin.clear(); + std::cin >> pid; + vparams.kd = pid; + std::cout << "Enter a number for quadrature pulses per second" << std::endl; + std::cin.clear(); + std::cin >> qpps; + vparams.qpps = qpps; + + break; + } + case 6: + { + std::cout << "Enter a number for proportional gain" << std::endl; + std::cin.clear(); + std::cin >> pid; + pparams.kp = pid; + std::cout << "Enter a number for integral gain" << std::endl; + std::cin.clear(); + std::cin >> pid; + pparams.ki = pid; + std::cout << "Enter a number for derivative gain" << std::endl; + std::cin.clear(); + std::cin >> pid; + pparams.kd = pid; + std::cout << "Enter a number for kiMax" << std::endl; + std::cin.clear(); + std::cin >> qpps; + pparams.kiMax = qpps; + std::cout << "Enter a number for deadzone" << std::endl; + std::cin.clear(); + std::cin >> qpps; + pparams.deadzone = qpps; + std::cout << "Enter a number for min" << std::endl; + std::cin.clear(); + std::cin >> qpps; + pparams.min = qpps; + std::cout << "Enter a number for max" << std::endl; + std::cin.clear(); + std::cin >> qpps; + pparams.max = qpps; + break; + } + case 7: + { + + std::cout << "Enter a number between 0 and 2" << std::endl; + std::cout << "0 -> M1" << std::endl; + std::cout << "1 -> M2" << std::endl; + std::cout << "2 -> M1&M2 (default)" << std::endl; + std::cin.clear(); + std::cin >> motors; + if ((motors < 0 ) || (motors > 2)) + { + std::cout << "Invalid entry, resetting to default" << std::endl; + motors = 2; + } + break; + } + default: + { + std::cout << "invalid choice" << std::endl; + } + } + } + while (choice != 0); + } + + void Diag::miscMenu() + { + int choice, n, i = 0, motors = 2; + Config cfg; + uint16_t encodedCfg; + bool cfgSet = 0; + std::vector defaultCfg; + do + { + std::cout << std::endl << std::endl << "Roboclaw Miscellaneous Menu" << std::endl; + std::cout << "0| exit" << std::endl; + std::cout << "1| read temperature" << std::endl; + std::cout << "2| read buffer length" << std::endl; + std::cout << "3| read config" << std::endl; + std::cout << "Warning: setting config can break things" << std::endl; + std::cout << "4| set config via encoded hex" << std::endl; + std::cout << "Motor selection applies to read buffer length" << std::endl; + std::cout << "5| Motor selection (1,2, or both. default is both) " << std::endl; + std::cout << "6| set config back to state after first read" << std::endl; + + std::cout << std::endl << std::endl << "Enter a choice" << std::endl; + std::cin.clear(); + std::cin >> choice; + switch (choice) + { + case 0: + { + std::cout << "Exiting" << std::endl; + break; + } + case 1: + { + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + std::cout << "Temperature of claw " << i << ": " << m_roboclaws[i]->readTemperature() << std::endl; + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + + } + std::cout << "Successfully read the temperature of " << i << " roboclaws" << std::endl; + break; + } + case 2: + { + //read buffer Length + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + switch (motors) + { + + case 0: + { + std::cout << "Claw " << i << " , motor M1, buffer length: "; + std::cout << m_roboclaws[i]->readBufferLen(Roboclaw::Motor::kM1) << std::endl; + break; + } + case 1: + { + std::cout << "Claw " << i << " , motor M2, buffer length: "; + std::cout << m_roboclaws[i]->readBufferLen(Roboclaw::Motor::kM2) << std::endl; + break; + } + case 2: + { + std::cout << "Claw " << i << " , motor M1&M2, buffer length(M1): "; + std::cout << m_roboclaws[i]->readBufferLens()[0] << std::endl; + std::cout << "buffer length m2: " << m_roboclaws[i]->readBufferLens()[1] << std::endl; + break; + } + } + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully read " << i << " roboclaw's buffer lengths" << std::endl; + break; + } + case 3: + { + //read config1 + std::cout << "See Roboclaw manual if you want to decode the output." << std::endl; + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + cfg = m_roboclaws[i]->getConfig(); + if (!cfgSet) + { + defaultCfg.push_back(cfg); + } + std::cout << "claw " << i << ": " << std::hex << cfg.get() << std::endl; + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + if (defaultCfg.size() >= m_roboclaws.size()) + { + cfgSet = true; + } + std::cout << "Successfully read config of " << i << "roboclaws" << std::endl; + break; + } + case 4: + { + //set config + std::cout << "Changes to config can break communications, or mess with voltages" << std::endl; + + for (i = 0; i < m_roboclaws.size(); i++) + { + std::cout << "Enter hex of encoded config settings to set for claw " << i << std::endl; + std::cin.clear(); + std::cin >> std::hex >> encodedCfg; + cfg.set(encodedCfg); + try + { + m_roboclaws[i]->setConfig(cfg); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully set config of " << i << "roboclaws" << std::endl; + break; + } + case 5: + { + std::cout << "Enter a number between 0 and 2" << std::endl; + std::cout << "0 -> M1" << std::endl; + std::cout << "1 -> M2" << std::endl; + std::cout << "2 -> M1&M2 (default)" << std::endl; + std::cin.clear(); + std::cin >> motors; + if ((motors < 0 ) || (motors > 2)) + { + std::cout << "Invalid entry, resetting to default" << std::endl; + motors = 2; + } + break; + } + case 6: + { + if (cfgSet) + { + std::cout << "Reverting changes to settings" << std::endl; + for (i = 0; i < m_roboclaws.size(); i++) + { + try + { + m_roboclaws[i]->setConfig(defaultCfg[i]); + } + catch (const std::exception& e) + { + std::cout << "claw " << i << "| " << e.what() << std::endl; + } + } + std::cout << "Successfully set config of " << i << "roboclaws" << std::endl; + } + else + { + std::cout << "Default configs are not available" << std::endl; + } + break; + } + default: + { + std::cout << "invalid choice" << std::endl; + break; + } + } + std::cout << std::endl << "Enter any number to continue..." << std::endl; + std::cin.clear(); + std::cin >> n; + } + while (choice != 0); + } + + void Diag::removeClaw(int index) + { + try + { + m_roboclaws.erase(m_roboclaws.begin() + index); + } + catch (...) + { + std::cout << "Enter a valid index." << std::endl; + } + } + + void Diag::recoverClaws() + { + m_roboclaws = m_roboclaws_original; + } + + units::Distance Diag::distancePrompt() + { + //distance + int n; + double mag; + units::Distance dist; + std::cout << "Choose a distance unit" << std::endl; + std::cout << "1: meter, 2: ft, 3: cm , 4: in, 5: mm" << std::endl; + std::cin.clear(); + std::cin >> n; + switch (n) + { + case 1: + { + dist = units::m; + break; + } + case 2: + { + dist = units::ft; + break; + } + case 3: + { + dist = units::cm; + break; + } + case 4: + { + dist = units::in; + break; + } + case 5: + { + dist = units::mm; + break; + } + default: + { + std::cout << "Enter a valid selection, defaulting to feet" << std::endl; + dist = units::ft; + } + } + + std::cout << "Enter a positive magnitude for distance" << std::endl; + std::cin.clear(); + std::cin >> mag; + dist *= mag; + return dist; + } + + units::Velocity Diag::velocityPrompt() + { + //velocity + units::Distance dist; + units::Velocity v; + units::Time t; + int n; + double mag; + std::cout << "Velocity: distance / time: " << std::endl; + dist = distancePrompt(); + + std::cout << "Select a time unit" << std::endl; + std::cout << "1: seconds, 2: ms, 3: minute, 4: hour" << std::endl; + std::cin.clear(); + std::cin >> n; + switch (n) + { + case 1: + { + t = units::s; + break; + } + case 2: + { + t = units::ms; + break; + } + case 3: + { + t = units::minute; + break; + } + case 4: + { + t = units::hr; + break; + } + default: + { + std::cout << "Enter a valid selection, defaulting to seconds" << std::endl; + t = units::s; + } + } + + std::cout << "Enter a signed magnitute" << std::endl; + std::cin.clear(); + std::cin >> mag; + v = dist / t; + v *= mag; + return v; + + } + + void Diag::start() + { + std::cout << "Roboclaw hardware testing and diagnostics" << std::endl; + std::cout << "WARNING: Physical access to the hardware is required. Ensure that it is "; + std::cout << "running off battery power, and the wheels should be free spinning." << std::endl; + std::cout << std::endl << "These tests aim to reveal problems that unit testing can't account for" << std::endl; + std::cout << std::endl << std::endl << std::endl; + mainMenu(); + + } + + } + } + } +} +#pragma GCC diagnostic pop diff --git a/core/motor_controllers/diag/diag_roboclaw.hpp b/core/motor_controllers/diag/diag_roboclaw.hpp new file mode 100644 index 0000000..0bd5205 --- /dev/null +++ b/core/motor_controllers/diag/diag_roboclaw.hpp @@ -0,0 +1,117 @@ +#include +#include +#include +#include +#include +/* +Interactive diagnostic tests, specifically for the Roboclaw's on the demo bot (for now) +DO NOT run without physical access, and powered on via battery. +Killing this program will not save you from the malice of motors. +*/ + +namespace rip +{ + namespace motorcontrollers + { + namespace roboclaw + { + namespace diag + { + + class Diag + { + public: + /* + Constructor - takes vector of memory addresses of roboclaw's + */ + Diag(std::vector roboclaws); + + /* + Main menu - switch statement, organizes submenus. Some actual + diag tests here for those who don't fit into a category well and + might be frequently used + */ + void mainMenu(); + /* + Menu that gives access to a list of battery voltage commands that can be run + for diagnostic purposes. + */ + void voltageMenu(); + /* + Attempts to read the status flag's of the roboclaw. + */ + void clawStatus(); + /* + Attempt's to read the version of the roboclaw. + */ + void clawVersion(); + /* + Attempts to send a basic drive command that allows the user to specify duty + */ + void simpleDrive(); + /* + Menu that gives access to a list ofdriving commands that can be run + for diagnostic purposes. + */ + void driveMenu(); + /* + Menu that gives access to a list of encoder commands that can be run + for diagnostic purposes. + */ + void encoderMenu(); + /* + Menu that gives access to a list of battery current commands that can be run + for diagnostic purposes. + */ + void currentMenu(); + /* + Menu that gives access to a list of configuration commands that can be run + for diagnostic purposes. + */ + void configMenu(); + /* + Menu that gives access to a list of PID tuning commands that can be run + for diagnostic purposes. + */ + void pidMenu(); + /* + Menu that gives access to a miscellaneous list of roboclaw commands that can be run + for diagnostic purposes. + */ + void miscMenu(); + /* + Start, shows some generic warning message followed by the main "menu" + */ + void start(); + /* + Use if you want to stop running diagnostics on a roboclaw. takes the index + of the roboclaw to remove. + */ + void removeClaw(int index); + /* + Used if some claws have been removed and the user wants the list to be restored + to its original state. + */ + void recoverClaws(); + /* + Prompts user to provide desired value to set in distance units, ie cm, ft, m, etc + */ + units::Distance distancePrompt(); + /* + prompts user to provide velocity in desired units, and magnitude. ie m/s, ft/ms. + */ + units::Velocity velocityPrompt(); + + private: + std::vector m_roboclaws; + std::vector m_roboclaws_original; + + units::Distance m_dist; + units::Acceleration m_accel; + units::Acceleration m_decel; + units::Velocity m_velocity; + }; + } + } + } +} diff --git a/core/motor_controllers/diag/diag_roboclaw_main.cpp b/core/motor_controllers/diag/diag_roboclaw_main.cpp new file mode 100644 index 0000000..a3976ec --- /dev/null +++ b/core/motor_controllers/diag/diag_roboclaw_main.cpp @@ -0,0 +1,81 @@ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#pragma GCC diagnostic ignored "-Wunused-parameter" +#pragma GCC diagnostic ignored "-Wswitch-default" +#pragma GCC diagnostic ignored "-Wsign-compare" +#pragma GCC diagnostic ignored "-Wimplicit-fallthrough=" + +#include +#include +#include +#include +#include +#include "diag_roboclaw.hpp" + +using namespace rip::motorcontrollers::roboclaw; +/* +Interactive diagnostic tests, specifically for the Roboclaw's on the demo bot (for now) +DO NOT run without physical access, and powered on via battery. +Killing this program will not save you from the malice of motors. +*/ + +int main(int argc, char** argv) +{ + //make serial stuff + nlohmann::json config1 = + { + {"address", 0x80}, + {"timeout", 1000}, + {"ticks_per_rev", 3591.84}, + {"wheel_radius", (2.875 / 2.0)* rip::units::in}, + {"device", "/dev/ttyAMA0"}, + {"baudrate", 115200} + }; + nlohmann::json config2 = config1; + config2["address"] = 0x81; + + Roboclaw testClawLeft(config2); + Roboclaw testClawRight(config1); + std::vector claws = {&testClawLeft, &testClawRight}; + + diag::Diag diagTool(claws); + diagTool.start(); + /* + MotorDynamics dynamics, stop; + stop.setSpeed(0); + stop.setDistance(0); + int64_t ticks; + rip::utilities::units::Velocity speed; + rip::utilities::units::Distance d2; + testClawRight.resetEncoders(); + testClawLeft.resetEncoders(); + + d2 = 2.875 * rip::utilities::units::pi * rip::utilities::units::in; + speed = rip::utilities::units::foot / rip::utilities::units::s; + dynamics.setDistance(d2); + dynamics.setSpeed(speed); + + + testClawRight.setDynamics(dynamics); + testClawLeft.setDynamics(dynamics); + testClawRight.setDynamics(stop, false); + testClawLeft.setDynamics(stop, false); + + //std::cout << "ticks m1 left " << testClawLeft.readEncoderRaw(Roboclaw::Motor::kM1) << std::endl; + /* + testClawRight.resetEncoders(); + testClawLeft.resetEncoders(); + + //d2 = .073025 * rip::utilities::units::pi * rip::utilities::units::m; + speed = 10 * rip::utilities::units::foot / rip::utilities::units::s; + dynamics.setDistance(d2); + dynamics.setSpeed(speed); + + testClawRight.setDynamics(dynamics); + testClawLeft.setDynamics(dynamics); + std::cout << "ticks m1 left " << testClawLeft.readEncoderRaw(Roboclaw::Motor::kM1) << std::endl; + */ + return 0; +} + +#pragma GCC diagnostic pop diff --git a/core/framework/include/run_once_action.hpp b/core/motor_controllers/include/motor_controllers/exceptions.hpp similarity index 51% rename from core/framework/include/run_once_action.hpp rename to core/motor_controllers/include/motor_controllers/exceptions.hpp index 773e410..bbfc912 100644 --- a/core/framework/include/run_once_action.hpp +++ b/core/motor_controllers/include/motor_controllers/exceptions.hpp @@ -17,57 +17,52 @@ * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ */ -#ifndef RUN_ONCE_ACTION_HPP -#define RUN_ONCE_ACTION_HPP - -#include "action.hpp" +#ifndef MOTORCONTROLLER_EXCEPTIONS_HPP +#define MOTORCONTROLLER_EXCEPTIONS_HPP +#include namespace rip { - /** - * Base action for something that only needs to be done - * once - */ - class RunOnceAction : public Action + namespace motorcontrollers { - public: /** - * Returns whether the action is finished - * - * @note Is always true + * @class CommandFailure + * @brief An exception for when the command fails */ - virtual bool isFinished() override; + NEW_EX(CommandFailure) /** - * Action has already happened, so nothing + * @class ReadFailure + * @brief An exception for when the response for a read command fails */ - virtual void update() override; + NEW_EX(ReadFailure) /** - * Where the action actually does stuff + * @class OutOfRange + * @brief An exception for when something falls out of range */ - virtual void setup() override; + NEW_EX(OutOfRange) /** - * [teardown description] + * @class UnknownDType + * @brief Unknown combination of motor dynamic parameters */ - virtual void teardown() override; - + NEW_EX(UnknownDType) /** - * This is where the magic happens for this action + * Json passed to Roboclaw constructor is null or has bad data. */ - virtual void runOnce() = 0; + NEW_EX(BadJson) /** - * @note No state to save + * @class SerialOpenFail + * @brief Exception thrown if serial device fails to open */ - virtual nlohmann::json save() const override; - + NEW_EX(SerialOpenFail) /** - * @note No state to restore + * @class InvalidCommMode + * @brief Exception thrown if invalid comm mode is in use */ - virtual void restore(const nlohmann::json& state) override; - }; + NEW_EX(InvalidCommMode) + } } - -#endif // RUN_ONCE_ACTION_HPP +#endif //MOTORCONTROLLER_EXCEPTIONS_HPP diff --git a/core/motor_controllers/include/motor_controllers/motor_dynamics.hpp b/core/motor_controllers/include/motor_controllers/motor_dynamics.hpp new file mode 100644 index 0000000..a3fb327 --- /dev/null +++ b/core/motor_controllers/include/motor_controllers/motor_dynamics.hpp @@ -0,0 +1,111 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ +#ifndef MOTOR_DYNAMICS_HPP +#define MOTOR_DYNAMICS_HPP + +#include + +#include + +namespace rip +{ + namespace motorcontrollers + { + /** + * @struct MotorDynamics + * @brief The dynamic properties for the motor + */ + class MotorDynamics + { + public: + /** + * @brief Constructor + */ + MotorDynamics(); + + /** + * @brief Sets the distance for the motor dynamics + * @param distance + */ + void setDistance(units::Distance distance); + + /** + * @brief Returns the distance (nullptr if distance isn't set) + * @returns The distance part of the motor dynamics + */ + std::shared_ptr getDistance() const; + + /** + * @brief Sets the speed + * @param speed + */ + void setSpeed(units::Velocity speed); + + /** + * @brief Returns the speed (nullptr if speed isn't set) + * @returns + */ + std::shared_ptr getSpeed() const; + + /** + * @brief Sets the acceleration + * @param acceleration + */ + void setAcceleration(units::Acceleration acceleration); + + /** + * @brief Returns the acceleration (nullptr if acceleration isn't set) + * @returns + */ + std::shared_ptr getAcceleration() const; + + /** + * @brief Sets the deceleration + * @param deceleration + */ + void setDeceleration(units::Acceleration deceleration); + + /** + * @brief Returns the deceleration (nullptr if deceleration isn't set) + * @return + */ + std::shared_ptr getDeceleration() const; + + enum class DType + { + kNone = 0x0, + kSpeed = 0x8, + kSpeedAccel = 0xC, + kSpeedDist = 0x9, + kSpeedAccelDist = 0xD, + kSpeedAccelDecelDist = 0xF + }; + + DType getDType() const; + + private: + std::shared_ptr m_distance; + std::shared_ptr m_speed; + std::shared_ptr m_acceleration; + std::shared_ptr m_deceleration; + }; // struct MotorDynamics + } // namespace roboclaw +} +#endif // MOTOR_DYNAMICS_HPP diff --git a/core/roboclaw/include/config.hpp b/core/motor_controllers/include/motor_controllers/roboclaw/config.hpp similarity index 94% rename from core/roboclaw/include/config.hpp rename to core/motor_controllers/include/motor_controllers/roboclaw/config.hpp index 5e9bb81..62fdbf8 100644 --- a/core/roboclaw/include/config.hpp +++ b/core/motor_controllers/include/motor_controllers/roboclaw/config.hpp @@ -21,12 +21,12 @@ #define CONFIG_HPP #include +#include "motor_controllers/exceptions.hpp" namespace rip { - namespace utilities + namespace motorcontrollers { - namespace roboclaw { /** @@ -143,8 +143,13 @@ namespace rip * @param config */ void set(uint16_t config); + /** + * @brief Get the config + * @param n/a + */ + uint16_t get() const; private: - uint16_t m_config; + uint16_t m_config = 0x80A7; }; } } diff --git a/core/roboclaw/include/pid_parameters.hpp b/core/motor_controllers/include/motor_controllers/roboclaw/pid_parameters.hpp similarity index 60% rename from core/roboclaw/include/pid_parameters.hpp rename to core/motor_controllers/include/motor_controllers/roboclaw/pid_parameters.hpp index ed64eb3..8c62291 100644 --- a/core/roboclaw/include/pid_parameters.hpp +++ b/core/motor_controllers/include/motor_controllers/roboclaw/pid_parameters.hpp @@ -24,7 +24,7 @@ namespace rip { - namespace utilities + namespace motorcontrollers { namespace roboclaw { @@ -32,25 +32,24 @@ namespace rip * @struct PIDParameters * @brief The parameters for PID */ - struct PIDParameters - { - float kp; //< Proportional Gain - float ki; //< Integral Gain - float kd; //< Derivative Gain - }; // struct PIDParameters - - struct VelocityPIDParameters : public PIDParameters + struct VelocityPIDParameters { - uint32_t qpps; //< Quadrature Pulses Per Second - Maximum number of ticks per second - }; // struct VelcityPIDParameters + float kp = static_cast(0x10000) / 65536; //< Proportional Gain + float ki = static_cast(0x8000) / 65536; //< Integral Gain + float kd = static_cast(0x4000) / 65536; //< Derivative Gain + uint32_t qpps = 44000; //< Quadrature Pulses Per Second - Maximum number of ticks per second + }; // struct VelocityPIDParameters - struct PositionPIDParameters : public PIDParameters + struct PositionPIDParameters { - uint32_t kiMax; //< Maximum Integral Windup - uint32_t deadzone; //< Deadzone in encoder counts - uint32_t min; //< Minimum Position - uint32_t max; //< Maximum Position + float kp = 0; + float ki = 0; + float kd = 0; + uint32_t kiMax = 0; //< Maximum Integral Windup + uint32_t deadzone = 0; //< Deadzone in encoder counts + uint32_t min = 0; //< Minimum Position + uint32_t max = 0; //< Maximum Position }; // struct PositionPIDParameters } // namespace roboclaw } diff --git a/core/roboclaw/include/roboclaw.hpp b/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp similarity index 85% rename from core/roboclaw/include/roboclaw.hpp rename to core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp index 3e05304..5eff2bc 100644 --- a/core/roboclaw/include/roboclaw.hpp +++ b/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp @@ -24,19 +24,23 @@ #include #include #include - #include -#include -#include +#include -#include "exceptions.hpp" -#include "motor_dynamics.hpp" -#include "pid_parameters.hpp" -#include "config.hpp" +#include +#include + +#include "motor_controllers/motor_dynamics.hpp" +#include "motor_controllers/roboclaw/pid_parameters.hpp" +#include "motor_controllers/roboclaw/config.hpp" +extern "C" +{ +#include "serial.h" +} namespace rip { - namespace utilities + namespace motorcontrollers { namespace roboclaw { @@ -47,7 +51,7 @@ namespace rip * @link http://www.ionmc.com/ * @link https://www.pololu.com/category/124/roboclaw-motor-controllers */ - class Roboclaw : serial::Serial + class Roboclaw : public framework::Subsystem { public: enum class Motor @@ -157,8 +161,19 @@ namespace rip * @brief Constructor * @param json config */ - Roboclaw(nlohmann::json config); + Roboclaw(const nlohmann::json& config, bool test = true); + /** + * @brief Destructor + * + */ + + virtual ~Roboclaw(); + /** + * @brief validates json config passed to roboclaw + * + */ + void validateConfig(const nlohmann::json& testcfg); ////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////// PWM ///////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////// @@ -183,6 +198,7 @@ namespace rip * @exception CommandFailure Thrown if the command fails * @exception OutOfRange Thrown if the voltage is out of range */ + void drive(Motor motor, int16_t speed); /** @@ -213,7 +229,7 @@ namespace rip * @exception CommandFailure Thrown if the command fails * @exception OutOfRange Thrown if the voltage is out of range */ - void setMainVoltages(units::Voltage min, units::Voltage max); + void setMainVoltages(const units::Voltage& min, const units::Voltage& max); /** * @brief Read the main battery voltage level connected to B+ and B- terminals. @@ -255,7 +271,7 @@ namespace rip * @exception CommandFailure Thrown if the command fails * @exception OutOfRange Thrown if the voltage is out of range */ - void setLogicVoltages(units::Voltage min, units::Voltage max); + void setLogicVoltages(const units::Voltage& min, const units::Voltage& max); /** * @brief Read a logic battery voltage level connected to LB+ and LB- terminals. @@ -295,7 +311,7 @@ namespace rip * * @returns The encoder count in ticks */ - long readEncoderRaw(Motor motor); + int32_t readEncoderRaw(Motor motor); /** * @brief Read the encoder count in ticks for both encoders @@ -304,7 +320,7 @@ namespace rip * @note Uses 2 sequential reads in order to get direction (which isn't * provided by the command for requesting both) */ - std::array readEncodersRaw(); + std::array readEncodersRaw(); /** * @brief Read the encoder value (as a distance) for a single encoder @@ -339,7 +355,7 @@ namespace rip * @param motor Which to motor to set the encoder of * @param d The distance to set the current encoder to */ - void setEncoder(Motor motor, units::Distance d); + void setEncoder(Motor motor, const units::Distance& d); /** * @brief Returns the encoder velocity in ticks per second for a single encoder @@ -348,7 +364,7 @@ namespace rip * * @returns The encoder velocity in ticks per second for a single encoder */ - long readEncoderVelocityRaw(Motor motor); + int32_t readEncoderVelocityRaw(Motor motor); /** * @brief Returns the encoder velocity (as a distance) @@ -364,7 +380,7 @@ namespace rip * * @returns The encoder velocities in ticks per second for both encoders */ - std::array readEncodersVelocityRaw(); + std::array readEncodersVelocityRaw(); /** * @brief Returns the encoder velocities (using actual units) for both encoders @@ -454,6 +470,7 @@ namespace rip */ void setDynamics(const MotorDynamics& dynamics, bool respectBuffer = true); + ////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////// Misc ////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////// @@ -486,7 +503,7 @@ namespace rip * @param motor Which motor * @param current The current limit */ - void setMaxCurrent(Motor motor, units::Current current); + void setMaxCurrent(Motor motor, const units::Current& current); /** * @brief readMaxCurrent @@ -509,6 +526,25 @@ namespace rip */ units::Temperature readTemperature(); + /* + @brief: reads the number of commands in the buffer for + both motors. + Send: [Address, 47] + Receive: [BufferM1, BufferM2, CRC(2 bytes)] + + @returns Buffer length for motors 1 and 2 + */ + std::vector readBufferLens(); + /* + @brief: reads the number of commands in the buffer for + both motors, returns the buffer for just the specified motor. + Send: [Address, 47] + Receive: [BufferM1, BufferM2, CRC(2 bytes)] + + @returns Buffer length for motors 1 or 2 + */ + uint8_t readBufferLen(Motor motor); + enum class S3Modes { kDefault = 0, //< Flip switch if in RC/Analog mode or E-Stop (latching) in Serial modes @@ -580,14 +616,29 @@ namespace rip }; /** - * @brief Returns the roboclaw status + * @brief Returns the complete roboclaw status as an array of bools + * corresponding to each status in order. + * Send: [Address, 90] + * Receive: [Status, CRC(2 bytes)] + * + * @return + */ + std::array readStatus(); + /** + * @brief Returns the status of a specific status as a bool * * Send: [Address, 90] * Receive: [Status, CRC(2 bytes)] * * @return */ - Status readStatus(); + bool readStatus(Status s); + + /* + Returns 0xFF for crc check. Overrided when using mock to bypass CRC check. + */ + virtual uint8_t returnFF(); + /** * @brief Set the config for the roboclaw @@ -597,7 +648,7 @@ namespace rip * * @param config */ - void setConfig(Config config); + void setConfig(const Config& config); /** * @brief Returns the config for the roboclaw @@ -609,8 +660,12 @@ namespace rip */ Config getConfig(); + virtual void stop() override; + + virtual bool diagnostic() override; + private: - static const uint8_t kMaxRetries = 3; + static const uint8_t kMaxRetries = 10; /** * @brief Clear the checksum @@ -623,8 +678,28 @@ namespace rip * @param data Data to update the checksum with */ void crcUpdate (uint8_t data); - + /* + Updates CRC + */ uint16_t crcGet(); +////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////// Serial ///////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + /* Wrappers for some of periphery's C serial methods. + These are probably going to be phased out when we find a better + home for them, but they can stay here for now <3 */ + virtual void write(serial_t* serial, std::vector command, size_t len); + + virtual uint8_t read(serial_t* serial, units::Time timeout_ms); + + void open(serial_t* serial, std::string device, uint32_t baudrate); + + void open(serial_t* serial, std::string device, + uint32_t baudrate, unsigned int databits, + serial_parity_t parity, unsigned int stopbits, + bool xonxoff, bool rtscts); + + void close(serial_t* serial); /** * @brief Sends N bytes to the roboclaw @@ -640,19 +715,21 @@ namespace rip { std::vector command = {m_address}; argsToVector<0, Args...>(command, std::make_tuple(args...)); - + uint8_t data; for (int try_ = 0; try_ < kMaxRetries; try_++) { crcClear(); - for (int index = 0; index < command.size(); index++) + for (unsigned int index = 0; index < command.size(); index++) { crcUpdate(command[index]); } uint16_t crc = crcGet(); command.push_back(static_cast(crc >> 8)); command.push_back(static_cast(crc)); - write(command); - if (static_cast(read()[0]) == 0xFF) + + write(&m_serial, command, command.size()); + data = read(&m_serial, m_timeout); + if (data == returnFF()) { return; } @@ -675,13 +752,13 @@ namespace rip { auto value = std::get(tup); size_t size_v = sizeof(value); - for (int i_v = 0; i_v < size_v; i_v++) + for (unsigned int i_v = 0; i_v < size_v; i_v++) { command.push_back(value >> (8 * (size_v - 1 - i_v))); } argsToVector < I + 1, Args... > (command, tup); - }; + } /** * @brief Ends the recursion @@ -693,7 +770,7 @@ namespace rip * @param args The argument to add to the vector */ template - typename std::enable_if::type argsToVector(std::vector& command, std::tuple args) {}; + typename std::enable_if::type argsToVector(std::vector& command, std::tuple args) {} /** * @brief Read N bytes @@ -703,13 +780,23 @@ namespace rip * * @return N byte response */ - std::vector readN(uint8_t n, Command cmd); - + virtual std::vector readN(uint8_t n, Command cmd); uint16_t m_crc; units::Time m_timeout; uint8_t m_address; double m_ticks_per_rev; units::Distance m_wheel_radius; + //serial members + const char* m_device; + uint32_t m_baudrate; + serial_t m_serial; + //advanced + unsigned int m_databits, m_stopbits; + bool m_xonxoff, m_rtscts; + bool m_faking = false; + serial_parity_t m_parity; + //TODO: support advanced serial opening + }; // class Roboclaw inline uint8_t operator >>(const Roboclaw::Command& cmd, size_t shift) diff --git a/core/roboclaw/src/config.cpp b/core/motor_controllers/src/config.cpp similarity index 68% rename from core/roboclaw/src/config.cpp rename to core/motor_controllers/src/config.cpp index f9d1ff1..a09e991 100644 --- a/core/roboclaw/src/config.cpp +++ b/core/motor_controllers/src/config.cpp @@ -17,10 +17,12 @@ * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ */ -#include "config.hpp" +#include "motor_controllers/roboclaw/config.hpp" +#include + namespace rip { - namespace utilities + namespace motorcontrollers { namespace roboclaw { @@ -35,7 +37,7 @@ namespace rip Config::CommMode Config::getCommMode() const { - return static_cast(m_config && 0x03); + return static_cast(m_config & 0x03); } void Config::setBatteryMode(BatteryMode battery_mode) @@ -49,13 +51,18 @@ namespace rip Config::BatteryMode Config::getBatteryMode() const { - return static_cast(m_config && 0x1C); + + return static_cast(m_config & 0x1C); } void Config::setBaudRate(BaudRate baud_rate) { + if (static_cast(this->getCommMode()) <= 1) + { + throw InvalidCommMode("Baudrate cannot be set when commMode is RC/Analog"); + } // Clear baud rate using bit mask - uint16_t clear = 0xFF10; + uint16_t clear = 0xFF1F; m_config &= clear; m_config |= static_cast(baud_rate); @@ -63,11 +70,23 @@ namespace rip Config::BaudRate Config::getBaudRate() const { - return static_cast(m_config && 0xE0); + switch (static_cast(this->getCommMode())) + { + case 0: + case 1: + throw InvalidCommMode("commMode is RC/Analog"); + case 2: + throw InvalidCommMode("Simple serial is write only"); + } + return static_cast(m_config & 0xE0); } void Config::setPacketAddress(PacketAddress packet_address) { + if (static_cast(this->getCommMode()) <= 1) + { + throw InvalidCommMode("PacketAddress cannot be set when commMode is RC/Analog"); + } // Clear packet address using bit mask uint16_t clear = 0xF0FF; m_config &= clear; @@ -77,6 +96,14 @@ namespace rip Config::PacketAddress Config::getPacketAddress() const { + switch (static_cast(this->getCommMode())) + { + case 0: + case 1: + throw InvalidCommMode("commMode is RC/Analog"); + case 2: + throw InvalidCommMode("Simple serial is write only"); + } return static_cast(m_config & 0xF00); } @@ -108,6 +135,11 @@ namespace rip { m_config = config; } + + uint16_t Config::get() const + { + return m_config; + } } } -} \ No newline at end of file +} diff --git a/core/motor_controllers/src/motor_dynamics.cpp b/core/motor_controllers/src/motor_dynamics.cpp new file mode 100644 index 0000000..84ebc4d --- /dev/null +++ b/core/motor_controllers/src/motor_dynamics.cpp @@ -0,0 +1,108 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ +#include "motor_controllers/motor_dynamics.hpp" + +#include "motor_controllers/exceptions.hpp" + +namespace rip +{ + namespace motorcontrollers + { + MotorDynamics::MotorDynamics() + : m_distance(nullptr) + , m_speed(nullptr) + , m_acceleration(nullptr) + , m_deceleration(nullptr) + {} + + void MotorDynamics::setDistance(units::Distance distance) + { + if (distance() < 0.0) + { + throw OutOfRange("Distance should be a positive value."); + } + m_distance = std::make_shared(distance()); + } + + std::shared_ptr MotorDynamics::getDistance() const + { + return m_distance; + } + + void MotorDynamics::setSpeed(units::Velocity speed) + { + m_speed = std::make_shared(speed()); + } + + std::shared_ptr MotorDynamics::getSpeed() const + { + return m_speed; + } + + void MotorDynamics::setAcceleration(units::Acceleration acceleration) + { + if (acceleration() < 0.0) + { + throw OutOfRange("Acceleration should be a positive value."); + } + m_acceleration = std::make_shared(acceleration()); + } + + std::shared_ptr MotorDynamics::getAcceleration() const + { + return m_acceleration; + } + + void MotorDynamics::setDeceleration(units::Acceleration deceleration) + { + if (deceleration() < 0.0) + { + throw OutOfRange("deceleration should be a positive value."); + } + m_deceleration = std::make_shared(deceleration()); + } + + std::shared_ptr MotorDynamics::getDeceleration() const + { + return m_deceleration; + } + + MotorDynamics::DType MotorDynamics::getDType() const + { + + DType d_type = static_cast(static_cast(static_cast(m_speed)) << 3 | static_cast(static_cast(m_acceleration)) << 2 | static_cast(static_cast(m_deceleration)) << 1 | static_cast(static_cast(m_distance))); + // std::cout << "D type debugging: " << std::hex << (static_cast(static_cast(m_speed)) << 3 | static_cast(static_cast(m_acceleration)) << 2 | static_cast(static_cast(m_deceleration)) << 1 | static_cast(static_cast(m_distance))) << std::endl; + switch (d_type) + { + case DType::kNone: + case DType::kSpeed: + case DType::kSpeedAccel: + case DType::kSpeedAccelDist: + case DType::kSpeedDist: + case DType::kSpeedAccelDecelDist: + break; + default: + throw UnknownDType(); + } + + return d_type; + } + } +} diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp new file mode 100644 index 0000000..7abbe4d --- /dev/null +++ b/core/motor_controllers/src/roboclaw.cpp @@ -0,0 +1,1058 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ + +#include "motor_controllers/roboclaw/roboclaw.hpp" + +#include "motor_controllers/exceptions.hpp" +#include + +namespace rip +{ + namespace motorcontrollers + { + namespace roboclaw + { + Roboclaw::Roboclaw(const nlohmann::json& config, bool test) + : Subsystem("") + { + /* + Initialization of roboclaw members variables + which are extracted from the json + */ + if (test) + { + validateConfig(config); + } + else + { + setName(config["name"]); + //required parameters + m_address = config.at("address").get(); + m_timeout = config.at("timeout"); + m_ticks_per_rev = config.at("ticks_per_rev"); + m_wheel_radius = config.at("wheel_radius"); + //serial + std::string temp = config.at("device"); + m_device = temp.c_str(); + m_baudrate = config.at("baudrate"); + if (config.find("advanced serial options") != config.end()) + { + m_databits = config.at("databits"); + m_stopbits = config.at("stopbits"); + m_xonxoff = config.at("xonxoff"); + m_rtscts = config.at("rtscts"); + m_parity = config.at("parity"); + } + } + if (config.find("faking") == config.end()) + { + if (config.find("advanced serial options") != config.end()) + { + open(&m_serial, m_device, m_baudrate, m_databits, m_parity, + m_stopbits, m_xonxoff, m_rtscts); + } + else + { + open(&m_serial, m_device, m_baudrate); + } + } + else + { + m_faking = true; + } + } + Roboclaw::~Roboclaw() + { + if(!m_faking) + { + stop(); + serial_close(&m_serial); + } + } + +////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////// PWM ///////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + + void Roboclaw::drive(Motor motor, int16_t speed) + { + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kM1Duty; + break; + case Motor::kM2: + cmd = Command::kM2Duty; + break; + default: + assert(false); + } + + writeN(cmd, speed); + + } + + void Roboclaw::drive(int16_t speed) + { + writeN(Command::kMixedDuty, speed, speed); + } + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////// Main Battery ////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + + void Roboclaw::setMainVoltages(const units::Voltage& min, const units::Voltage& max) + { + if (min.to(units::V) < 6 || min.to(units::V) > 34) + { + throw OutOfRange(fmt::format("Minimum main battery voltage of {} is out of the 6V-34V range", min.to(units::V))); + } + + if (max.to(units::V) < 6 || max.to(units::V) > 34) + { + throw OutOfRange(fmt::format("Maximum main battery voltage of {} is out of the 6V-34V range", max.to(units::V))); + } + if (min() >= max()) + { + throw OutOfRange(fmt::format("Minimum main battery voltage of {} should be less than the maximum main battery voltage of {}.", min.to(units::V), max.to(units::V))); + } + + uint16_t min_v = min.to(units::V) * 10; + uint16_t max_v = max.to(units::V) * 10; + writeN(Command::kSetMainVoltages, min_v, max_v); + } + + units::Voltage Roboclaw::readMainBatteryVoltage() + { + std::vector response = readN(2, Command::kGetMBatt); + uint16_t v = static_cast((response[0] << 8) + response[1]); + return static_cast(v) / 10.0 * units::V; + } + + std::array Roboclaw::readMinMaxMainVoltages() + { + std::array rv; + std::vector response = readN(4, Command::kGetMinMaxMainVoltages); + for (uint8_t i = 0; i < 2; i++) + { + rv[1] += static_cast(static_cast(response[2 + i]) << (8 * (1 - i))); + rv[0] += static_cast(static_cast(response[i]) << (8 * (1 - i))); + } + rv[0] = rv[0]() / 10.0 * units::V; + rv[1] = rv[1]() / 10.0 * units::V; + return rv; + } + +////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////// Logic Battery ////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + + void Roboclaw::setLogicVoltages(const units::Voltage& min, const units::Voltage& max) + { + if (min.to(units::V) < 6 || min.to(units::V) > 34) + { + throw OutOfRange(fmt::format("Minimum logic battery voltage of {} is out of the 6V-34V range", min.to(units::V))); + } + + if (max.to(units::V) < 6 || max.to(units::V) > 34) + { + // throw OutOfRange(fmt::format("Maximum logic battery voltage of {} is out of the 6V-34V range", max.to(units::V))); + } + if (min() >= max()) + { + throw OutOfRange(fmt::format("Minimum logic battery voltage of {} should be less than the maximum logic battery voltage of {}.", min.to(units::V), max.to(units::V))); + } + uint16_t min_v = min.to(units::V) * 10; + uint16_t max_v = max.to(units::V) * 10; + writeN(Command::kSetLogicVoltages, min_v, max_v); + } + + units::Voltage Roboclaw::readLogicBatteryVoltage() + { + std::vector response = readN(2, Command::kGetLBatt); + uint16_t v = static_cast((response[0] << 8) + response[1]); + return static_cast(v) / 10.0 * units::V; + } + + std::array Roboclaw::readMinMaxLogicVoltages() + { + std::array rv; + std::vector response = readN(4, Command::kGetMinMaxLogicVoltages); + for (uint8_t i = 0; i < 2; i++) + { + rv[0] += response[2 + i] << (8 * (1 - i)); + rv[1] += response[i] << (8 * (1 - i)); + } + rv[0] = rv[0]() / 10.0 * units::V; + rv[1] = rv[1]() / 10.0 * units::V; + return rv; + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////// Encoders //////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////////// + + int32_t Roboclaw::readEncoderRaw(Motor motor) + { + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kGetM1Enc; + break; + case Motor::kM2: + cmd = Command::kGetM2Enc; + break; + default: + assert(false); + } + std::vector response = readN(5, cmd); + + int32_t rv = 0; + + // First 4 bytes are the ticks per second + rv = (static_cast((response[0] << 8 * 3) + (response[1] << 8 * 2) + (response[2] << 8) + response[3])); + + return rv; + } + + std::array Roboclaw::readEncodersRaw() + { + std::vector response = readN(8, Command::kGetEncoders); + std::array rv; + rv[0] = static_cast((response[0] << 8 * 3) + (response[1] << 8 * 2) + (response[2] << 8) + response[3]); + rv[1] = static_cast((response[4] << 8 * 3) + (response[5] << 8 * 2) + (response[6] << 8) + response[7]); + return rv; + } + + units::Distance Roboclaw::readEncoder(Motor motor) + { + int32_t ticks = readEncoderRaw(motor); + return (static_cast(ticks) / m_ticks_per_rev) * m_wheel_radius() * (M_PI * 2); + } + + std::array Roboclaw::readEncoders() + { + std::array ticks; + ticks[0] = readEncoder(Motor::kM1); + ticks[1] = readEncoder(Motor::kM2); + return ticks; + } + /* + Currently broken, sign conversions + */ + void Roboclaw::setEncoderRaw(Motor motor, int ticks) + { + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kSetM1EncCount; + break; + case Motor::kM2: + cmd = Command::kSetM2EncCount; + break; + default: + assert(false); + } + writeN(cmd, ticks); + + } + + void Roboclaw::setEncoder(Motor motor, const units::Distance& d) + { + setEncoderRaw(motor, d.to(units::mm) * m_ticks_per_rev / m_wheel_radius.to(units::mm) / (M_PI * 2)); + } + + int32_t Roboclaw::readEncoderVelocityRaw(Motor motor) + { + // Pair of the count which has a range of 0 to 4,294,967,295 and status + std::vector response; + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kGetM1Speed; + break; + case Motor::kM2: + cmd = Command::kGetM2Speed; + break; + default: + assert(false); + } + + + response = readN(5, cmd); + + int32_t rv; + // First 4 bytes are the ticks per second + rv = (static_cast((response[0] << 8 * 3) + (response[1] << 8 * 2) + (response[2] << 8) + response[3])); + + return rv; + } + + units::Velocity Roboclaw::readEncoderVelocity(Motor motor) + { + return static_cast(readEncoderVelocityRaw(motor)) / m_ticks_per_rev * m_wheel_radius() * (M_PI * 2); + } + + std::array Roboclaw::readEncodersVelocityRaw() + { + std::vector response = readN(8, Command::kGetISpeeds); + std::array rv; + rv[0] = static_cast((response[0] << 8 * 3) + (response[1] << 8 * 2) + (response[2] << 8) + response[3]); + rv[1] = static_cast((response[4] << 8 * 3) + (response[5] << 8 * 2) + (response[6] << 8) + response[7]); + return rv; + } + + std::array Roboclaw::readEncodersVelocity() + { + std::array ticks = readEncodersVelocityRaw(); + std::array rv; + rv[0] = ticks[0] / m_ticks_per_rev * m_wheel_radius() * (M_PI * 2); + rv[1] = ticks[1] / m_ticks_per_rev * m_wheel_radius() * (M_PI * 2); + return rv; + } + + void Roboclaw::resetEncoders() + { + writeN(Command::kResetEnc); + } + +////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////// Version ///////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + + std::string Roboclaw::readVersion() + { + uint8_t data; + std::string version; + + std::vector command = {m_address, static_cast(Command::kGetVersion)}; + + for (uint8_t try_ = 0; try_ < kMaxRetries; try_++) + { + data = 0; + + crcClear(); + + for (uint8_t i = 0; i < command.size(); i++) + { + crcUpdate(command[i]); + } + write(&m_serial, command, command.size()); + + for (uint8_t i = 0; i < 48; i++) + { + if (data != -1) + { + data = read(&m_serial, m_timeout); + version += data; + crcUpdate(version[i]); + if (version[i] == 0) + { + uint16_t ccrc; + data = read(&m_serial, m_timeout); + if (data != -1) + { + ccrc = static_cast(data) << 8; + data = read(&m_serial, m_timeout); + + if (data != -1) + { + ccrc |= data; + if (crcGet() == ccrc) + { + return version; + } + else + { + throw CommandFailure(); + } + } + } + break; + } + } + else + { + break; + } + } + } + + throw CommandFailure(); + } + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////// PID /////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + + void Roboclaw::setVelocityPID(Motor motor, const VelocityPIDParameters& parameters) + { + uint32_t kp = parameters.kp * 65536; + uint32_t ki = parameters.ki * 65536; + uint32_t kd = parameters.kd * 65536; + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kSetM1PID; + break; + case Motor::kM2: + cmd = Command::kSetM2PID; + break; + default: + assert(false); + } + writeN(cmd, kd, kp, ki, parameters.qpps); + } + + VelocityPIDParameters Roboclaw::readVelocityPID(Motor motor) + { + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kReadM1PID; + break; + case Motor::kM2: + cmd = Command::kReadM2PID; + break; + default: + assert(false); + } + + std::vector response = readN(16, cmd); + + VelocityPIDParameters v; + uint32_t p = 0, i = 0, d = 0; + v.qpps = 0; + for (uint8_t index = 0; index < 4; index++) + { + p |= response[index] << (8 * (3 - index)); + i |= response[4 + index] << (8 * (3 - index)); + d |= response[8 + index] << (8 * (3 - index)); + v.qpps |= response[12 + index] << (8 * (3 - index)); + } + v.kp = static_cast(p) / 65536; + v.ki = static_cast(i) / 65536; + v.kd = static_cast(d) / 65536; + + return v; + } + + void Roboclaw::setPositionPID(Motor motor, const PositionPIDParameters& parameters) + { + uint32_t kp = parameters.kp * 1024; + uint32_t ki = parameters.ki * 1024; + uint32_t kd = parameters.kd * 1024; + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kSetM1PosPID; + break; + case Motor::kM2: + cmd = Command::kSetM2PosPID; + break; + default: + assert(false); + } + + writeN(cmd, kd, kp, ki, parameters.kiMax, parameters.deadzone, + parameters.min, parameters.max); + } + + PositionPIDParameters Roboclaw::readPositionPID(Motor motor) + { + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kReadM1PosPID; + break; + case Motor::kM2: + cmd = Command::kReadM2PosPID; + break; + default: + assert(false); + } + + std::vector response = readN(28, cmd); + PositionPIDParameters pid; + uint32_t p = 0, i = 0, d = 0; + for (uint8_t index = 0; index < 4; index++) + { + p |= response[index] << (8 * (3 - index)); + i |= response[4 + index] << (8 * (3 - index)); + d |= response[8 + index] << (8 * (3 - index)); + pid.kiMax |= response[12 + index] << (8 * (3 - index)); + pid.deadzone |= response[16 + index] << (8 * (3 - index)); + pid.min |= response[20 + index] << (8 * (3 - index)); + pid.max |= response[24 + index] << (8 * (3 - index)); + } + pid.kp = static_cast(p) / 1024; + pid.ki = static_cast(i) / 1024; + pid.kd = static_cast(d) / 1024; + return pid; + } + + void Roboclaw::setDynamics(Motor motor, const MotorDynamics& dynamics, bool respectBuffer) + { + Command cmd; + int32_t speed; + uint32_t accel, dist, decel; + + switch (dynamics.getDType()) + { + case MotorDynamics::DType::kNone: + return; + case MotorDynamics::DType::kSpeed: + // Send: [Address, 35 or 36, Speed(4 Bytes), CRC(2 bytes)] + // Receive: [0xFF] + switch (motor) + { + case Motor::kM1: + cmd = Command::kM1Speed; //!< 35 + break; + case Motor::kM2: + cmd = Command::kM2Speed; //!< 36 + break; + default: + assert(false); + } + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + writeN(cmd, speed); + return; + case MotorDynamics::DType::kSpeedAccel: + // Send: [Address, 38 or 39, Accel(4 Bytes), Speed(4 Bytes), CRC(2 bytes)] + // Receive: [0xFF] + switch (motor) + { + case Motor::kM1: + cmd = Command::kM1SpeedAccel; //!< 38 + break; + case Motor::kM2: + cmd = Command::kM2SpeedAccel; //!< 39 + break; + default: + assert(false); + } + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / (m_wheel_radius * M_PI * 2)).to(1 / (units::s * units::s)) * m_ticks_per_rev); + writeN(cmd, accel, speed); + return; + case MotorDynamics::DType::kSpeedDist: + // Send: [Address, 41 or 42, Speed(4 Bytes), Distance(4 Bytes), Buffer, CRC(2 bytes)] + // Receive: [0xFF] + switch (motor) + { + case Motor::kM1: + cmd = Command::kM1SpeedDist; //!< 41 + break; + case Motor::kM2: + cmd = Command::kM2SpeedDist; //!< 42 + break; + default: + assert(false); + } + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / (m_wheel_radius * M_PI * 2)).to(units::none) * m_ticks_per_rev); + // std::cout << "Debugging: dist raw value " << dist << std::endl; + writeN(cmd, speed, dist, static_cast(respectBuffer)); + return; + case MotorDynamics::DType::kSpeedAccelDist: + // Send: [Address, 44 or 45, Accel(4 bytes), Speed(4 Bytes), Distance(4 Bytes), Buffer, CRC(2 bytes)] + // Receive: [0xFF] + switch (motor) + { + case Motor::kM1: + cmd = Command::kM1SpeedAccelDist; //!< 44 + break; + case Motor::kM2: + cmd = Command::kM2SpeedAccelDist; //!< 45 + break;default: + assert(false); + } + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + + writeN(cmd, accel, speed, dist, static_cast(respectBuffer)); + return; + case MotorDynamics::DType::kSpeedAccelDecelDist: + // Send: [Address, 65, Accel(4 bytes), Speed(4 Bytes), Deccel(4 bytes), Position(4 Bytes), Buffer, CRC(2 bytes)] + // Receive: [0xFF] + switch (motor) + { + case Motor::kM1: + cmd = Command::kM1SpeedAccelDeccelPos; //!< 44 + break; + case Motor::kM2: + cmd = Command::kM2SpeedAccelDeccelPos; //!< 45 + break; + default: + assert(false); + } + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + decel = static_cast((*dynamics.getDeceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + writeN(cmd, accel, speed, decel, dist, static_cast(respectBuffer)); + return; + default: + assert(false); + } + } + + void Roboclaw::setDynamics(const MotorDynamics& dynamics, bool respectBuffer) + { + Command cmd; + int32_t speed; + uint32_t accel, dist, decel; + + switch (dynamics.getDType()) + { + case MotorDynamics::DType::kNone: + return; + case MotorDynamics::DType::kSpeed: + // Send: [Address, 37, Speed(4 Bytes), CRC(2 bytes)] + // Receive: [0xFF] + cmd = Command::kMixedSpeed; + + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + // std::cout << "debugging: " << speed << std::endl; + writeN(cmd, speed, speed); + return; + case MotorDynamics::DType::kSpeedAccel: + // Send: [Address, 40, Accel(4 Bytes), Speed(4 Bytes), CRC(2 bytes)] + // Receive: [0xFF] + cmd = Command::kMixedSpeedAccel; + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + writeN(cmd, accel, speed, speed); + return; + case MotorDynamics::DType::kSpeedDist: + // Send: [Address, 43, Speed(4 Bytes), Distance(4 Bytes), Buffer, CRC(2 bytes)] + // Receive: [0xFF] + cmd = Command::kMixedSpeedDist; + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + // std::cout << "Debugging: dist raw value " << dist << std::endl; + writeN(cmd, speed, dist, speed, dist, static_cast(respectBuffer)); + return; + case MotorDynamics::DType::kSpeedAccelDist: + // Send: [Address, 46, Accel(4 bytes), Speed(4 Bytes), Distance(4 Bytes), Buffer, CRC(2 bytes)] + // Receive: [0xFF] + cmd = Command::kMixedSpeedAccelDist; + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + + writeN(cmd, accel, speed, dist, speed, dist, static_cast(respectBuffer)); + return; + case MotorDynamics::DType::kSpeedAccelDecelDist: + // Send: [Address, 67, Accel(4 bytes), Speed(4 Bytes), Deccel(4 bytes), Position(4 Bytes), Buffer, CRC(2 bytes)] + // Receive: [0xFF] + cmd = Command::kMixedSpeedAccelDeccelPos; + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + decel = static_cast((*dynamics.getDeceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + writeN(cmd, accel, speed, decel, dist, accel, speed, decel, dist, static_cast(respectBuffer)); + return; + default: + assert(false); + } + } +////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////// Communication ////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + std::vector Roboclaw::readN(uint8_t n, Command cmd) + { + uint8_t crc; + + uint8_t value = 0; + uint8_t trys = kMaxRetries; + int16_t data; + + std::vector command = {m_address, static_cast(cmd)}; + for (uint8_t try_ = 0; try_ < kMaxRetries; try_++) + { + crcClear(); + for (uint8_t i = 0; i < command.size(); i++) + { + crcUpdate(command[i]); + } + + write(&m_serial, command, command.size()); + + std::vector response; + uint8_t data; + for (uint8_t i = 0; i < n; i++) + { + data = read(&m_serial, m_timeout); + crcUpdate(data); + response.push_back(data); + if (data == -1) + { + continue; + } + } + + if (data != -1) + { + uint16_t ccrc; + data = read(&m_serial, m_timeout); + if (data != -1) + { + ccrc = static_cast(data) << 8; + data = read(&m_serial, m_timeout); + if (data != -1) + { + ccrc |= data; + if (crcGet() == ccrc) + { + return response; + } + } + } + } + } + + throw ReadFailure(); + } + + void Roboclaw::write(serial_t* serial, std::vector command, size_t len) + { + if (serial_write(serial, &command[0], len) < 0) + { + throw CommandFailure(serial_errmsg(serial)); + } + } + + uint8_t Roboclaw::read(serial_t* serial, units::Time timeout) + { + uint8_t data; + if (serial_read(serial, &data, 1, static_cast(timeout())) < 0) + { + throw ReadFailure(serial_errmsg(serial)); + } + return data; + } + + void Roboclaw::open(serial_t* serial, std::string device, + uint32_t baudrate, unsigned int databits, + serial_parity_t parity, unsigned int stopbits, + bool xonxoff, bool rtscts) + { + if (serial_open_advanced(serial, device.c_str(), baudrate, databits, parity, + stopbits, xonxoff, rtscts) < 0) + { + throw SerialOpenFail(serial_errmsg(serial)); + } + } + + void Roboclaw::open(serial_t* serial, std::string device, uint32_t baudrate) + { + if (serial_open(serial, device.c_str(), baudrate) < 0) + { + throw SerialOpenFail(serial_errmsg(serial)); + } + } + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////// Current /////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + std::array Roboclaw::readCurrents() + { + std::array rv; + std::vector response = readN(4, Command::kGetCurrents); + for (uint8_t i = 0; i < 2; i++) + { + rv[0] += response[2 + i] << (8 * (1 - i)); + rv[1] += response[i] << (8 * (1 - i)); + } + rv[0] = rv[0]() / 100.0 * units::A; + rv[1] = rv[1]() / 100.0 * units::A; + return rv; + } + + units::Current Roboclaw::readCurrent(Motor motor) + { + std::array rv = Roboclaw::readCurrents(); + switch (motor) + { + case Motor::kM1: + return rv[0]; + case Motor::kM2: + return rv[1]; + default: + assert(false); + } + } + + void Roboclaw::setMaxCurrent(Motor motor, const units::Current& current) + { + uint32_t value = current.to(units::A) * 100; // Convert to 10 mA + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kSetM1MaxCurrent; + break; + case Motor::kM2: + cmd = Command::kSetM2MaxCurrent; + break; + default: + assert(false); + } + writeN(cmd, value, 0); // Second value is a regular int (4 bytes) '0' + } + + units::Current Roboclaw::readMaxCurrent(Motor motor) + { + Command cmd; + switch (motor) + { + case Motor::kM1: + cmd = Command::kGetM1MaxCurrent; + break; + case Motor::kM2: + cmd = Command::kGetM2MaxCurrent; + break; + default: + assert(false); + } + std::vector response = readN(4, cmd); + // last 2 bytes are 0 because the minimum current is always 0 + return static_cast(static_cast((response[0] << 8 * 3) + (response[1] << 8 * 2) + (response[2] << 8) + response[3])) / 100.0 * units::A; + + } +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////// Status //////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + bool Roboclaw::readStatus(Roboclaw::Status s) + { + std::vector response = readN(2, Command::kGetError); + uint16_t value = static_cast((response[0] << 8) + response[1]); + uint16_t status = static_cast(s); + + //ASSUMPTION MADE: If there is any error, status:normal is false. + if (status == 0) + { + return !value; + } + + return (value & status) == status; + } + + std::array Roboclaw::readStatus() + { + //ASSUMPTION MADE: If there is any error, status:normal is false. + std::vector response = readN(2, Command::kGetError); + uint16_t value = static_cast((response[0] << 8) + response[1]); + std::array status; + uint16_t mask = 1; + if (value == 0) + { + //status normal + status[0] = true; + for (int i = 1; i < 17; i++) + { + status[i] = false; + } + return status; + } + else + { + status[0] = false; + for (int i = 1; i < 17; i++) + { + if ((value & mask) == mask) + { + status[i] = true; + } + else + { + status[i] = false; + } + mask = (mask << 1); + + } + return status; + } + } + + units::Temperature Roboclaw::readTemperature() + { + std::vector response = readN(2, Command::kGetTemp); + uint16_t value = static_cast((response[0] << 8) + response[1]); + return static_cast(value) / 10.0 * units::degC; //todo(Andrew): Figure out the units + } + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////// Misc ////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + + void Roboclaw::crcClear() + { + m_crc = 0; + } + + uint8_t Roboclaw::returnFF() + { + return 0xFF; + } + + void Roboclaw::setConfig(const Config& config) + { + writeN(Command::kSetConfig, config.get()); + } + + Config Roboclaw::getConfig() + { + Config config; + std::vector response = readN(2, Command::kGetConfig); + config.set(static_cast((response[0] << 8) + response[1])); + return config; + } + + void Roboclaw::stop() + { + drive(0); + } + + bool Roboclaw::diagnostic() + { + // todo + } + + void Roboclaw::crcUpdate(uint8_t data) + { + m_crc ^= (static_cast(data) << 8); + for (uint8_t i = 0; i < 8; i++) + { + if (m_crc & 0x8000) + { + m_crc = (m_crc << 1) ^ 0x1021; + } + else + { + m_crc <<= 1; + } + } + } + + uint16_t Roboclaw::crcGet() + { + return m_crc; + } + + std::vector Roboclaw::readBufferLens() + { + /* + Max number is 64. 128 if the buffer is empty. + 0 if the last command is being executed. + */ + std::vector response = readN(2, Command::kGetBuffers); + return response; + } + uint8_t Roboclaw::readBufferLen(Motor motor) + { + /* + Max number is 64. 128 if the buffer is empty. + 0 if the last command is being executed. + */ + std::vector response = readN(2, Command::kGetBuffers); + switch (motor) + { + case Motor::kM1: + return response[0]; + case Motor::kM2: + return response[1]; + default: + assert(false); + } + } + + void Roboclaw::validateConfig(const nlohmann::json& testcfg) + { + std::string vars[] = {"name", "address", "timeout", "ticks_per_rev", "wheel_radius", "baudrate", "device"}; + if (testcfg.empty()) + { + throw BadJson("JSON file was empty"); + } + for (int i = 0; i < 7; i++) + { + if (testcfg.find(vars[i]) == testcfg.end()) + { + throw BadJson(vars[i] + " was not found within json cfg."); + } + } + if (testcfg.size() < 7) + { + throw BadJson("Not enough config values, min size: " + std::to_string(7)); + } + + for (int i = 1; i < 5; i++) + { + if (testcfg[vars[i]] < 0.0) + { + throw OutOfRange(vars[i] + " should be a positive value"); + } + } + if (testcfg["baudrate"] < 0) + { + throw OutOfRange("baudrate should be positive."); + } + if (testcfg["wheel_radius"] == 0) + { + throw OutOfRange("wheel radius cannot = 0"); + } + try + { + setName(testcfg["name"]); + m_address = testcfg.at("address").get(); + m_timeout = testcfg.at("timeout"); + m_ticks_per_rev = testcfg.at("ticks_per_rev"); + m_wheel_radius = testcfg.at("wheel_radius"); + std::string temp = testcfg.at("device"); + m_device = temp.c_str(); + m_baudrate = testcfg.at("baudrate"); + if (testcfg.find("advanced serial options") != testcfg.end()) + { + m_databits = testcfg.at("databits"); + m_stopbits = testcfg.at("stopbits"); + m_xonxoff = testcfg.at("xonxoff"); + m_rtscts = testcfg.at("rtscts"); + m_parity = testcfg.at("parity"); + } + } + catch (...) + { + throw BadJson("Failed to set 1 or more values"); + } + } + } + } +} diff --git a/core/motor_controllers/test/mock_roboclaw.cpp b/core/motor_controllers/test/mock_roboclaw.cpp new file mode 100644 index 0000000..1c7c52a --- /dev/null +++ b/core/motor_controllers/test/mock_roboclaw.cpp @@ -0,0 +1,74 @@ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" + +#include "mock_roboclaw.hpp" + +namespace rip +{ + namespace motorcontrollers + { + namespace roboclaw + { + namespace mocks + { + MockRoboclaw::MockRoboclaw(nlohmann::json config, bool test) : Roboclaw(config, test) + {} + + std::string MockRoboclaw::getLastSent() + { + return m_last_sent; + } + + void MockRoboclaw::setcResponse(const std::vector response) + { + m_cresponse = response; + } + + std::vector MockRoboclaw::readN(uint8_t n, Command cmd) + { + return m_cresponse; + } + + void MockRoboclaw::write(serial_t* m_serial, std::vector command, size_t len) + { + m_last_cmd = command; + } + uint8_t MockRoboclaw::read(serial_t* serial, units::Time timeout_ms) + { + return 0; + } + + void MockRoboclaw::setBytes(size_t bytes) + { + m_bytes = bytes; + } + + uint8_t MockRoboclaw::returnFF() + { + return m_bytes; + } + + std::vector MockRoboclaw::getLastCmd() + { + return m_last_cmd; + } + + //debugging tool for me, will remove eventually + void MockRoboclaw::printResponse() + { + if (m_last_cmd.size() == 0) + { + // std::cout << "vector empty" << std::endl; + return; + } + for (std::vector::const_iterator i = m_last_cmd.begin(); i != m_last_cmd.end(); i++) + { + // std::cout << std::hex << static_cast(*i) << ' '; + } + } + + } + } + } +} +#pragma GCC diagnostic pop diff --git a/core/motor_controllers/test/mock_roboclaw.hpp b/core/motor_controllers/test/mock_roboclaw.hpp new file mode 100644 index 0000000..1f1b9f3 --- /dev/null +++ b/core/motor_controllers/test/mock_roboclaw.hpp @@ -0,0 +1,45 @@ +#ifndef MOCK_ROBOCLAW_HPP +#define MOCK_ROBOCLAW_HPP + +#include "motor_controllers/roboclaw/roboclaw.hpp" + +namespace rip +{ + namespace motorcontrollers + { + namespace roboclaw + { + namespace mocks + { + class MockRoboclaw : public Roboclaw + { + public: + + //MockRoboclaw(); + MockRoboclaw(nlohmann::json config = {{"name", "mock_roboclaw"},{"address", 0x80}, {"timeout", 100 * units::ms}, + {"ticks_per_rev", 360.0}, {"wheel_radius", 4 * units::cm}, {"device", "/dev/ttyS0"}, + {"baudrate", 115200}, {"faking", 1} + }, bool test = 1); + void setcResponse(const std::vector response); + void setResponse(const std::string& response); + uint8_t returnFF() override; + std::vector readN(uint8_t n, Command cmd) override; + //size_t read(uint8_t *buffer, size_t size) override; + void write(serial_t* m_serial, std::vector command, size_t len) override; + uint8_t read(serial_t* serial, units::Time timeout_ms) override; + std::vector getLastCmd(); + std::string getLastSent(); + void printResponse(); + void setBytes(size_t bytes = 0xFF); + private: + std::string m_last_sent; + std::vector m_last_cmd; + std::vector m_cresponse; + std::string m_response; + size_t m_bytes = 0xFF; + }; + } + } + } +} +#endif //MOCK_ROBOCLAW_HPP diff --git a/core/roboclaw/test/test_main.cpp b/core/motor_controllers/test/test_main.cpp similarity index 100% rename from core/roboclaw/test/test_main.cpp rename to core/motor_controllers/test/test_main.cpp diff --git a/core/motor_controllers/test/test_roboclaw.cpp b/core/motor_controllers/test/test_roboclaw.cpp new file mode 100644 index 0000000..e611c1d --- /dev/null +++ b/core/motor_controllers/test/test_roboclaw.cpp @@ -0,0 +1,1368 @@ +#include "mock_roboclaw.hpp" +#include + +#include + +#include + +#define ROBOCLAW_TEST_NEAR_ACCURACY_CM 0.01 // required precision in centimeters + +namespace rip +{ + namespace motorcontrollers + { + namespace roboclaw + { + namespace tests + { + nlohmann::json j; + using Roboclaw = mocks::MockRoboclaw; + + TEST(RoboclawCore, loadJson) + { + j["name"] = "mock_roboclaw"; + j["faking"] = "yep"; + //test some types of variables that are not json obj + ASSERT_THROW(std::make_shared("strayTaco"), BadJson); + ASSERT_THROW(std::make_shared(42), BadJson); + + //test if json null + ASSERT_THROW(std::make_shared(j), BadJson); + units::Time t = 3000.0; + units::Distance d = .1; + j["address"] = 0x80; //uint8_t + j["timeout"] = t.to(units::ms); + j["ticks_per_rev"] = 100.0;//double + j["device"] = "/dev/ttyS0"; + j["baudrate"] = 115200; + + //test for missing entries + ASSERT_THROW(std::make_shared(j), BadJson); + j["wheel_radius"] = "lol"; + //tests for invalid values of entries + ASSERT_THROW(std::make_shared(j), BadJson); + j["wheel_radius"] = d.to(units::cm); + j["timeout"] = -3.0; + + ASSERT_THROW(std::make_shared(j), OutOfRange); + j["timeout"] = 10.0; + j["ticks_per_rev"] = j; + ASSERT_THROW(std::make_shared(j), BadJson); + //test extra entries + j["extra"] = 1; + ASSERT_THROW(std::make_shared(j), BadJson); + j.erase("extra"); + //wheel radius cannot = 0 + j["wheel_radius"] = 0; + ASSERT_THROW(std::make_shared(j), OutOfRange); + //ticks per rev should be a pos. # + j["ticks_per_rev"] = -100.0; + ASSERT_THROW(std::make_shared(j), OutOfRange); + j["timeout"] = t.to(units::ms); + j["ticks_per_rev"] = 100.0; + j["wheel_radius"] = d.to(units::cm); + //given valid parameters, does not throw an exception + std::make_shared(j); + ASSERT_NO_THROW(std::make_shared(j)); + //TODO: Reasonableness checks/nonfatal exceptions + } + + TEST(RoboclawCore, DriveM1Forward) + { + std::shared_ptr testClaw(new Roboclaw); + //assuming valid roboclaw object at this point due to prior test + std::vector response = {0xFF}; + testClaw->setcResponse(response); + //CRC check failure throws command failure + testClaw->setBytes(55); + ASSERT_THROW(testClaw->drive(Roboclaw::Motor::kM1, Roboclaw::kFullSpeedForward), CommandFailure); + testClaw->setBytes(0); + //sends correct message: addr, 32 or 33, duty, CRC + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 32); + //concatenating elements 2 and 3 should give duty + ASSERT_EQ(testClaw->getLastCmd()[2], 0x7f); + ASSERT_EQ(testClaw->getLastCmd()[3], 0xff); + //final test: given proper conditions, does not throw an exception + //correct size of byte array + ASSERT_EQ(testClaw->getLastCmd().size(), 6u); + testClaw->drive(Roboclaw::Motor::kM1, Roboclaw::kFullSpeedForward); + ASSERT_NO_THROW(testClaw->drive(Roboclaw::Motor::kM1, Roboclaw::kFullSpeedForward)); + + } + + TEST(RoboclawCore, DriveM1Backward) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + testClaw->setBytes(55); + ASSERT_THROW(testClaw->drive(Roboclaw::Motor::kM1, Roboclaw::kFullSpeedBackward), CommandFailure); + testClaw->setBytes(0); + + //sends correct message: addr, 32 or 33, duty, CRC + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 32); + //concatenating elements 2 and 3 should give duty + ASSERT_EQ(testClaw->getLastCmd()[2], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[3], 0x1); + //correct size of byte array + ASSERT_EQ(testClaw->getLastCmd().size(), 6u); + //final test: given proper conditions, does not throw an exception + testClaw->drive(Roboclaw::Motor::kM1, Roboclaw::kFullSpeedBackward); + ASSERT_NO_THROW(testClaw->drive(Roboclaw::Motor::kM1, Roboclaw::kFullSpeedBackward)); + + } + + TEST(RoboclawCore, DriveM2Forward) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + + testClaw->setBytes(55); + ASSERT_THROW(testClaw->drive(Roboclaw::Motor::kM2, Roboclaw::kFullSpeedForward), CommandFailure); + testClaw->setBytes(0); + + //sends correct message: addr, 32 or 33, duty, CRC + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 33); + //concatenating elements 2 and 3 should give duty + ASSERT_EQ(testClaw->getLastCmd()[2], 0x7f); + ASSERT_EQ(testClaw->getLastCmd()[3], 0xff); + //correct size of byte array + ASSERT_EQ(testClaw->getLastCmd().size(), 6u); + //final test: given proper conditions, does not throw an exception + ASSERT_NO_THROW(testClaw->drive(Roboclaw::Motor::kM2, Roboclaw::kFullSpeedForward)); + + } + + TEST(RoboclawCore, DriveM2Backward) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + + testClaw->setBytes(55); + ASSERT_THROW(testClaw->drive(Roboclaw::Motor::kM1, Roboclaw::kFullSpeedBackward), CommandFailure); + testClaw->setBytes(0); + + //sends correct message: addr, 32 or 33, duty, CRC + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 32); + //concatenating elements 2 and 3 should give duty + ASSERT_EQ(testClaw->getLastCmd()[2], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[3], 0x1); + //correct size of byte array + ASSERT_EQ(testClaw->getLastCmd().size(), 6u); + //final test: given proper conditions, does not throw an exception + ASSERT_NO_THROW(testClaw->drive(Roboclaw::Motor::kM2, Roboclaw::kFullSpeedBackward)); + + } + TEST(RoboclawCore, DriveForward) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + + testClaw->setBytes(55); + ASSERT_THROW(testClaw->drive(Roboclaw::kFullSpeedForward), CommandFailure); + testClaw->setBytes(0); + + //sends correct message: addr, 34, duty, CRC + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 34); + //concatenating elements 2 and 3 should give duty + ASSERT_EQ(testClaw->getLastCmd()[2], 0x7f); + ASSERT_EQ(testClaw->getLastCmd()[3], 0xff); + ASSERT_EQ(testClaw->getLastCmd()[4], 0x7f); + ASSERT_EQ(testClaw->getLastCmd()[5], 0xff); + //correct size of byte array + ASSERT_EQ(testClaw->getLastCmd().size(), 8u); + //final test: given proper conditions, does not throw an exception + ASSERT_NO_THROW(testClaw->drive(Roboclaw::kFullSpeedForward)); + + } + TEST(RoboclawCore, DriveBackward) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + + testClaw->setBytes(55); + ASSERT_THROW(testClaw->drive(Roboclaw::kFullSpeedBackward), CommandFailure); + testClaw->setBytes(0); + + //sends correct message: addr, 34, duty, CRC + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 34); + //concatenating elements 2 and 3 should give duty + ASSERT_EQ(testClaw->getLastCmd()[2], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[3], 0x1); + ASSERT_EQ(testClaw->getLastCmd()[4], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[5], 0x1); + //correct size of byte array + ASSERT_EQ(testClaw->getLastCmd().size(), 8u); + //final test: given proper conditions, does not throw an exception + ASSERT_NO_THROW(testClaw->drive(Roboclaw::kFullSpeedBackward)); + + } + TEST(RoboclawCore, Battery) + { + /* + Note: Roboclaw treats numbers for voltage in deciVolts, since protocol + does not use floats. Current responses are increments of 10 mA. + */ + std::shared_ptr testClaw(new Roboclaw); + std::vector response = {0, 80}; + units::Voltage v; + units::Current a; + std::array rv; + testClaw->setBytes(0); + //voltage out of range + ASSERT_THROW(std::make_shared(j)->setLogicVoltages(-3 * units::V, 8 * units::V), OutOfRange); + ASSERT_THROW(std::make_shared(j)->setLogicVoltages(6, 300), OutOfRange); + ASSERT_THROW(std::make_shared(j)->setMainVoltages(-3, 8), OutOfRange); + ASSERT_THROW(std::make_shared(j)->setMainVoltages(6, 300), OutOfRange); + //min voltage should be less than max voltage + ASSERT_THROW(std::make_shared(j)->setMainVoltages(18, 6), OutOfRange); + + //set voltage, check to see if that is actual voltage + //verify proper command transmission + testClaw->setLogicVoltages(8 * units::V, 24 * units::V); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 58); + ASSERT_DOUBLE_EQ(static_cast((testClaw->getLastCmd()[2] << 8) + testClaw->getLastCmd()[3]), 80.0); + ASSERT_DOUBLE_EQ(static_cast((testClaw->getLastCmd()[4] << 8) + testClaw->getLastCmd()[5]), 240.0); + testClaw->setcResponse(response); + //initial voltage should be set to minimum voltage + v = testClaw->readLogicBatteryVoltage(); + EXPECT_DOUBLE_EQ(v.to(units::V), 8.0); + //verify proper command transmission + testClaw->setMainVoltages(8 * units::V, 24 * units::V); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 57); + ASSERT_DOUBLE_EQ(static_cast((testClaw->getLastCmd()[2] << 8) + testClaw->getLastCmd()[3]), 80.0); + ASSERT_DOUBLE_EQ(static_cast((testClaw->getLastCmd()[4] << 8) + testClaw->getLastCmd()[5]), 240.0); + + response = {0, 80, 0, 240}; + testClaw->setcResponse(response); + rv = testClaw->readMinMaxMainVoltages(); + EXPECT_DOUBLE_EQ(rv[0].to(units::V), 8.0); + EXPECT_DOUBLE_EQ(rv[1].to(units::V), 24.0); + v = testClaw->readMainBatteryVoltage(); + + //readCurrent returns diff. than actual + //verify proper command transmission + /* + Send: [Address, 134, MaxCurrent(4 bytes), 0, 0, 0, 0, CRC(2 bytes)] + */ + testClaw->setMaxCurrent(Roboclaw::Motor::kM1, 5 * units::A); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], 133); + ASSERT_DOUBLE_EQ(static_cast((testClaw->getLastCmd()[2] << 8 * 3) + (testClaw->getLastCmd()[3] << 8 * 2) + (testClaw->getLastCmd()[4] << 8) + testClaw->getLastCmd()[5]), 500.0); + //min current should always be 0 + ASSERT_DOUBLE_EQ(static_cast((testClaw->getLastCmd()[6] << 8 * 3) + (testClaw->getLastCmd()[7] << 8 * 2) + (testClaw->getLastCmd()[8] << 8) + testClaw->getLastCmd()[9]), 0.0); + //Receive: [MaxCurrent(4 bytes), MinCurrent(4 bytes), CRC(2 bytes)] + response = {0, 0, 1, 0xF4}; + testClaw->setcResponse(response); + ASSERT_LE(testClaw->readCurrent(Roboclaw::Motor::kM1), 5 * units::A); + ASSERT_EQ(testClaw->readMaxCurrent(Roboclaw::Motor::kM1), 5 * units::A); + ASSERT_EQ(testClaw->readCurrent(Roboclaw::Motor::kM1), 5 * units::A); + ASSERT_EQ(testClaw->readCurrents()[0], 5 * units::A); + + + } + TEST(RoboclawCore, Status) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + testClaw->setBytes(0); + std::array status; + //readstatus tests, status normal + response = {0, 0}; + testClaw->setcResponse(response); + status = testClaw->readStatus(); + EXPECT_TRUE(testClaw->readStatus(Roboclaw::Status::kNormal)); + for (int i = 1; i < 17; i++) + { + EXPECT_FALSE(status[i]); + } + //opposite of above case, normal is false but all other statuses true + response = {0xFF, 0xFF}; + testClaw->setcResponse(response); + status = testClaw->readStatus(); + EXPECT_FALSE(testClaw->readStatus(Roboclaw::Status::kNormal)); + for (int i = 1; i < 17; i++) + { + EXPECT_TRUE(status[i]); + } + //m1 and m2 over current warning + response = {0, 3}; + testClaw->setcResponse(response); + EXPECT_FALSE(testClaw->readStatus(Roboclaw::Status::kNormal)); + EXPECT_TRUE(testClaw->readStatus(Roboclaw::Status::kM1OverCurrentWarning)); + EXPECT_TRUE(testClaw->readStatus(Roboclaw::Status::kM2OverCurrentWarning)); + + } + + TEST(RoboclawEncoder, ReadEncoderM1) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + units::Velocity v; + units::Distance d; + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04); + testClaw->setBytes(0); + + //distance tests + //motor 1 + //no movement + response = {0x0, 0x0, 0x0, 0x0, 0x0}; + testClaw->setcResponse(response); + + EXPECT_DOUBLE_EQ(testClaw->readEncoderRaw(Roboclaw::Motor::kM1), 0.0); + d = testClaw->readEncoder(Roboclaw::Motor::kM1); + EXPECT_DOUBLE_EQ(d(), 0.0); + + //no movement (backwards) (could cause trouble during type conversions) + response = {0x0, 0x0, 0x0, 0x0, 2}; + testClaw->setcResponse(response); + + EXPECT_DOUBLE_EQ(testClaw->readEncoderRaw(Roboclaw::Motor::kM1), 0.0); + d = testClaw->readEncoder(Roboclaw::Motor::kM1); + EXPECT_DOUBLE_EQ(d.to(units::cm), 0.0); + + //forward test + response = {0x00, 0x00, 0xDE, 0xAD, 0x0}; + testClaw->setcResponse(response); + + EXPECT_DOUBLE_EQ(testClaw->readEncoderRaw(Roboclaw::Motor::kM1), (int32_t) 0x0000DEAD); + d = testClaw->readEncoder(Roboclaw::Motor::kM1); + EXPECT_NEAR( + d.to(units::cm), + (((1.0 * (int32_t)0x0000DEAD) / ticks_per_rev) * (wheel_radius.to(units::cm) * M_PI * 2)), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::cm) // absolute error allowed + ); + //backwards/negative + + response = {0x00, 0x00, 0xBE, 0xEF, 2}; + testClaw->setcResponse(response); + EXPECT_DOUBLE_EQ(testClaw->readEncoderRaw(Roboclaw::Motor::kM1), (int32_t) 0x0000BEEF); + d = testClaw->readEncoder(Roboclaw::Motor::kM1); + EXPECT_NEAR( + d.to(units::cm), + (((1.0 * (int32_t)0x0000BEEF) / ticks_per_rev) * (wheel_radius.to(units::cm) * M_PI * 2)), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::cm) // absolute error allowed + ); + + } + + TEST(RoboclawEncoder, ReadEncoderM2) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + units::Distance d; + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04); + testClaw->setBytes(0); + + //motor 2 + //no movement + response = {0x0, 0x0, 0x0, 0x0, 0x0}; + testClaw->setcResponse(response); + + EXPECT_DOUBLE_EQ(testClaw->readEncoderRaw(Roboclaw::Motor::kM2), 0.0); + d = testClaw->readEncoder(Roboclaw::Motor::kM2); + EXPECT_DOUBLE_EQ(d(), 0.0); + + //no movement (backwards) (could cause trouble during type conversions) + response = {0x0, 0x0, 0x0, 0x0, 2}; + testClaw->setcResponse(response); + + EXPECT_DOUBLE_EQ(testClaw->readEncoderRaw(Roboclaw::Motor::kM2), 0.0); + d = testClaw->readEncoder(Roboclaw::Motor::kM2); + EXPECT_DOUBLE_EQ(d.to(units::cm), 0.0); + + //forward test + response = {0x00, 0x00, 0xF0, 0x0D, 0x0}; + testClaw->setcResponse(response); + + EXPECT_DOUBLE_EQ(testClaw->readEncoderRaw(Roboclaw::Motor::kM2), (int32_t)0x0000F00D); + d = testClaw->readEncoder(Roboclaw::Motor::kM2); + EXPECT_NEAR( + d.to(units::cm), + (static_cast(0x0000F00D) / ticks_per_rev) * wheel_radius.to(units::cm) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::cm) // absolute error allowed + ); + //backwards/negative + + response = {0x00, 0x00, 0xC0, 0x01, 2}; + testClaw->setcResponse(response); + EXPECT_DOUBLE_EQ(testClaw->readEncoderRaw(Roboclaw::Motor::kM2), (int32_t)0x0000C001); + d = testClaw->readEncoder(Roboclaw::Motor::kM2); + EXPECT_NEAR( + d.to(units::cm), + (static_cast(0x0000C001) / ticks_per_rev) * wheel_radius.to(units::cm) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::cm) // absolute error allowed + ); + + } + + TEST(RoboclawEncoder, ReadEncoderVelocityM1) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + units::Velocity v; + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04);//base unit meter + testClaw->setBytes(0); + //velocity tests + //motor 1 + //still + response = {0x0, 0x0, 0x0, 0x0, 0x0}; + testClaw->setcResponse(response); + EXPECT_DOUBLE_EQ(testClaw->readEncoderVelocityRaw(Roboclaw::Motor::kM1), 0.0); + v = testClaw->readEncoderVelocity(Roboclaw::Motor::kM1); + EXPECT_DOUBLE_EQ(v.to(units::m / units::s), 0.0); + //still(negative) + response = {0x0, 0x0, 0x0, 0x0, 2}; + testClaw->setcResponse(response); + EXPECT_DOUBLE_EQ(testClaw->readEncoderVelocityRaw(Roboclaw::Motor::kM1), 0.0); + v = testClaw->readEncoderVelocity(Roboclaw::Motor::kM1); + EXPECT_DOUBLE_EQ(v.to(units::m / units::s), 0.0); + + //Forward + response = {0x00, 0x00, 0xCA, 0xFE, 0x0}; + testClaw->setcResponse(response); + EXPECT_DOUBLE_EQ(testClaw->readEncoderVelocityRaw(Roboclaw::Motor::kM1), (int32_t)0xCAFE); + v = testClaw->readEncoderVelocity(Roboclaw::Motor::kM1); + EXPECT_NEAR( + v.to(units::m / units::s), + (static_cast(0xCAFE) / ticks_per_rev) * wheel_radius.to(units::m) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::m) + ); + //backward + response = {0xFF, 0xFF, 0xF1, 0x00, 0x2}; // negative values (twos complement) + testClaw->setcResponse(response); + v = testClaw->readEncoderVelocity(Roboclaw::Motor::kM1); + EXPECT_DOUBLE_EQ(testClaw->readEncoderVelocityRaw(Roboclaw::Motor::kM1), (int32_t) - 0x0F00); + EXPECT_NEAR( + v.to(units::m / units::s), + (static_cast(-0x0F00) / ticks_per_rev) * wheel_radius.to(units::m) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::m) + ); + + + } + + TEST(RoboclawEncoder, ReadEncoderVelocityM2) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + units::Velocity v; + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04);//base unit meter + testClaw->setBytes(0); + + //motor 2 + //still + response = {0x0, 0x0, 0x0, 0x0, 0x0}; + testClaw->setcResponse(response); + EXPECT_DOUBLE_EQ(testClaw->readEncoderVelocityRaw(Roboclaw::Motor::kM2), 0.0); + v = testClaw->readEncoderVelocity(Roboclaw::Motor::kM2); + EXPECT_DOUBLE_EQ(v.to(units::m / units::s), 0.0); + //still(negative) + response = {0x0, 0x0, 0x0, 0x0, 2}; + testClaw->setcResponse(response); + EXPECT_DOUBLE_EQ(testClaw->readEncoderVelocityRaw(Roboclaw::Motor::kM2), 0.0); + v = testClaw->readEncoderVelocity(Roboclaw::Motor::kM2); + EXPECT_DOUBLE_EQ(v.to(units::m / units::s), 0.0); + + //Forward + response = {0x0, 0x0, 0xFF, 0xFF, 0x0}; + testClaw->setcResponse(response); + EXPECT_DOUBLE_EQ(testClaw->readEncoderVelocityRaw(Roboclaw::Motor::kM2), 0xFFFF); + v = testClaw->readEncoderVelocity(Roboclaw::Motor::kM2); + EXPECT_NEAR( + v.to(units::m / units::s), + (static_cast(0xFFFF) / ticks_per_rev) * wheel_radius.to(units::m) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::m) + ); + //backward + response = {0xFF, 0xFF, 0xF3, 0x00, 0x2}; // twos complement for negatives + testClaw->setcResponse(response); + v = testClaw->readEncoderVelocity(Roboclaw::Motor::kM2); + EXPECT_DOUBLE_EQ(testClaw->readEncoderVelocityRaw(Roboclaw::Motor::kM2), -0x0D00); + EXPECT_NEAR( + v.to(units::m / units::s), + (static_cast(-0x0D00) / ticks_per_rev) * wheel_radius.to(units::m) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::m) + ); + + } + + TEST(RoboclawEncoder, PluralStationary) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + units::Velocity v; + units::Distance d, d2; + units::Length wheel_radius(units::m * 0.04); + testClaw->setBytes(0); + + //stationary, + + response = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; + testClaw->setcResponse(response); + + d = testClaw->readEncoder(Roboclaw::Motor::kM1); + d2 = testClaw->readEncoders()[0]; + EXPECT_DOUBLE_EQ(d2.to(units::cm), d.to(units::cm)); + d = testClaw->readEncoder(Roboclaw::Motor::kM2); + d2 = testClaw->readEncoders()[1]; + EXPECT_DOUBLE_EQ(d2.to(units::cm), d.to(units::cm)); + + EXPECT_DOUBLE_EQ(testClaw->readEncodersRaw()[0], 0.0); + EXPECT_DOUBLE_EQ(testClaw->readEncodersRaw()[1], 0.0); + v = testClaw->readEncodersVelocity()[0]; + EXPECT_DOUBLE_EQ(v.to(units::cm / units::s), 0.0); + v = testClaw->readEncodersVelocity()[1]; + EXPECT_DOUBLE_EQ(v.to(units::cm / units::s), 0.0); + + + + //stationary, - + response = {0x0, 0x0, 0x0, 0x0, 0, 0 , 0, 0, 0x2, 0x2}; + testClaw->setcResponse(response); + + d = testClaw->readEncoder(Roboclaw::Motor::kM1); + d2 = testClaw->readEncoders()[0]; + EXPECT_DOUBLE_EQ(d2.to(units::cm), d.to(units::cm)); + + d = testClaw->readEncoder(Roboclaw::Motor::kM2); + d2 = testClaw->readEncoders()[1]; + EXPECT_DOUBLE_EQ(d2.to(units::cm), d.to(units::cm)); + + EXPECT_DOUBLE_EQ(testClaw->readEncodersRaw()[0], 0.0); + EXPECT_DOUBLE_EQ(testClaw->readEncodersRaw()[1], 0.0); + + EXPECT_DOUBLE_EQ(testClaw->readEncodersVelocityRaw()[0], 0.0); + EXPECT_DOUBLE_EQ(testClaw->readEncodersVelocityRaw()[1], 0.0); + + v = testClaw->readEncodersVelocity()[0]; + EXPECT_DOUBLE_EQ(v.to(units::cm / units::s), 0.0); + v = testClaw->readEncodersVelocity()[1]; + EXPECT_DOUBLE_EQ(v.to(units::cm / units::s), 0.0); + + } + + TEST(RoboclawEncoder, PluralForward) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + units::Velocity v; + units::Distance d, d2; + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04); + testClaw->setBytes(0); + + //Forward + response = {0x0, 0x0, 0xAB, 0x1, 0x0, 0x0, 0xAB, 0x1, 0x0, 0x0}; + testClaw->setcResponse(response); + d = testClaw->readEncoder(Roboclaw::Motor::kM1); + d2 = testClaw->readEncoders()[0]; + EXPECT_DOUBLE_EQ(d2(), d()); + d = testClaw->readEncoder(Roboclaw::Motor::kM2); + d2 = testClaw->readEncoders()[1]; + EXPECT_DOUBLE_EQ(d2(), d()); + + EXPECT_DOUBLE_EQ(testClaw->readEncodersRaw()[0], 0xAB01); + EXPECT_DOUBLE_EQ(testClaw->readEncodersRaw()[1], 0xAB01); + + EXPECT_DOUBLE_EQ(testClaw->readEncodersVelocityRaw()[0], 0xAB01); + EXPECT_DOUBLE_EQ(testClaw->readEncodersVelocityRaw()[1], 0xAB01); + + v = testClaw->readEncodersVelocity()[0]; + EXPECT_NEAR( + v.to(units::m / units::s), + static_cast(0xAB01) / ticks_per_rev * wheel_radius.to(units::m) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::m) + ); + v = testClaw->readEncodersVelocity()[1]; + EXPECT_NEAR( + v.to(units::m / units::s), + static_cast(0xAB01) / ticks_per_rev * wheel_radius.to(units::m) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::m) + ); + + } + + TEST(RoboclawEncoder, PluralBackward) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + units::Velocity v; + units::Distance d, d2; + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04); + testClaw->setBytes(0); + + //backwards + response = + { + 0xFF, 0xFF, 0x54, 0xFF, // this one negative + 0x00, 0x00, 0xAB, 0x01, + 0x02, 0x02 + }; + testClaw->setcResponse(response); + + d = testClaw->readEncoder(Roboclaw::Motor::kM1); + d2 = testClaw->readEncoders()[0]; + EXPECT_DOUBLE_EQ(d2.to(units::cm), d.to(units::cm)); + d = testClaw->readEncoder(Roboclaw::Motor::kM2); + d2 = testClaw->readEncoders()[1]; + EXPECT_DOUBLE_EQ(d2.to(units::cm), d.to(units::cm)); + + EXPECT_DOUBLE_EQ(testClaw->readEncodersRaw()[0], -43777); + EXPECT_DOUBLE_EQ(testClaw->readEncodersRaw()[1], 43777); + + EXPECT_DOUBLE_EQ(testClaw->readEncodersVelocityRaw()[0], -43777); + EXPECT_DOUBLE_EQ(testClaw->readEncodersVelocityRaw()[1], 43777); + + v = testClaw->readEncodersVelocity()[0]; + EXPECT_NEAR( + v.to(units::m / units::s), + static_cast(-43777) / ticks_per_rev * wheel_radius.to(units::m) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::m) + ); + v = testClaw->readEncodersVelocity()[1]; + EXPECT_NEAR( + v.to(units::m / units::s), + static_cast(43777) / ticks_per_rev * wheel_radius.to(units::m) * (M_PI * 2), + units::Length(units::cm * ROBOCLAW_TEST_NEAR_ACCURACY_CM).to(units::m) + ); + + } + + TEST(RoboclawEncoder, setTicksRaw) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + units::Velocity v; + units::Distance d, d2; + units::Length wheel_radius(units::m * 0.04); + int ticks, ticksc; + testClaw->setBytes(0); + + //ensure proper transmission of setters + ticks = 0xABCD; + testClaw->setEncoderRaw(Roboclaw::Motor::kM1, ticks); + response = testClaw->getLastCmd(); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], static_cast(Roboclaw::Command::kSetM1EncCount)); + ticksc = static_cast((response[2] << 8 * 3) + (response[3] << 8 * 2) + (response[4] << 8) + response[5]); + ASSERT_EQ(ticks, ticksc); + //positive + testClaw->setEncoderRaw(Roboclaw::Motor::kM2, ticks); + response = testClaw->getLastCmd(); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], static_cast(Roboclaw::Command::kSetM2EncCount)); + ticksc = static_cast((response[2] << 8 * 3) + (response[3] << 8 * 2) + (response[4] << 8) + response[5]); + ASSERT_EQ(ticks, ticksc); + //negative ticks + ticks = -30000; + testClaw->setEncoderRaw(Roboclaw::Motor::kM2, ticks); + response = testClaw->getLastCmd(); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], static_cast(Roboclaw::Command::kSetM2EncCount)); + ticksc = static_cast((response[2] << 8 * 3) + (response[3] << 8 * 2) + (response[4] << 8) + response[5]); + ASSERT_EQ(ticks, ticksc); + + + //reset encoder + testClaw->resetEncoders(); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], static_cast(Roboclaw::Command::kResetEnc)); + + } + TEST(RoboclawEncoder, setTicks) + { + + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + const double ticks_per_rev = 360.0; + units::Velocity v; + units::Distance d, d2; + units::Length wheel_radius(units::m * 0.04); + testClaw->setBytes(0); + + d = 100 * units::cm; + testClaw->setEncoder(Roboclaw::Motor::kM1, d); + response = testClaw->getLastCmd(); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], static_cast(Roboclaw::Command::kSetM1EncCount)); + EXPECT_DOUBLE_EQ( + static_cast(d.to(units::cm) * ticks_per_rev / wheel_radius.to(units::cm) / (M_PI * 2)), + (response[2] << 8 * 3) + (response[3] << 8 * 2) + (response[4] << 8) + response[5] + ); + + testClaw->setEncoder(Roboclaw::Motor::kM2, d); + response = testClaw->getLastCmd(); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], static_cast(Roboclaw::Command::kSetM2EncCount)); + EXPECT_DOUBLE_EQ( + static_cast(d.to(units::cm) * ticks_per_rev / wheel_radius.to(units::cm) / (M_PI * 2)), + (response[2] << 8 * 3) + (response[3] << 8 * 2) + (response[4] << 8) + response[5] + ); + + d = -100 * units::cm; + testClaw->setEncoder(Roboclaw::Motor::kM1, d); + response = testClaw->getLastCmd(); + ASSERT_EQ(testClaw->getLastCmd()[0], 0x80); + ASSERT_EQ(testClaw->getLastCmd()[1], static_cast(Roboclaw::Command::kSetM1EncCount)); + EXPECT_DOUBLE_EQ( + static_cast(d.to(units::cm) * ticks_per_rev / wheel_radius.to(units::cm) / (M_PI * 2)), + (response[2] << 8 * 3) + (response[3] << 8 * 2) + (response[4] << 8) + response[5] + ); + + } + + TEST(RoboclawDynamics, Class) + { + std::shared_ptr dynamics(new MotorDynamics); + units::Distance d = 1.0, d2; + units::Velocity v = 1.0, v2;//default unit m/s? + units::Acceleration a = 1.0, a2; + + //returns nullptrs when instantiated + ASSERT_EQ(dynamics->getDistance(), nullptr); + ASSERT_EQ(dynamics->getSpeed(), nullptr); + ASSERT_EQ(dynamics->getAcceleration(), nullptr); + ASSERT_EQ(dynamics->getDeceleration(), nullptr); + ASSERT_EQ(dynamics->getDType(), MotorDynamics::DType::kNone); + + dynamics->setDistance(d); + dynamics->setSpeed(v); + dynamics->setAcceleration(a); + dynamics->setDeceleration(a); + + d2 = *(dynamics->getDistance()); + ASSERT_DOUBLE_EQ(d2(), d()); + v2 = *(dynamics->getSpeed()); + ASSERT_DOUBLE_EQ(v2(), v()); + a2 = *(dynamics->getAcceleration()); + ASSERT_DOUBLE_EQ(a2(), a()); + a2 = *(dynamics->getDeceleration()); + ASSERT_DOUBLE_EQ(a2(), a()); + + ASSERT_EQ(dynamics->getDType(), MotorDynamics::DType::kSpeedAccelDecelDist); + //negative values + d = -1; + a = -1; + ASSERT_THROW(dynamics->setDistance(d), OutOfRange); + ASSERT_THROW(dynamics->setAcceleration(a), OutOfRange); + ASSERT_THROW(dynamics->setDeceleration(a), OutOfRange); + + } + TEST(RoboclawDynamics, setDynamicsM1) + { + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04); + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + std::vector message; + testClaw->setBytes(0); + std::shared_ptr dynamics(new MotorDynamics); + + + units::Distance d = 1.0; + units::Velocity v = 99.5;//default unit m/s? + units::Acceleration a = 45.0; + //dynamics empty + /* + Notes on setDynamics: speed is only signed parameter, dist, acc, dec, + are unsigned as defined by Roboclaw manual. + */ + EXPECT_NO_THROW(testClaw->setDynamics(Roboclaw::Motor::kM1, *dynamics)); + //ensure proper transmission + //motor 1 + //speed (+) + dynamics->setSpeed(v); + testClaw->setDynamics(Roboclaw::Motor::kM1, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM1Speed)); + //speed = static_cast((*dynamics.getSpeed() / m_wheel_radius)() * m_ticks_per_rev); + EXPECT_DOUBLE_EQ(static_cast((v / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed (-) + v = -1; + dynamics->setSpeed(v); + testClaw->setDynamics(Roboclaw::Motor::kM1, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM1Speed)); + EXPECT_DOUBLE_EQ(static_cast((v / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed 0 + v = 0; + dynamics->setSpeed(v); + testClaw->setDynamics(Roboclaw::Motor::kM1, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM1Speed)); + //speed = static_cast((*dynamics.getSpeed() / m_wheel_radius)() * m_ticks_per_rev); + EXPECT_DOUBLE_EQ(0.0, (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed & acceleration(+) + v = 1; + dynamics->setSpeed(v); + dynamics->setAcceleration(a); + testClaw->setDynamics(Roboclaw::Motor::kM1, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM1SpeedAccel)); + EXPECT_DOUBLE_EQ(static_cast((a / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed & distance(+) + dynamics->setDistance(d); + testClaw->setDynamics(Roboclaw::Motor::kM1, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM1SpeedAccelDist)); + EXPECT_DOUBLE_EQ(static_cast((d / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[6] << 8 * 3) + (message[7] << 8 * 2) + (message[8] << 8) + message[9]); + + //speed & acceleration & distance & deceleration(+) + dynamics->setDeceleration(a); + testClaw->setDynamics(Roboclaw::Motor::kM1, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM1SpeedAccelDeccelPos)); + EXPECT_DOUBLE_EQ(static_cast((a / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[10] << 8 * 3) + (message[11] << 8 * 2) + (message[12] << 8) + message[13]); + + + + } + + TEST(RoboclawDynamics, setDynamicsM2) + { + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04); + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + std::vector message; + testClaw->setBytes(0); + std::shared_ptr dynamics(new MotorDynamics); + + units::Distance d = 1.0; + units::Velocity v = 99.5;//default unit m/s? + units::Acceleration a = 45.0; + //dynamics empty + /* + Notes on setDynamics: speed is only signed parameter, dist, acc, dec, + are unsigned as defined by Roboclaw manual. + */ + EXPECT_NO_THROW(testClaw->setDynamics(Roboclaw::Motor::kM2, *dynamics)); + //ensure proper transmission + //motor 1 + //speed (+) + dynamics->setSpeed(v); + testClaw->setDynamics(Roboclaw::Motor::kM2, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM2Speed)); + //speed = static_cast((*dynamics.getSpeed() / m_wheel_radius)() * m_ticks_per_rev); + EXPECT_DOUBLE_EQ(static_cast((v / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed (-) + v = -1; + dynamics->setSpeed(v); + testClaw->setDynamics(Roboclaw::Motor::kM2, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM2Speed)); + EXPECT_DOUBLE_EQ(static_cast((v / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed 0 + v = 0; + dynamics->setSpeed(v); + testClaw->setDynamics(Roboclaw::Motor::kM2, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM2Speed)); + //speed = static_cast((*dynamics.getSpeed() / m_wheel_radius)() * m_ticks_per_rev); + EXPECT_DOUBLE_EQ(0.0, (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed & acceleration(+) + v = 1; + dynamics->setSpeed(v); + dynamics->setAcceleration(a); + testClaw->setDynamics(Roboclaw::Motor::kM2, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM2SpeedAccel)); + EXPECT_DOUBLE_EQ(static_cast((a / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed & distance(+) + dynamics->setDistance(d); + testClaw->setDynamics(Roboclaw::Motor::kM2, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM2SpeedAccelDist)); + EXPECT_DOUBLE_EQ(static_cast((d / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[6] << 8 * 3) + (message[7] << 8 * 2) + (message[8] << 8) + message[9]); + + //speed & acceleration & distance & deceleration(+) + dynamics->setDeceleration(a); + testClaw->setDynamics(Roboclaw::Motor::kM2, *dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kM2SpeedAccelDeccelPos)); + EXPECT_DOUBLE_EQ(static_cast((a / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[10] << 8 * 3) + (message[11] << 8 * 2) + (message[12] << 8) + message[13]); + + + + } + + + TEST(RoboclawDynamics, setDynamicsPlural) + { + double ticks_per_rev = 360.0; + units::Length wheel_radius(units::m * 0.04); + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + std::vector message; + testClaw->setBytes(0); + std::shared_ptr dynamics(new MotorDynamics); + + units::Distance d = 1.0; + units::Velocity v = 99.5;//default unit m/s? + units::Acceleration a = 45.0; + //dynamics empty + /* + Notes on setDynamics: speed is only signed parameter, dist, acc, dec, + are unsigned as defined by Roboclaw manual. + */ + EXPECT_NO_THROW(testClaw->setDynamics(*dynamics)); + //ensure proper transmission + //motors 1&2 + //speed (+) + dynamics->setSpeed(v); + testClaw->setDynamics(*dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kMixedSpeed)); + //speed = static_cast((*dynamics.getSpeed() / m_wheel_radius)() * m_ticks_per_rev); + EXPECT_DOUBLE_EQ(static_cast((v / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed (-) + v = -1; + dynamics->setSpeed(v); + testClaw->setDynamics(*dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kMixedSpeed)); + EXPECT_DOUBLE_EQ(static_cast((v / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed 0 + v = 0; + dynamics->setSpeed(v); + testClaw->setDynamics(*dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kMixedSpeed)); + //speed = static_cast((*dynamics.getSpeed() / m_wheel_radius)() * m_ticks_per_rev); + EXPECT_DOUBLE_EQ(0.0, (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed & acceleration(+) + v = 1; + dynamics->setSpeed(v); + dynamics->setAcceleration(a); + testClaw->setDynamics(*dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kMixedSpeedAccel)); + EXPECT_DOUBLE_EQ(static_cast((a / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[2] << 8 * 3) + (message[3] << 8 * 2) + (message[4] << 8) + message[5]); + //speed & distance(+) + dynamics->setDistance(d); + testClaw->setDynamics(*dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kMixedSpeedAccelDist)); + EXPECT_DOUBLE_EQ(static_cast((d / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[6] << 8 * 3) + (message[7] << 8 * 2) + (message[8] << 8) + message[9]); + + //speed & acceleration & distance & deceleration(+) + dynamics->setDeceleration(a); + testClaw->setDynamics(*dynamics); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kMixedSpeedAccelDeccelPos)); + EXPECT_DOUBLE_EQ(static_cast((a / wheel_radius / (M_PI * 2))() * ticks_per_rev), + (message[10] << 8 * 3) + (message[11] << 8 * 2) + (message[12] << 8) + message[13]); + + + + } + + TEST(RoboclawPID, VelocityM1) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response, message; + testClaw->setBytes(0); + + std::shared_ptr vparams(new VelocityPIDParameters); + + //given response, received properly + //p,i,d,qpps, 4 bytes each + response = {0x0E, 0xAD, 0xBE, 0xEF,//d + 0x12, 0x34, 0x56, 0x78, //p + 0xAB, 0xCD, 0xEF, 0x00, //i + 0x0, 0x01, 0x00, 0x00 + }; //qpps + testClaw->setcResponse(response); + *vparams = testClaw->readVelocityPID(Roboclaw::Motor::kM1); + //should be > 0 + EXPECT_GE(vparams->kp, 0); + EXPECT_GE(vparams->ki, 0); + EXPECT_GE(vparams->kd, 0); + + EXPECT_FLOAT_EQ(vparams->kp, static_cast(0x0EADBEEF) / 65536); + EXPECT_FLOAT_EQ(vparams->ki, static_cast(0x12345678) / 65536); + EXPECT_FLOAT_EQ(vparams->kd, static_cast(0xABCDEF00) / 65536); + EXPECT_EQ(vparams->qpps, 0x00010000); + + //validate transmissions + testClaw->setVelocityPID(Roboclaw::Motor::kM1, *vparams); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kSetM1PID)); + //d + ASSERT_NEAR(static_cast((message[2] << 8 * 3) + (message[3] << 8 * 2) + + (message[4] << 8) + message[5]), 0xABCDEF00, 20); + //p (B/c of float, the int is close but not = the original value) + ASSERT_NEAR(static_cast((message[6] << 8 * 3) + (message[7] << 8 * 2) + + (message[8] << 8) + message[9]), 0x0EADBEEF, 20); + //i + ASSERT_NEAR((message[10] << 8 * 3) + (message[11] << 8 * 2) + + (message[12] << 8) + message[13], 0x12345678, 20); + //qpps + ASSERT_EQ((message[14] << 8 * 3) + (message[15] << 8 * 2) + + (message[16] << 8) + message[17], 0x10000); + + + + + } + + TEST(RoboclawPID, VelocityM2) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response, message; + testClaw->setBytes(0); + std::shared_ptr vparams(new VelocityPIDParameters); + + //given response, received properly + //p,i,d,qpps, 4 bytes each + response = {0xDE, 0xAD, 0xBE, 0xEF,//p + 0x12, 0x34, 0x56, 0x78, //i + 0xAB, 0xCD, 0xEF, 0x00, //d + 0x0, 0x01, 0x00, 0x00 + }; //qpps + testClaw->setcResponse(response); + *vparams = testClaw->readVelocityPID(Roboclaw::Motor::kM2); + //should be > 0 + EXPECT_GE(vparams->kp, 0); + EXPECT_GE(vparams->ki, 0); + EXPECT_GE(vparams->kd, 0); + EXPECT_FLOAT_EQ(vparams->kp, static_cast(0xDEADBEEF) / 65536); + EXPECT_FLOAT_EQ(vparams->ki, static_cast(0x12345678) / 65536); + EXPECT_FLOAT_EQ(vparams->kd, static_cast(0xABCDEF00) / 65536); + EXPECT_EQ(vparams->qpps, 0x00010000); + + //validate transmissions + testClaw->setVelocityPID(Roboclaw::Motor::kM2, *vparams); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kSetM2PID)); + //d + ASSERT_NEAR(static_cast((message[2] << 8 * 3) + (message[3] << 8 * 2) + + (message[4] << 8) + message[5]), 0xABCDEF00, 20); + //p + ASSERT_NEAR(static_cast((message[6] << 8 * 3) + (message[7] << 8 * 2) + + (message[8] << 8) + message[9]), 0xDEADBEEF, 20); + //i + ASSERT_NEAR(static_cast((message[10] << 8 * 3) + (message[11] << 8 * 2) + + (message[12] << 8) + message[13]), 0x12345678, 20); + //qpps + ASSERT_EQ((message[14] << 8 * 3) + (message[15] << 8 * 2) + + (message[16] << 8) + message[17], 0x10000); + + + + } + + TEST(RoboclawPID, PositionM1) + { + /* + Receive: [P(4 bytes), I(4 bytes), D(4 bytes), MaxI(4 byte), Deadzone(4 byte), + MinPos(4 byte), MaxPos(4 byte), CRC(2 bytes)] + */ + std::shared_ptr pparams(new PositionPIDParameters); + std::shared_ptr testClaw(new Roboclaw); + std::vector response, message; + testClaw->setBytes(0); + + //values should be initialized to 0 + EXPECT_FLOAT_EQ(pparams->kp, 0); + EXPECT_FLOAT_EQ(pparams->ki, 0); + EXPECT_FLOAT_EQ(pparams->kd, 0); + EXPECT_EQ(pparams->kiMax, 0u); + EXPECT_EQ(pparams->deadzone, 0u); + EXPECT_EQ(pparams->min, 0u); + EXPECT_EQ(pparams->max, 0u); + + response = {0xDE, 0xAD, 0xBE, 0xEF,//p + 0x12, 0x34, 0x56, 0x78, //i + 0xAB, 0xCD, 0xEF, 0x00, //d + 0x0, 0x11, 0x00, 0x00, //maxI + 0x01, 0x35, 0x79, 0xBD, //deadzone + 0x00, 0x00, 0x11, 0x00, //minpos + 0x0F, 0xFF, 0xFF, 0xFF + }; //maxpos + testClaw->setcResponse(response); + + *pparams = testClaw->readPositionPID(Roboclaw::Motor::kM2); + //value should be greater than 0 + EXPECT_GE(pparams->kp, 0); + EXPECT_GE(pparams->ki, 0); + EXPECT_GE(pparams->kd, 0); + EXPECT_GE(pparams->kiMax, 0u); + EXPECT_GE(pparams->deadzone, 0u); + EXPECT_GE(pparams->min, 0u); + EXPECT_GE(pparams->max, 0u); + + //floats are trash + ASSERT_NEAR(pparams->kp, static_cast(0xDEADBEEF) / 1024, 20); + EXPECT_NEAR(pparams->ki, static_cast(0x12345678) / 1024, 20); + ASSERT_NEAR(pparams->kd, static_cast(0xABCDEF00) / 1024, 20); + EXPECT_EQ(pparams->kiMax, 0x00110000); + EXPECT_EQ(pparams->deadzone, 0x013579BD); + EXPECT_EQ(pparams->min, 0x00001100); + EXPECT_EQ(pparams->max, 0x0FFFFFFF); + + //validate transmissions + testClaw->setPositionPID(Roboclaw::Motor::kM1, *pparams); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kSetM1PosPID)); + //d + EXPECT_NEAR(static_cast((message[2] << 8 * 3) + (message[3] << 8 * 2) + + (message[4] << 8) + message[5]), 0xABCDEF00, 20); + //p + ASSERT_NEAR(static_cast((message[6] << 8 * 3) + (message[7] << 8 * 2) + + (message[8] << 8) + message[9]), 0xDEADBEEF, 20); + //i + ASSERT_NEAR((message[10] << 8 * 3) + (message[11] << 8 * 2) + + (message[12] << 8) + message[13], 0x12345678, 20); + //kiMax + ASSERT_EQ((message[14] << 8 * 3) + (message[15] << 8 * 2) + + (message[16] << 8) + message[17], 0x110000); + //deadzone + ASSERT_EQ(static_cast((message[18] << 8 * 3) + (message[19] << 8 * 2) + + (message[20] << 8) + message[21]), 0x13579BD); + //min + ASSERT_EQ((message[22] << 8 * 3) + (message[23] << 8 * 2) + + (message[24] << 8) + message[25], 0x1100); + //max + ASSERT_EQ((message[26] << 8 * 3) + (message[27] << 8 * 2) + + (message[28] << 8) + message[29], 0x0FFFFFFF); + + } + + TEST(RoboclawPID, PositionM2) + { + std::shared_ptr pparams(new PositionPIDParameters); + std::shared_ptr testClaw(new Roboclaw); + std::vector response, message; + testClaw->setBytes(0); + + /* + Receive: [P(4 bytes), I(4 bytes), D(4 bytes), MaxI(4 byte), Deadzone(4 byte), + MinPos(4 byte), MaxPos(4 byte), CRC(2 bytes)] + */ + + response = {0xDE, 0xAD, 0xBE, 0xEF,//p + 0x12, 0x34, 0x56, 0x78, //i + 0xAB, 0xCD, 0xEF, 0x00, //d + 0x0, 0x01, 0x00, 0x00, //maxI + 0x01, 0x35, 0x79, 0xBD, //deadzone + 0x00, 0x00, 0x11, 0x00, //minpos + 0x0F, 0xFF, 0xFF, 0xFF + }; //maxpos + testClaw->setcResponse(response); + + + *pparams = testClaw->readPositionPID(Roboclaw::Motor::kM2); + //value should be greater than 0 + EXPECT_GE(pparams->kp, 0); + EXPECT_GE(pparams->ki, 0); + EXPECT_GE(pparams->kd, 0); + EXPECT_GE(pparams->kiMax, 0u); + EXPECT_GE(pparams->deadzone, 0u); + EXPECT_GE(pparams->min, 0u); + EXPECT_GE(pparams->max, 0u); + + ASSERT_NEAR(pparams->kp, static_cast(0xDEADBEEF) / 1024, 20); + EXPECT_NEAR(pparams->ki, static_cast(0x12345678) / 1024, 20); + ASSERT_NEAR(pparams->kd, static_cast(0xABCDEF00) / 1024, 20); + EXPECT_EQ(pparams->kiMax, 0x00010000); + EXPECT_EQ(pparams->deadzone, 0x013579BD); + EXPECT_EQ(pparams->min, 0x00001100); + EXPECT_EQ(pparams->max, 0x0FFFFFFF); + + //validate transmissions + testClaw->setPositionPID(Roboclaw::Motor::kM1, *pparams); + message = testClaw->getLastCmd(); + ASSERT_EQ(message[0], 0x80); + ASSERT_EQ(message[1], static_cast(Roboclaw::Command::kSetM1PosPID)); + //d + ASSERT_NEAR(static_cast((message[2] << 8 * 3) + (message[3] << 8 * 2) + + (message[4] << 8) + message[5]), 0xABCDEF00, 20); + //p + ASSERT_NEAR(static_cast((message[6] << 8 * 3) + (message[7] << 8 * 2) + + (message[8] << 8) + message[9]), 0xDEADBEEF, 20); + //i + ASSERT_NEAR(static_cast((message[10] << 8 * 3) + (message[11] << 8 * 2) + + (message[12] << 8) + message[13]), 0x12345678, 20); + //kiMax + ASSERT_EQ((message[14] << 8 * 3) + (message[15] << 8 * 2) + + (message[16] << 8) + message[17], 0x10000); + //deadzone + ASSERT_EQ((message[18] << 8 * 3) + (message[19] << 8 * 2) + + (message[20] << 8) + message[21], 0x13579BD); + //min + ASSERT_EQ((message[22] << 8 * 3) + (message[23] << 8 * 2) + + (message[24] << 8) + message[25], 0x1100); + //max + ASSERT_EQ((message[26] << 8 * 3) + (message[27] << 8 * 2) + + (message[28] << 8) + message[29], 0x0FFFFFFF); + + + + } + + TEST(RoboclawConfig, ClassSetters) + { + std::shared_ptr config(new Config); + //comm mode setters work properly + uint16_t mask; + for (mask = 0; mask < 3; mask++) + { + config->setCommMode(static_cast(mask)); + ASSERT_EQ((config->get() & mask), mask); + } + //battery setters work properly + for (mask = 0; mask < 4 * 8; mask += 4) + { + config->setBatteryMode(static_cast(mask)); + EXPECT_EQ((config->get() & mask), mask); + } + + config->setCommMode(Config::CommMode::kAnalog); + ASSERT_THROW(config->getBaudRate(), InvalidCommMode); + config->setCommMode(Config::CommMode::kPacketSerial); + //Baudrate setters work properly + for (mask = 0; mask < 8 * 32; mask += 32) + { + config->setBaudRate(static_cast(mask)); + EXPECT_EQ((config->get() & mask), mask); + } + //packet addr setters work properly + for (mask = 0; mask < 8 * 256; mask += 256) + { + config->setPacketAddress(static_cast(mask)); + EXPECT_EQ((config->get() & mask), mask); + } + + config->setSwapButtons(1); + mask = 0x4000; + EXPECT_EQ(config->get() & mask, mask); + + config->setSwapEncoders(1); + mask = 0x2000; + EXPECT_EQ(config->get() & mask, mask); + } + TEST(RoboclawConfig, ClassGetters) + { + std::shared_ptr config(new Config); + + //packet serial + uint16_t cfg = 0x80A7; + config->set(cfg); + ASSERT_EQ(static_cast(config->getCommMode()), + static_cast(Config::CommMode::kPacketSerial)); + EXPECT_EQ(static_cast(config->getPacketAddress()), + static_cast(Config::PacketAddress::k80)); + EXPECT_EQ(static_cast(config->getBatteryMode()), + static_cast(Config::BatteryMode::kAuto)); + EXPECT_EQ(static_cast(config->getBaudRate()), + static_cast(Config::BaudRate::k115200)); + + //RC + battery off + cfg = 0; + config->set(cfg); + ASSERT_EQ(static_cast(config->getCommMode()), + static_cast(Config::CommMode::kRC)); + ASSERT_THROW(config->getPacketAddress(), InvalidCommMode); + ASSERT_THROW(config->getBaudRate(), InvalidCommMode); + EXPECT_EQ(static_cast(config->getBatteryMode()), + static_cast(Config::BatteryMode::kOff)); + } + TEST(RoboclawConfig, Other) + { + std::shared_ptr config(new Config); + std::shared_ptr testClaw(new Roboclaw); + std::vector response, message; + testClaw->setBytes(0); + response = {0x80, 0xA7}; + testClaw->setcResponse(response); + *config = testClaw->getConfig(); + EXPECT_EQ(config->get(), 0x80A7); + //validate transmission + config->set(0x80A7); + testClaw->setConfig(*config); + message = testClaw->getLastCmd(); + EXPECT_EQ(message[0], 0x80); + EXPECT_EQ(message[1], 98); + EXPECT_EQ((message[2] << 8) + message[3], 0x80A7); + } + + TEST(RoboclawMisc, Temperature) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + testClaw->setBytes(0); + units::Temperature t; + /* + readtemperature, temp is in celsius. + default unit is kelvin, but 1K = 1C + */ + response = {0x1, 0xF4}; + testClaw->setcResponse(response); + t = testClaw->readTemperature(); + EXPECT_DOUBLE_EQ(t(), 50.0); + + } + TEST(RoboclawMisc, readBuffers) + { + std::shared_ptr testClaw(new Roboclaw); + std::vector response; + testClaw->setBytes(0); + + response = {0x80, 0x3F}; + testClaw->setcResponse(response); + EXPECT_EQ(testClaw->readBufferLens()[0], 0x80); + EXPECT_EQ(testClaw->readBufferLens()[1], 0x3F); + } + } + } + } +} diff --git a/core/navigation/CMakeLists.txt b/core/navigation/CMakeLists.txt index c817d8d..d937a43 100644 --- a/core/navigation/CMakeLists.txt +++ b/core/navigation/CMakeLists.txt @@ -2,20 +2,7 @@ cmake_minimum_required(VERSION 3.1) message("Building navigation") project(navigation) -if(GIT_BRANCH MATCHES "path_planner") - add_subdirectory(path_planner) - add_subdirectory(path_planner2) -elseif(GIT_BRANCH MATCHES "navx") - add_subdirectory(navx) -elseif(GIT_BRANCH MATCHES "path_follower") - add_subdirectory(path_planner) - add_subdirectory(path_planner2) - #add_subdirectory(drivetrains) - add_subdirectory(path_follower) -else() - add_subdirectory(navx) - add_subdirectory(path_planner) - add_subdirectory(path_planner2) - #add_subdirectory(drivetrains) - add_subdirectory(path_follower) -endif() +add_subdirectory(path_planner) +add_subdirectory(actions) +add_subdirectory(drivetrains) +add_subdirectory(navx) diff --git a/core/navigation/README.md b/core/navigation/README.md index 3678fb6..0af2845 100644 --- a/core/navigation/README.md +++ b/core/navigation/README.md @@ -10,5 +10,8 @@ Created by Robert Haschke, Erik Weitnauter, and Helge Ritter for their paper "On Provides first-, second-, and third-order online trajectory planning. - ## Path Planner + +Two planners are included: + - Spline planner + - TEB planner (Timed Elastic Band) diff --git a/core/navigation/actions/CMakeLists.txt b/core/navigation/actions/CMakeLists.txt new file mode 100644 index 0000000..085d3cb --- /dev/null +++ b/core/navigation/actions/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.1.0) +message("Building navigation actions") +project(navigation_actions) + +file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS "include/navigation_actions/*.hpp") +file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES "src/*.cpp") + +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) +target_link_libraries(${PROJECT_NAME} framework drivetrains motor_controllers navx) +target_include_directories(${PROJECT_NAME} PUBLIC include) diff --git a/core/navigation/actions/include/navigation_actions/drive_arc.hpp b/core/navigation/actions/include/navigation_actions/drive_arc.hpp new file mode 100644 index 0000000..818847c --- /dev/null +++ b/core/navigation/actions/include/navigation_actions/drive_arc.hpp @@ -0,0 +1,46 @@ +#ifndef DRIVE_ARC_HPP +#define DRIVE_ARC_HPP + +#include + +#include + +namespace rip +{ + namespace navigation + { + namespace actions + { + class DriveArc : public framework::Action + { + public: + + + + /** + * Returns whether or not the action has finished execution. + */ + virtual bool isFinished() override; + + /** + * Iteratively called until {@see Action#isFinished()} returns true + */ + virtual void update(nlohmann::json& state) override; + + /** + * Run once before the main code + */ + virtual void setup(nlohmann::json& state) override; + + /** + * Run once after finished + */ + virtual void teardown(nlohmann::json& state) override; + + private: + }; + } + } +} + +#endif // DRIVE_ARC_HPP diff --git a/core/navigation/actions/include/navigation_actions/drive_straight.hpp b/core/navigation/actions/include/navigation_actions/drive_straight.hpp new file mode 100644 index 0000000..7bbd698 --- /dev/null +++ b/core/navigation/actions/include/navigation_actions/drive_straight.hpp @@ -0,0 +1,58 @@ +#ifndef DRIVE_STRAIGHT_HPP +#define DRIVE_STRAIGHT_HPP + +#include + +#include +#include + +#include + + +namespace rip +{ + namespace navigation + { + namespace actions + { + class DriveStraight : public framework::Action + { + public: + DriveStraight(const std::string& name, std::shared_ptr drivetrain, const units::Distance& distance, double p, double i, double d); + + DriveStraight(const std::string& name, std::shared_ptr drivetrain, const units::Time& time, const units::Velocity& speed); + + /** + * Returns whether or not the action has finished execution. + */ + virtual bool isFinished() override; + + /** + * Iteratively called until {@see Action#isFinished()} returns true + */ + virtual void update(nlohmann::json& state) override; + + /** + * Run once before the main code + */ + virtual void setup(nlohmann::json& state) override; + + /** + * Run once after finished + */ + virtual void teardown(nlohmann::json& state) override; + + private: + bool m_use_time; + units::Distance m_distance; + units::Time m_time; + std::chrono::time_point m_start_time; + units::Velocity m_speed; + std::shared_ptr m_drivetrain; + }; + + } + } +} + +#endif // DRIVE_STRAIGHT_HPP diff --git a/core/navigation/actions/include/navigation_actions/drive_straight_trapezoid.hpp b/core/navigation/actions/include/navigation_actions/drive_straight_trapezoid.hpp new file mode 100644 index 0000000..7471549 --- /dev/null +++ b/core/navigation/actions/include/navigation_actions/drive_straight_trapezoid.hpp @@ -0,0 +1,56 @@ +#ifndef DRIVE_STRAIGHT_TRAPEZOID_HPP +#define DRIVE_STRAIGHT_TRAPEZOID_HPP + +#include + +#include + +namespace rip +{ + namespace navigation + { + namespace actions + { + class DriveStraightTrapezoid : public framework::Action + { + public: + + /** + * Returns whether or not the action has finished execution. + */ + virtual bool isFinished() override + { + // todo + } + + /** + * Iteratively called until {@see Action#isFinished()} returns true + */ + virtual void update(nlohmann::json& state) override + { + // todo + } + + /** + * Run once before the main code + */ + virtual void setup(nlohmann::json& state) override + { + // todo + } + + /** + * Run once after finished + */ + virtual void teardown(nlohmann::json& state) override + { + // todo + } + + private: + }; + } + } +} + +#endif // DRIVE_STRAIGHT_TRAPEZOID_HPP diff --git a/core/navigation/actions/include/navigation_actions/follow_trajectory.hpp b/core/navigation/actions/include/navigation_actions/follow_trajectory.hpp new file mode 100644 index 0000000..2da303e --- /dev/null +++ b/core/navigation/actions/include/navigation_actions/follow_trajectory.hpp @@ -0,0 +1,56 @@ +#ifndef FOLLOW_TRAJECTORY_HPP +#define DRIVE_ARC_HPP + +#include + +#include + +namespace rip +{ + namespace navigation + { + namespace actions + { + class FollowTrajectory : public framework::Action + { + public: + + /** + * Returns whether or not the action has finished execution. + */ + virtual bool isFinished() override + { + // todo + } + + /** + * Iteratively called until {@see Action#isFinished()} returns true + */ + virtual void update(nlohmann::json& state) override + { + // todo + } + + /** + * Run once before the main code + */ + virtual void setup(nlohmann::json& state) override + { + // todo + } + + /** + * Run once after finished + */ + virtual void teardown(nlohmann::json& state) override + { + // todo + } + + private: + }; + } + } +} + +#endif // FOLLOW_TRAJECTORY_HPP diff --git a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp new file mode 100644 index 0000000..f8b6a7b --- /dev/null +++ b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp @@ -0,0 +1,56 @@ +#ifndef TURN_TO_ANGLE_HPP +#define TURN_TO_ANGLE_HPP + +#include + +#include + +namespace rip +{ + namespace navigation + { + namespace actions + { + class TurnToAngle : public framework::Action + { + public: + + /** + * Returns whether or not the action has finished execution. + */ + virtual bool isFinished() override + { + // todo + } + + /** + * Iteratively called until {@see Action#isFinished()} returns true + */ + virtual void update(nlohmann::json& state) override + { + // todo + } + + /** + * Run once before the main code + */ + virtual void setup(nlohmann::json& state) override + { + // todo + } + + /** + * Run once after finished + */ + virtual void teardown(nlohmann::json& state) override + { + // todo + } + + private: + }; + } + } +} + +#endif // TURN_TO_ANGLE_HPP diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp new file mode 100644 index 0000000..84a269d --- /dev/null +++ b/core/navigation/actions/src/drive_arc.cpp @@ -0,0 +1,30 @@ +#include "navigation_actions/drive_arc.hpp" + +namespace rip +{ + namespace navigation + { + namespace actions + { + bool DriveArc::isFinished() + { + // todo + } + + void DriveArc::update(nlohmann::json& state) + { + // todo + } + + void DriveArc::setup(nlohmann::json& state) + { + // todo + } + + void DriveArc::teardown(nlohmann::json& state) + { + // todo + } + } + } +} diff --git a/core/navigation/actions/src/drive_straight.cpp b/core/navigation/actions/src/drive_straight.cpp new file mode 100644 index 0000000..23ce6e8 --- /dev/null +++ b/core/navigation/actions/src/drive_straight.cpp @@ -0,0 +1,53 @@ +#include "navigation_actions/drive_straight.hpp" +#include +namespace rip +{ + namespace navigation + { + namespace actions + { + DriveStraight::DriveStraight(const std::string& name, std::shared_ptr drivetrain, const units::Distance& distance, double p, double i, double d) + : Action(name) + , m_use_time(false) + , m_distance(distance) + , m_drivetrain(drivetrain) + {} + + DriveStraight::DriveStraight(const std::string& name, std::shared_ptr drivetrain, const units::Time& time, const units::Velocity& speed) + : Action(name) + , m_use_time(true) + , m_time(time) + , m_speed(speed) + , m_drivetrain(drivetrain) + {} + + bool DriveStraight::isFinished() + { + std::chrono::time_point current = std::chrono::system_clock::now(); + units::Time diff = std::chrono::duration_cast(current - m_start_time).count() * units::ms; + misc::Logger::getInstance()->debug("Diff time: {}", diff.to(units::s)); + return diff >= m_time; + } + + void DriveStraight::update(nlohmann::json& state) + { + return; + } + + void DriveStraight::setup(nlohmann::json& state) + { + misc::Logger::getInstance()->debug("Driving Straight"); + m_start_time = std::chrono::system_clock::now(); + //misc::Logger::getInstance()->debug("Start time: {}", m_start_time.to(units::s)); + motorcontrollers::MotorDynamics dynamics; + dynamics.setSpeed(m_speed); + m_drivetrain->drive(dynamics); + } + + void DriveStraight::teardown(nlohmann::json& state) + { + m_drivetrain->stop(); + } + } + } +} diff --git a/core/navigation/actions/src/drive_straight_trapezoid.cpp b/core/navigation/actions/src/drive_straight_trapezoid.cpp new file mode 100644 index 0000000..c846381 --- /dev/null +++ b/core/navigation/actions/src/drive_straight_trapezoid.cpp @@ -0,0 +1,12 @@ +#include "navigation_actions/drive_straight_trapezoid.hpp" + +namespace rip +{ + namespace navigation + { + namespace actions + { + + } + } +} \ No newline at end of file diff --git a/core/navigation/actions/src/follow_trajectory.hpp b/core/navigation/actions/src/follow_trajectory.hpp new file mode 100644 index 0000000..dad8c25 --- /dev/null +++ b/core/navigation/actions/src/follow_trajectory.hpp @@ -0,0 +1,12 @@ +#include "navigation_actions/follow_trajectory.hpp" + +namespace rip +{ + namespace navigation + { + namespace actions + { + + } + } +} \ No newline at end of file diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp new file mode 100644 index 0000000..0577d9a --- /dev/null +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -0,0 +1,12 @@ +#include "navigation_actions/turn_to_angle.hpp" + +namespace rip +{ + namespace navigation + { + namespace actions + { + + } + } +} \ No newline at end of file diff --git a/core/navigation/drivetrains/CMakeLists.txt b/core/navigation/drivetrains/CMakeLists.txt index c55d825..645d1cd 100644 --- a/core/navigation/drivetrains/CMakeLists.txt +++ b/core/navigation/drivetrains/CMakeLists.txt @@ -8,5 +8,5 @@ file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cpp) file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/*.hpp) add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) -target_link_libraries(${PROJECT_NAME} fmt json units spdlog roboclaw) +target_link_libraries(${PROJECT_NAME} fmt json units spdlog motor_controllers navx misc) target_include_directories(${PROJECT_NAME} PUBLIC include) diff --git a/core/navigation/drivetrains/include/drivetrain.hpp b/core/navigation/drivetrains/include/drivetrain.hpp deleted file mode 100644 index 739bf4a..0000000 --- a/core/navigation/drivetrains/include/drivetrain.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef DRIVE_TRAIN_HPP -#define DRIVE_TRAIN_HPP - -#include "nav_command.hpp" - -namespace rip -{ - namespace navigation - { - /** - * Abstract base class for the drive train - */ - class Drivetrain : public SubSystem - { - public: - /** - * Drive all the motors - * @param power [-1, 1] - */ - virtual void drive(double power) = 0; - - /** - * Drive left and right separately - * @param left [-1, 1] - * @param right [-1, 1] - */ - virtual void drive(double left, double right) = 0; - - /** - * Drive all the motors - * - * all range from [-1, 1] - */ - virtual void drive(double front_left, double front_right, double back_left, double back_rightk) = 0; - - /** - * Single command to all motors - */ - virtual void drive(const NavCommand& command) = 0; - - /** - * Command left and right sides separately - */ - virtual void drive(const NavCommand& left, const NavCommand& right) = 0; - - /** - * Command four wheels separately - */ - virtual void drive(const NavCommand& front_left, const NavCommand& front_right, const NavCommand& back_left, const NavCommand& back_right) = 0; - - virtual bool diagnostic() override; - }; - } -} - -#endif // DRIVE_TRAIN_HPP \ No newline at end of file diff --git a/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp new file mode 100644 index 0000000..d03f7dc --- /dev/null +++ b/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp @@ -0,0 +1,63 @@ +#ifndef DRIVE_TRAIN_HPP +#define DRIVE_TRAIN_HPP + +#include +#include + +namespace rip +{ + namespace navigation + { + namespace drivetrains + { + /** + * Abstract base class for the drive train + */ + class Drivetrain : public framework::Subsystem + { + using MotorDynamics = motorcontrollers::MotorDynamics; + public: + Drivetrain(const std::string& name) + : Subsystem(name) + {} + + /** + * Drive all the motors + * @param power [-1, 1] + */ + virtual void drive(double power) = 0; + + /** + * Drive left and right separately + * @param left [-1, 1] + * @param right [-1, 1] + */ + virtual void drive(double left, double right) = 0; + + /** + * Drive all the motors + * + * all range from [-1, 1] + */ + virtual void drive(double front_left, double front_right, double back_left, double back_rightk) = 0; + + /** + * Single command to all motors + */ + virtual void drive(const MotorDynamics& command) = 0; + + /** + * Command left and right sides separately + */ + virtual void drive(const MotorDynamics& left, const MotorDynamics& right) = 0; + + /** + * Command four wheels separately + */ + virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, const MotorDynamics& back_left, const MotorDynamics& back_right) = 0; + }; + } + } +} + +#endif // DRIVE_TRAIN_HPP diff --git a/core/navigation/drivetrains/include/drivetrains/exceptions.hpp b/core/navigation/drivetrains/include/drivetrains/exceptions.hpp new file mode 100644 index 0000000..2d376d6 --- /dev/null +++ b/core/navigation/drivetrains/include/drivetrains/exceptions.hpp @@ -0,0 +1,17 @@ +#ifndef DRIVETRAINS_EXCEPTIONS_HPP +#define DRIVETRAINS_EXCEPTIONS_HPP + +#include + +namespace rip +{ + namespace navigation + { + namespace drivetrains + { + NEW_EX(OutOfRangeException) + } + } +} + +#endif // DRIVETRAINS_EXCEPTIONS_HPP diff --git a/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp new file mode 100644 index 0000000..a731f1f --- /dev/null +++ b/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp @@ -0,0 +1,78 @@ +#ifndef TWO_ROBOCLAW_DRIVE_TRAIN_HPP +#define TWO_ROBOCLAW_DRIVE_TRAIN_HPP + +#include + +#include +#include + +#include "drivetrain.hpp" + +namespace rip +{ + namespace navigation + { + namespace drivetrains + { + /** + * Abstract base class for the drive train + */ + class TwoRoboclawDrivetrain : public Drivetrain + { + using Roboclaw = motorcontrollers::roboclaw::Roboclaw; + using NavX = navx::NavX; + using MotorDynamics = motorcontrollers::MotorDynamics; + public: + TwoRoboclawDrivetrain(const std::string& name, std::shared_ptr left, std::shared_ptr right, std::shared_ptr navx = nullptr); + + ~TwoRoboclawDrivetrain(); + + /** + * Drive all the motors + * @param power [-1.0, 1.0] + */ + virtual void drive(double power) override; + + /** + * Drive left and right separately + * @param left [-1.0, 1.0] + * @param right [-1.0, 1.0] + */ + virtual void drive(double left, double right) override; + + /** + * Drive all the motors + * + * all range from [-1.0, 1.0] + */ + virtual void drive(double front_left, double front_right, double back_left, double back_rightk) override; + + /** + * Single command to all motors + */ + virtual void drive(const MotorDynamics& command) override; + + /** + * Command left and right sides separately + */ + virtual void drive(const MotorDynamics& left, const MotorDynamics& right) override; + + /** + * Command four wheels separately + */ + virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, const MotorDynamics& back_left, const MotorDynamics& back_right) override; + + + virtual void stop() override; + + virtual bool diagnostic() override; + private: + std::shared_ptr m_left; + std::shared_ptr m_right; + std::shared_ptr m_navx; + }; + } + } +} + +#endif // DRIVE_TRAIN_HPP diff --git a/core/navigation/drivetrains/include/nav_command.hpp b/core/navigation/drivetrains/include/nav_command.hpp deleted file mode 100644 index f315531..0000000 --- a/core/navigation/drivetrains/include/nav_command.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef NAV_COMMAND_HPP -#define NAV_COMMAND_HPP - -namespace rip -{ - namespace subsystem - { - class NavCommand - { - public: - NavCommand(const Velocity& velocity, const Acceleration& acceleration); - - Velocity velocity() const; - - Acceleration acceleration() const; - - private: - Velocity m_velocity; - Acceleration m_acceleraton; - }; - } -} - - -#endif // NAV_COMMAND_HPP diff --git a/core/navigation/drivetrains/include/two_roboclaw_drivetrain.hpp b/core/navigation/drivetrains/include/two_roboclaw_drivetrain.hpp deleted file mode 100644 index 533cf8d..0000000 --- a/core/navigation/drivetrains/include/two_roboclaw_drivetrain.hpp +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef TWO_ROBOCLAW_DRIVE_TRAIN_HPP -#define TWO_ROBOCLAW_DRIVE_TRAIN_HPP - -#include - -#include "drivetrain.hpp" -#include "nav_command.hpp" - -namespace rip -{ - namespace subsystem - { - /** - * Abstract base class for the drive train - */ - class TwoRoboclawDriveTrain : public Drivetrain - { - using Roboclaw = roboclaw::Roboclaw; - public: - TwoRoboclawDriveTrain(const Roboclaw& left, const Roboclaw& right) - : m_left(left) - , m_right(right) - {} - - /** - * Drive all the motors - * @param power [-1, 1] - */ - virtual void drive(double power) override; - - /** - * Drive left and right separately - * @param left [-1, 1] - * @param right [-1, 1] - */ - virtual void drive(double left, double right) override; - - /** - * Drive all the motors - * - * all range from [-1, 1] - */ - virtual void drive(double front_left, double front_right, double back_left, double back_rightk) override; - - /** - * Single command to all motors - */ - virtual void drive(const NavCommand& command) override; - - /** - * Command left and right sides separately - */ - virtual void drive(const NavCommand& left, const NavCommand& right) override; - - /** - * Command four wheels separately - */ - virtual void drive(const NavCommand& front_left, const NavCommand& front_right, const NavCommand& back_left, const NavCommand& back_right) override; - - virtual void stop() override - { - // todo - } - - private: - Roboclaw m_left; - Roboclaw m_right; - }; - } -} - -#endif // DRIVE_TRAIN_HPP diff --git a/core/navigation/drivetrains/src/drivetrain.cpp b/core/navigation/drivetrains/src/drivetrain.cpp deleted file mode 100644 index 0ec09da..0000000 --- a/core/navigation/drivetrains/src/drivetrain.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "drivetrain.hpp" - -namespace rip -{ - namespace subsystem - { - - bool Drivetrain::diagnostic() - { - drive(1.0); - drive(-1.0); - - drive(1.0, 0.0); - drive(-1.0, 0.0); - drive(0.0, 1.0); - drive(0.0, -1.0); - - drive(1.0, 0.0, 0.0, 0.0); - drive(-1.0, 0.0, 0.0, 0.0); - drive(0.0, 1.0, 0.0, 0.0); - drive(0.0, -1.0, 0.0, 0.0); - drive(0.0, 0.0, 1.0, 0.0); - drive(0.0, 0.0, -1.0, 0.0); - drive(0.0, 0.0, 0.0, 1.0); - drive(0.0, 0.0, 0.0, -1.0); - } - } -} diff --git a/core/navigation/drivetrains/src/nav_command.cpp b/core/navigation/drivetrains/src/nav_command.cpp deleted file mode 100644 index 367f941..0000000 --- a/core/navigation/drivetrains/src/nav_command.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "nav_command.hpp" - -namespace rip -{ - namespace subsystem - { - NavCommand::NavCommand(const Velocity& velocity, const Acceleration& acceleration) - : m_velocity(velocity) - , m_acceleraton(acceleration) - {} - - Velocity NavCommand::velocity() const - { - return m_velocity; - } - - Acceleration NavCommand::acceleration() const - { - return m_acceleraton; - } - - } -} diff --git a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp new file mode 100644 index 0000000..97da501 --- /dev/null +++ b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp @@ -0,0 +1,92 @@ +#include "drivetrains/two_roboclaw_drivetrain.hpp" +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace drivetrains + { + TwoRoboclawDrivetrain::TwoRoboclawDrivetrain(const std::string& name, std::shared_ptr left, std::shared_ptr right, std::shared_ptr navx) + : Drivetrain(name) + , m_left(left) + , m_right(right) + , m_navx(navx) + {} + + TwoRoboclawDrivetrain::~TwoRoboclawDrivetrain() + { + stop(); + } + + void TwoRoboclawDrivetrain::drive(double power) + { + if (std::abs(power) > 1) + { + throw OutOfRangeException("out of range"); + } + m_left->drive(32767 * power); + m_right->drive(32767 * power); + } + + void TwoRoboclawDrivetrain::drive(double left, double right) + { + if (std::abs(left) > 1 || std::abs(right) > 1) + { + throw OutOfRangeException("out of range"); + } + m_left->drive(32767 * left); + m_right->drive(32767 * right); + } + + void TwoRoboclawDrivetrain::drive(double front_left, double front_right, double back_left, double back_right) + { + if (std::abs(front_left) > 1 || std::abs(front_right) > 1 || std::abs(back_left) > 1 || std::abs(back_right) > 1) + { + throw OutOfRangeException("out of range"); + } + + m_left->drive(Roboclaw::Motor::kM1, 32767 * front_left); + m_right->drive(Roboclaw::Motor::kM1, 32767 * front_right); + + m_left->drive(Roboclaw::Motor::kM2, 32767 * back_left); + m_right->drive(Roboclaw::Motor::kM2, 32767 * back_right); + } + + void TwoRoboclawDrivetrain::drive(const MotorDynamics& command) + { + m_left->setDynamics(command); + m_right->setDynamics(command); + } + + void TwoRoboclawDrivetrain::drive(const MotorDynamics& left, const MotorDynamics& right) + { + m_left->setDynamics(left); + m_right->setDynamics(right); + } + + void TwoRoboclawDrivetrain::drive(const MotorDynamics& front_left, const MotorDynamics& front_right, const MotorDynamics& back_left, const MotorDynamics& back_right) + { + m_left->setDynamics(Roboclaw::Motor::kM1, front_left); + m_right->setDynamics(Roboclaw::Motor::kM1, front_right); + + m_left->setDynamics(Roboclaw::Motor::kM2, back_left); + m_right->setDynamics(Roboclaw::Motor::kM2, back_right); + + } + + void TwoRoboclawDrivetrain::stop() + { + m_left->drive(0); + m_right->drive(0); + } + + bool TwoRoboclawDrivetrain::diagnostic() + { + // todo + } + } + }//subsystem +}//rip diff --git a/core/navigation/navx/CMakeLists.txt b/core/navigation/navx/CMakeLists.txt index 0dd7e3b..d25b510 100644 --- a/core/navigation/navx/CMakeLists.txt +++ b/core/navigation/navx/CMakeLists.txt @@ -1,4 +1,40 @@ cmake_minimum_required(VERSION 3.1) project(navx) -# dummy \ No newline at end of file +# Get all .cpp files except for main.cpp +file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS "include/navx/*.hpp") +file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES "src/*.cpp") + +# Create the navx library, set to c++11, link external libraries, and set include dir +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) +if(ENABLE_TESTING) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") +endif() +target_link_libraries(${PROJECT_NAME} units spdlog periphery pthread) +target_include_directories(${PROJECT_NAME} PUBLIC include) + +if(ENABLE_DIAG) + file(GLOB_RECURSE ${PROJECT_NAME}_DIAG_SOURCES "diag/*.cpp") + add_executable(${PROJECT_NAME}_diag ${${PROJECT_NAME}_DIAG_SOURCES}) + set_property(TARGET ${PROJECT_NAME}_diag PROPERTY CXX_STANDARD 11) + target_link_libraries(${PROJECT_NAME}_diag ${PROJECT_NAME}) + target_link_libraries(${PROJECT_NAME}_diag periphery units) +endif() + +if(ENABLE_TESTING) + # Get test source files and test data files + file(GLOB_RECURSE ${PROJECT_NAME}_TEST_SOURCES "test/*.cpp") + file(GLOB_RECURSE ${PROJECT_NAME}_TEST_DATA "test/data/*") + + # Copy test data to outdir/test + include(FileOutputs) + make_outputs(${CMAKE_CURRENT_SOURCE_DIR} "${${PROJECT_NAME}_TEST_DATA}" ${CMAKE_CURRENT_BINARY_DIR} testDataOutputs) + + # Create arduino_gen_test executable, set to c++11, link navx library and google test libraries + add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES} ${testDataOutputs}) + set_property(TARGET ${PROJECT_NAME}_test PROPERTY CXX_STANDARD 11) + target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) + target_link_libraries(${PROJECT_NAME}_test gmock gtest googletest_rip_macros) + +endif() diff --git a/core/navigation/navx/Doxyfile b/core/navigation/navx/Doxyfile index 9c89d4a..b50851f 100644 --- a/core/navigation/navx/Doxyfile +++ b/core/navigation/navx/Doxyfile @@ -1,17 +1,17 @@ # Doxyfile 1.8.11 # This file describes the settings to be used by the documentation system -# doxygen (www.doxygen.org) for a project. +# doxygen(www.doxygen.org) for a project. # -# All text after a double hash (##) is considered a comment and is placed in +# All text after a double hash(##) is considered a comment and is placed in # front of the TAG it is preceding. # -# All text after a single hash (#) is considered a comment and will be ignored. +# All text after a single hash(#) is considered a comment and will be ignored. # The format is: # TAG = value [value, ...] # For lists, items can also be appended using: # TAG += value [value, ...] -# Values that contain spaces should be placed between quotes (\" \"). +# Values that contain spaces should be placed between quotes(\" \"). #--------------------------------------------------------------------------- # Project related configuration options @@ -19,14 +19,14 @@ # This tag specifies the encoding used for all characters in the config file # that follow. The default is UTF-8 which is also the encoding used for all text -# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# before the first occurrence of this tag. Doxygen uses libiconv(or the iconv # built into libc) for the transcoding. See http://www.gnu.org/software/libiconv # for the list of possible encodings. # The default value is: UTF-8. DOXYFILE_ENCODING = UTF-8 -# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# The PROJECT_NAME tag is a single word(or a sequence of words surrounded by # double-quotes, unless you are using Doxywizard) that should identify the # project for which the documentation is generated. This name is used in the # title of most generated pages and in a few other places. @@ -53,7 +53,7 @@ PROJECT_BRIEF = PROJECT_LOGO = -# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# The OUTPUT_DIRECTORY tag is used to specify the(relative or absolute) path # into which the generated documentation will be written. If a relative path is # entered, it will be relative to the location where doxygen was started. If # left blank the current directory will be used. @@ -61,7 +61,7 @@ PROJECT_LOGO = OUTPUT_DIRECTORY = doc # If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- -# directories (in 2 levels) under the output directory of each output format and +# directories(in 2 levels) under the output directory of each output format and # will distribute the generated files over these directories. Enabling this # option can be useful when feeding doxygen a huge amount of source files, where # putting all generated files in the same directory would otherwise causes @@ -82,11 +82,11 @@ ALLOW_UNICODE_NAMES = NO # documentation generated by doxygen is written. Doxygen will use this # information to generate all constant output in the proper language. # Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, -# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), -# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, -# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), -# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, -# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English(United States), +# Esperanto, Farsi(Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en(Japanese with English messages), +# Korean, Korean-en(Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian(Farsi), Polish, Portuguese, Romanian, Russian, # Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, # Ukrainian and Vietnamese. # The default value is: English. @@ -95,7 +95,7 @@ OUTPUT_LANGUAGE = English # If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member # descriptions after the members that are listed in the file and class -# documentation (similar to Javadoc). Set to NO to disable this. +# documentation(similar to Javadoc). Set to NO to disable this. # The default value is: YES. BRIEF_MEMBER_DESC = YES @@ -114,7 +114,7 @@ REPEAT_BRIEF = YES # as the leading text of the brief description, will be stripped from the text # and the result, after processing the whole list, is used as the annotated # text. Otherwise, the brief description is used as-is. If left blank, the -# following values are used ($name is automatically replaced with the name of +# following values are used($name is automatically replaced with the name of # the entity):The $name class, The $name widget, The $name file, is, provides, # specifies, contains, represents, a, an and the. @@ -163,7 +163,7 @@ STRIP_FROM_PATH = STRIP_FROM_INC_PATH = -# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter(but # less readable) file names. This can be useful is your file systems doesn't # support long names like on DOS, Mac, or CD-ROM. # The default value is: NO. @@ -171,24 +171,24 @@ STRIP_FROM_INC_PATH = SHORT_NAMES = NO # If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the -# first line (until the first dot) of a Javadoc-style comment as the brief +# first line(until the first dot) of a Javadoc-style comment as the brief # description. If set to NO, the Javadoc-style will behave just like regular Qt- -# style comments (thus requiring an explicit @brief command for a brief +# style comments(thus requiring an explicit @brief command for a brief # description.) # The default value is: NO. JAVADOC_AUTOBRIEF = YES # If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first -# line (until the first dot) of a Qt-style comment as the brief description. If -# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# line(until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments(thus # requiring an explicit \brief command for a brief description.) # The default value is: NO. QT_AUTOBRIEF = NO # The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a -# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# multi-line C++ special comment block(i.e. a block of //! or /// comments) as # a brief description. This used to be the default behavior. The new default is # to treat a multi-line C++ comment block as a detailed description. Set this # tag to YES if you prefer the old behavior instead. @@ -223,14 +223,14 @@ TAB_SIZE = 4 # name=value # For example adding # "sideeffect=@par Side Effects:\n" -# will allow you to put the command \sideeffect (or @sideeffect) in the +# will allow you to put the command \sideeffect(or @sideeffect) in the # documentation, which will result in a user-defined paragraph with heading # "Side Effects:". You can put \n's in the value part of an alias to insert # newlines. ALIASES = -# This tag can be used to specify a number of word-keyword mappings (TCL only). +# This tag can be used to specify a number of word-keyword mappings(TCL only). # A mapping has the form "name=value". For example adding "class=itcl::class" # will allow you to use the command class in the itcl::class meaning. @@ -269,12 +269,12 @@ OPTIMIZE_OUTPUT_VHDL = NO # extension. Doxygen has a built-in mapping, but you can override or extend it # using this tag. The format is ext=language, where ext is a file extension, and # language is one of the parsers supported by doxygen: IDL, Java, Javascript, -# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: +# C#, C, C++, D, PHP, Objective-C, Python, Fortran(fixed format Fortran: # FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: # Fortran. In the later case the parser tries to guess whether the code is fixed # or free formatted code, this is the default for Fortran type files), VHDL. For -# instance to make doxygen treat .inc files as Fortran files (default is PHP), -# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# instance to make doxygen treat .inc files as Fortran files(default is PHP), +# and .f files as C(default is Fortran), use: inc=Fortran f=C. # # Note: For files without extension you can use no_extension as a placeholder. # @@ -301,10 +301,10 @@ MARKDOWN_SUPPORT = YES AUTOLINK_SUPPORT = YES -# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want -# to include (a tag file for) the STL sources as input, then you should set this +# If you use STL classes(i.e. std::string, std::vector, etc.) but do not want +# to include(a tag file for) the STL sources as input, then you should set this # tag to YES in order to let doxygen match functions declarations and -# definitions whose arguments contain STL classes (e.g. func(std::string); +# definitions whose arguments contain STL classes(e.g. func(std::string); # versus func(std::string) {}). This also make the inheritance and collaboration # diagrams that involve STL classes more complete and accurate. # The default value is: NO. @@ -317,7 +317,7 @@ BUILTIN_STL_SUPPORT = YES CPP_CLI_SUPPORT = NO -# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# Set the SIP_SUPPORT tag to YES if your project consists of sip(see: # http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen # will parse them like normal C++ but will assume all classes use public instead # of private inheritance when no explicit protection keyword is present. @@ -337,7 +337,7 @@ IDL_PROPERTY_SUPPORT = YES # If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC # tag is set to YES then doxygen will reuse the documentation of the first -# member in the group (if any) for the other members of the group. By default +# member in the group(if any) for the other members of the group. By default # all members of a group must be documented explicitly. # The default value is: NO. @@ -351,8 +351,8 @@ DISTRIBUTE_GROUP_DOC = NO GROUP_NESTED_COMPOUNDS = NO # Set the SUBGROUPING tag to YES to allow class member groups of the same type -# (for instance a group of public functions) to be put as a subgroup of that -# type (e.g. under the Public Functions section). Set it to NO to prevent +#(for instance a group of public functions) to be put as a subgroup of that +# type(e.g. under the Public Functions section). Set it to NO to prevent # subgrouping. Alternatively, this can be done per class using the # \nosubgrouping command. # The default value is: YES. @@ -360,8 +360,8 @@ GROUP_NESTED_COMPOUNDS = NO SUBGROUPING = YES # When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions -# are shown inside the group in which they are included (e.g. using \ingroup) -# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# are shown inside the group in which they are included(e.g. using \ingroup) +# instead of on a separate page(for HTML and Man pages) or section(for LaTeX # and RTF). # # Note that this feature does not work in combination with @@ -372,10 +372,10 @@ INLINE_GROUPED_CLASSES = NO # When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions # with only public data fields or simple typedef fields will be shown inline in -# the documentation of the scope in which they are defined (i.e. file, +# the documentation of the scope in which they are defined(i.e. file, # namespace, or group documentation), provided this scope is documented. If set -# to NO, structs, classes, and unions are shown on a separate page (for HTML and -# Man pages) or section (for LaTeX and RTF). +# to NO, structs, classes, and unions are shown on a separate page(for HTML and +# Man pages) or section(for LaTeX and RTF). # The default value is: NO. INLINE_SIMPLE_STRUCTS = NO @@ -436,7 +436,7 @@ EXTRACT_PACKAGE = NO EXTRACT_STATIC = YES -# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes(and structs) defined # locally in source files will be included in the documentation. If set to NO, # only classes defined in header files are included. Does not have any effect # for Java sources. @@ -478,7 +478,7 @@ HIDE_UNDOC_MEMBERS = NO HIDE_UNDOC_CLASSES = NO # If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend -# (class|struct|union) declarations. If set to NO, these declarations will be +#(class|struct|union) declarations. If set to NO, these declarations will be # included in the documentation. # The default value is: NO. @@ -514,7 +514,7 @@ CASE_SENSE_NAMES = YES HIDE_SCOPE_NAMES = NO -# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# If the HIDE_COMPOUND_REFERENCE tag is set to NO(default) then doxygen will # append additional text to a page's title, such as Class Reference. If set to # YES the compound reference will be hidden. # The default value is: NO. @@ -547,7 +547,7 @@ FORCE_LOCAL_INCLUDES = NO INLINE_INFO = YES # If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the -# (detailed) documentation of file and class members alphabetically by member +#(detailed) documentation of file and class members alphabetically by member # name. If set to NO, the members will appear in declaration order. # The default value is: YES. @@ -562,7 +562,7 @@ SORT_MEMBER_DOCS = YES SORT_BRIEF_DOCS = NO # If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the -# (brief and detailed) documentation of class members so that constructors and +#(brief and detailed) documentation of class members so that constructors and # destructors are listed first. If set to NO the constructors will appear in the # respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. # Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief @@ -600,25 +600,25 @@ SORT_BY_SCOPE_NAME = NO STRICT_PROTO_MATCHING = NO -# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# The GENERATE_TODOLIST tag can be used to enable(YES) or disable(NO) the todo # list. This list is created by putting \todo commands in the documentation. # The default value is: YES. GENERATE_TODOLIST = YES -# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# The GENERATE_TESTLIST tag can be used to enable(YES) or disable(NO) the test # list. This list is created by putting \test commands in the documentation. # The default value is: YES. GENERATE_TESTLIST = YES -# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# The GENERATE_BUGLIST tag can be used to enable(YES) or disable(NO) the bug # list. This list is created by putting \bug commands in the documentation. # The default value is: YES. GENERATE_BUGLIST = YES -# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# The GENERATE_DEPRECATEDLIST tag can be used to enable(YES) or disable(NO) # the deprecated list. This list is created by putting \deprecated commands in # the documentation. # The default value is: YES. @@ -651,21 +651,21 @@ SHOW_USED_FILES = YES # Set the SHOW_FILES tag to NO to disable the generation of the Files page. This # will remove the Files entry from the Quick Index and from the Folder Tree View -# (if specified). +#(if specified). # The default value is: YES. SHOW_FILES = YES # Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces # page. This will remove the Namespaces entry from the Quick Index and from the -# Folder Tree View (if specified). +# Folder Tree View(if specified). # The default value is: YES. SHOW_NAMESPACES = YES # The FILE_VERSION_FILTER tag can be used to specify a program or script that -# doxygen should invoke to get the current version for each file (typically from -# the version control system). Doxygen will invoke the program by executing (via +# doxygen should invoke to get the current version for each file(typically from +# the version control system). Doxygen will invoke the program by executing(via # popen()) the command command input-file, where command is the value of the # FILE_VERSION_FILTER tag, and input-file is the name of an input file provided # by doxygen. Whatever the program writes to standard output is used as the file @@ -708,7 +708,7 @@ CITE_BIB_FILES = QUIET = NO # The WARNINGS tag can be used to turn on/off the warning messages that are -# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# generated to standard error(stderr) by doxygen. If WARNINGS is set to YES # this implies that the warnings are on. # # Tip: Turn warnings on while writing the documentation. @@ -749,7 +749,7 @@ WARN_AS_ERROR = NO # can produce. The string should contain the $file, $line, and $text tags, which # will be replaced by the file and line number from which the warning originated # and the warning text. Optionally the format may contain $version, which will -# be replaced by the version of the file (if it could be obtained via +# be replaced by the version of the file(if it could be obtained via # FILE_VERSION_FILTER) # The default value is: $file:$line: $text. @@ -757,7 +757,7 @@ WARN_FORMAT = "$file:$line: $text" # The WARN_LOGFILE tag can be used to specify a file to which warning and error # messages should be written. If left blank the output is written to standard -# error (stderr). +# error(stderr). WARN_LOGFILE = @@ -775,15 +775,15 @@ INPUT = src include # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses -# libiconv (or the iconv built into libc) for the transcoding. See the libiconv -# documentation (see: http://www.gnu.org/software/libiconv) for the list of +# libiconv(or the iconv built into libc) for the transcoding. See the libiconv +# documentation(see: http://www.gnu.org/software/libiconv) for the list of # possible encodings. # The default value is: UTF-8. INPUT_ENCODING = UTF-8 # If the value of the INPUT tag contains directories, you can use the -# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# FILE_PATTERNS tag to specify one or more wildcard patterns(like *.cpp and # *.h) to filter out the source-files in the directories. # # Note that for custom extensions or not directly supported extensions you also @@ -814,7 +814,7 @@ RECURSIVE = YES EXCLUDE = # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or -# directories that are symbolic links (a Unix file system feature) are excluded +# directories that are symbolic links(a Unix file system feature) are excluded # from the input. # The default value is: NO. @@ -830,7 +830,7 @@ EXCLUDE_SYMLINKS = NO EXCLUDE_PATTERNS = # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names -# (namespaces, classes, functions, etc.) that should be excluded from the +#(namespaces, classes, functions, etc.) that should be excluded from the # output. The symbol name can be a fully qualified name, a word, or if the # wildcard * is used, a substring. Examples: ANamespace, AClass, # AClass::ANamespace, ANamespace::*Test @@ -841,13 +841,13 @@ EXCLUDE_PATTERNS = EXCLUDE_SYMBOLS = # The EXAMPLE_PATH tag can be used to specify one or more files or directories -# that contain example code fragments that are included (see the \include +# that contain example code fragments that are included(see the \include # command). EXAMPLE_PATH = # If the value of the EXAMPLE_PATH tag contains directories, you can use the -# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern(like *.cpp and # *.h) to filter out the source-files in the directories. If left blank all # files are included. @@ -861,14 +861,14 @@ EXAMPLE_PATTERNS = EXAMPLE_RECURSIVE = NO # The IMAGE_PATH tag can be used to specify one or more files or directories -# that contain images that are to be included in the documentation (see the +# that contain images that are to be included in the documentation(see the # \image command). IMAGE_PATH = # The INPUT_FILTER tag can be used to specify a program that doxygen should # invoke to filter for each input file. Doxygen will invoke the filter program -# by executing (via popen()) the command: +# by executing(via popen()) the command: # # # @@ -890,7 +890,7 @@ INPUT_FILTER = # The FILTER_PATTERNS tag can be used to specify filters on a per file pattern # basis. Doxygen will compare the file name with each pattern and apply the # filter if there is a match. The filters are a list of the form: pattern=filter -# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +#(like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how # filters are used. If the FILTER_PATTERNS tag is empty or if none of the # patterns match the file name, INPUT_FILTER is applied. # @@ -900,24 +900,24 @@ INPUT_FILTER = FILTER_PATTERNS = -# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter(if set using # INPUT_FILTER) will also be used to filter the input files that are used for -# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# producing the source files to browse(i.e. when SOURCE_BROWSER is set to YES). # The default value is: NO. FILTER_SOURCE_FILES = NO # The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file -# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# pattern. A pattern will override the setting for FILTER_PATTERN(if any) and # it is also possible to disable source filtering for a specific pattern using -# *.ext= (so without naming a filter). +# *.ext=(so without naming a filter). # This tag requires that the tag FILTER_SOURCE_FILES is set to YES. FILTER_SOURCE_PATTERNS = # If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that # is part of the input, its contents will be placed on the main page -# (index.html). This can be useful if you have a project on for instance GitHub +#(index.html). This can be useful if you have a project on for instance GitHub # and want to reuse the introduction page also for the doxygen output. USE_MDFILE_AS_MAINPAGE = @@ -968,7 +968,7 @@ REFERENCES_RELATION = NO REFERENCES_LINK_SOURCE = YES -# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# If SOURCE_TOOLTIPS is enabled(the default) then hovering a hyperlink in the # source code will show a tooltip with additional information such as prototype, # brief description and links to the definition and documentation. Since this # will make the HTML file larger and loading of large files a bit slower, you @@ -981,7 +981,7 @@ SOURCE_TOOLTIPS = YES # If the USE_HTAGS tag is set to YES then the references to source code will # point to the HTML generated by the htags(1) tool instead of doxygen built-in # source browser. The htags tool is part of GNU's global source tagging system -# (see http://www.gnu.org/software/global/global.html). You will need version +#(see http://www.gnu.org/software/global/global.html). You will need version # 4.8.6 or higher. # # To use it do the following: @@ -990,8 +990,8 @@ SOURCE_TOOLTIPS = YES # - Make sure the INPUT points to the root of the source tree # - Run doxygen as normal # -# Doxygen will invoke htags (and that will in turn invoke gtags), so these -# tools must be available from the command line (i.e. in the search path). +# Doxygen will invoke htags(and that will in turn invoke gtags), so these +# tools must be available from the command line(i.e. in the search path). # # The result: instead of the source browser generated by doxygen, the links to # source code will now point to the output of htags. @@ -1009,7 +1009,7 @@ USE_HTAGS = NO VERBATIM_HEADERS = YES # If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the -# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the +# clang parser(see: http://clang.llvm.org/) for more accurate parsing at the # cost of reduced performance. This can be particularly helpful with template # rich C++ code for which doxygen's built-in parser lacks the necessary type # information. @@ -1047,7 +1047,7 @@ COLS_IN_ALPHA_INDEX = 5 # In case all classes in a project start with a common prefix, all classes will # be put under the same header in the alphabetical index. The IGNORE_PREFIX tag -# can be used to specify a prefix (or a list of prefixes) that should be ignored +# can be used to specify a prefix(or a list of prefixes) that should be ignored # while generating the index headers. # This tag requires that the tag ALPHABETICAL_INDEX is set to YES. @@ -1071,7 +1071,7 @@ GENERATE_HTML = YES HTML_OUTPUT = html # The HTML_FILE_EXTENSION tag can be used to specify the file extension for each -# generated HTML page (for example: .htm, .php, .asp). +# generated HTML page(for example: .htm, .php, .asp). # The default value is: .html. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1082,7 +1082,7 @@ HTML_FILE_EXTENSION = .html # standard header. # # To get valid HTML the header file that includes any scripts and style sheets -# that doxygen needs, which is dependent on the configuration options used (e.g. +# that doxygen needs, which is dependent on the configuration options used(e.g. # the setting GENERATE_TREEVIEW). It is highly recommended to start with a # default header using # doxygen -w html new_header.html new_footer.html new_stylesheet.css @@ -1113,7 +1113,7 @@ HTML_FOOTER = # See also section "Doxygen usage" for information on how to generate the style # sheet that doxygen normally uses. # Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as -# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# it is more robust and this tag(HTML_STYLESHEET) will in the future become # obsolete. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1125,7 +1125,7 @@ HTML_STYLESHEET = # This is preferred over using HTML_STYLESHEET since it does not replace the # standard style sheet and is therefore more robust against future updates. # Doxygen will copy the style sheet files to the output directory. -# Note: The order of the extra style sheet files is of importance (e.g. the last +# Note: The order of the extra style sheet files is of importance(e.g. the last # style sheet in the list overrules the setting of the previous ones in the # list). For an example see the documentation. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1153,7 +1153,7 @@ HTML_EXTRA_FILES = HTML_COLORSTYLE_HUE = 220 -# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# The HTML_COLORSTYLE_SAT tag controls the purity(or saturation) of the colors # in the HTML output. For a value of 0 the output will use grayscales only. A # value of 255 will produce the most vivid colors. # Minimum value: 0, maximum value: 255, default value: 100. @@ -1192,7 +1192,7 @@ HTML_DYNAMIC_SECTIONS = NO # With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries # shown in the various tree structured indices initially; the user can expand # and collapse entries dynamically later on. Doxygen will expand the tree to -# such a level that at most the specified number of entries are visible (unless +# such a level that at most the specified number of entries are visible(unless # a fully collapsed tree already exceeds this amount). So setting the number of # entries 1 will produce a full collapsed tree by default. 0 is a special value # representing an infinite number of entries and will result in a full expanded @@ -1204,8 +1204,8 @@ HTML_INDEX_NUM_ENTRIES = 100 # If the GENERATE_DOCSET tag is set to YES, additional index files will be # generated that can be used as input for Apple's Xcode 3 integrated development -# environment (see: http://developer.apple.com/tools/xcode/), introduced with -# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# environment(see: http://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5(Leopard). To create a documentation set, doxygen will generate a # Makefile in the HTML output directory. Running make will produce the docset in # that directory and running make install will install the docset in # ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at @@ -1218,7 +1218,7 @@ GENERATE_DOCSET = NO # This tag determines the name of the docset feed. A documentation feed provides # an umbrella under which multiple documentation sets from a single provider -# (such as a company or product suite) can be grouped. +#(such as a company or product suite) can be grouped. # The default value is: Doxygen generated docs. # This tag requires that the tag GENERATE_DOCSET is set to YES. @@ -1249,13 +1249,13 @@ DOCSET_PUBLISHER_NAME = Publisher # If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three # additional HTML index files: index.hhp, index.hhc, and index.hhk. The # index.hhp is a project file that can be read by Microsoft's HTML Help Workshop -# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +#(see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on # Windows. # # The HTML Help Workshop contains a compiler that can convert all HTML output -# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# generated by doxygen into a single compiled HTML file(.chm). Compiled HTML # files are now used as the Windows 98 help format, and will replace the old -# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# Windows help format(.hlp) on all Windows platforms in the future. Compressed # HTML files also contain an index, a table of contents, and you can search for # words in the documentation. The HTML workshop also contains a viewer for # compressed HTML files. @@ -1271,8 +1271,8 @@ GENERATE_HTMLHELP = NO CHM_FILE = -# The HHC_LOCATION tag can be used to specify the location (absolute path -# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# The HHC_LOCATION tag can be used to specify the location(absolute path +# including file name) of the HTML help compiler(hhc.exe). If non-empty, # doxygen will try to run the HTML help compiler on the generated index.hhp. # The file has to be specified with full path. # This tag requires that the tag GENERATE_HTMLHELP is set to YES. @@ -1280,20 +1280,20 @@ CHM_FILE = HHC_LOCATION = # The GENERATE_CHI flag controls if a separate .chi index file is generated -# (YES) or that it should be included in the master .chm file (NO). +#(YES) or that it should be included in the master .chm file(NO). # The default value is: NO. # This tag requires that the tag GENERATE_HTMLHELP is set to YES. GENERATE_CHI = NO -# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index(hhk), content(hhc) # and project file content. # This tag requires that the tag GENERATE_HTMLHELP is set to YES. CHM_INDEX_ENCODING = # The BINARY_TOC flag controls whether a binary table of contents is generated -# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +#(YES) or a normal table of contents(NO) in the .chm file. Furthermore it # enables the Previous and Next buttons. # The default value is: NO. # This tag requires that the tag GENERATE_HTMLHELP is set to YES. @@ -1310,7 +1310,7 @@ TOC_EXPAND = NO # If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and # QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that # can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help -# (.qch) of the generated HTML documentation. +#(.qch) of the generated HTML documentation. # The default value is: NO. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1325,7 +1325,7 @@ QCH_FILE = # The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help # Project output. For more information please see Qt Help Project / Namespace -# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +#(see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). # The default value is: org.doxygen.Project. # This tag requires that the tag GENERATE_QHP is set to YES. @@ -1333,7 +1333,7 @@ QHP_NAMESPACE = org.doxygen.Project # The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt # Help Project output. For more information please see Qt Help Project / Virtual -# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- +# Folders(see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- # folders). # The default value is: doc. # This tag requires that the tag GENERATE_QHP is set to YES. @@ -1342,7 +1342,7 @@ QHP_VIRTUAL_FOLDER = doc # If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom # filter to add. For more information please see Qt Help Project / Custom -# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# Filters(see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- # filters). # This tag requires that the tag GENERATE_QHP is set to YES. @@ -1350,14 +1350,14 @@ QHP_CUST_FILTER_NAME = # The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the # custom filter to add. For more information please see Qt Help Project / Custom -# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# Filters(see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- # filters). # This tag requires that the tag GENERATE_QHP is set to YES. QHP_CUST_FILTER_ATTRS = # The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this -# project's filter section matches. Qt Help Project / Filter Attributes (see: +# project's filter section matches. Qt Help Project / Filter Attributes(see: # http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). # This tag requires that the tag GENERATE_QHP is set to YES. @@ -1392,7 +1392,7 @@ ECLIPSE_DOC_ID = org.doxygen.Project # If you want full control over the layout of the generated HTML pages it might # be necessary to disable the index and replace it with your own. The -# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# DISABLE_INDEX tag can be used to turn on/off the condensed index(tabs) at top # of each HTML page. A value of NO enables the index and the value YES disables # it. Since the tabs in the index contain the same information as the navigation # tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. @@ -1404,10 +1404,10 @@ DISABLE_INDEX = NO # The GENERATE_TREEVIEW tag is used to specify whether a tree-like index # structure should be generated to display hierarchical information. If the tag # value is set to YES, a side panel will be generated containing a tree-like -# index structure (just like the one that is generated for HTML Help). For this +# index structure(just like the one that is generated for HTML Help). For this # to work a browser that supports JavaScript, DHTML, CSS and frames is required -# (i.e. any modern browser). Windows users are probably better off using the -# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +#(i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets(see HTML_EXTRA_STYLESHEET) one can # further fine-tune the look of the index. As an example, the default style # sheet generated by doxygen has an example that shows how to put an image at # the root of the tree instead of the PROJECT_NAME. Since the tree basically has @@ -1428,8 +1428,8 @@ GENERATE_TREEVIEW = YES ENUM_VALUES_PER_LINE = 4 -# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used -# to set the initial width (in pixels) of the frame in which the tree is shown. +# If the treeview is enabled(see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width(in pixels) of the frame in which the tree is shown. # Minimum value: 0, maximum value: 1500, default value: 250. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1462,7 +1462,7 @@ FORMULA_FONTSIZE = 10 FORMULA_TRANSPARENT = YES -# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax(see # http://www.mathjax.org) which uses client side Javascript for the rendering # instead of using pre-rendered bitmaps. Use this if you do not have LaTeX # installed or if you want to formulas look prettier in the HTML output. When @@ -1474,10 +1474,10 @@ FORMULA_TRANSPARENT = YES USE_MATHJAX = NO # When MathJax is enabled you can set the default output format to be used for -# the MathJax output. See the MathJax site (see: +# the MathJax output. See the MathJax site(see: # http://docs.mathjax.org/en/latest/output.html) for more details. -# Possible values are: HTML-CSS (which is slower, but has the best -# compatibility), NativeMML (i.e. MathML) and SVG. +# Possible values are: HTML-CSS(which is slower, but has the best +# compatibility), NativeMML(i.e. MathML) and SVG. # The default value is: HTML-CSS. # This tag requires that the tag USE_MATHJAX is set to YES. @@ -1505,7 +1505,7 @@ MATHJAX_EXTENSIONS = # The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces # of code that will be used on startup of the MathJax code. See the MathJax site -# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +#(see: http://docs.mathjax.org/en/latest/output.html) for more details. For an # example see the documentation. # This tag requires that the tag USE_MATHJAX is set to YES. @@ -1514,12 +1514,12 @@ MATHJAX_CODEFILE = # When the SEARCHENGINE tag is enabled doxygen will generate a search box for # the HTML output. The underlying search engine uses javascript and DHTML and # should work on any modern browser. Note that when using HTML help -# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +#(GENERATE_HTMLHELP), Qt help(GENERATE_QHP), or docsets(GENERATE_DOCSET) # there is already a search function so this one should typically be disabled. # For large projects the javascript based search engine can be slow, then # enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to # search using the keyboard; to jump to the search box use + S -# (what the is depends on the OS and browser, but it is typically +#(what the is depends on the OS and browser, but it is typically # , /