diff --git a/.gitignore b/.gitignore index 890bf54..999e036 100644 --- a/.gitignore +++ b/.gitignore @@ -1,26 +1,221 @@ -###Git Ignore Filetypes### +Release/ +Debug/ +Build/ +Drivers/ +Catch/ +googletest/ +TestFootage + -# JetBrains -.idea/ -#compiled source# -################# + +# Created by https://www.gitignore.io/api/macos,windows,linux,jetbrains,cmake,c++,node + +### C++ ### +# Prerequisites +*.d +*.cbp + +# Compiled Object files +*.slo +*.lo *.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries *.so +*.dylib +*.dll -Release/ -Debug/ -Build/ +# Fortran module files +*.mod +*.smod -#cmake-generated files# -.cmake +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### CMake ### CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +Makefile +cmake-build-* +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake + +### JetBrains ### +# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm +# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 + +# User-specific stuff: +.idea/ + +## File-based project format: +*.iws + +## Plugin-specific files: + +# IntelliJ +/out/ + +# mpeltonen/sbt-idea plugin +.idea_modules/ + +# JIRA plugin +atlassian-ide-plugin.xml + +# Crashlytics plugin (for Android Studio and IntelliJ) +com_crashlytics_export_strings.xml +crashlytics.properties +crashlytics-build.properties +fabric.properties + +### JetBrains Patch ### +# Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721 + +# *.iml +# modules.xml +# .idea/misc.xml +# *.ipr + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory -# NPM +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +*.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### Node ### +# Logs +logs +*.log +npm-debug.log* +yarn-debug.log* +yarn-error.log* + +# Runtime data +pids +*.pid +*.seed +*.pid.lock + +# Directory for instrumented libs generated by jscoverage/JSCover +lib-cov + +# Coverage directory used by tools like istanbul +coverage + +# nyc test coverage +.nyc_output + +# Grunt intermediate storage (http://gruntjs.com/creating-plugins#storing-task-files) +.grunt + +# Bower dependency directory (https://bower.io/) +bower_components + +# node-waf configuration +.lock-wscript + +# Compiled binary addons (http://nodejs.org/api/addons.html) +build/Release + +# Dependency directories node_modules/ +jspm_packages/ -# Submodules -Drivers/ -googletest/ +# Typescript v1 declaration files +typings/ -TestFootage +# Optional npm cache directory +.npm + +# Optional eslint cache +.eslintcache + +# Optional REPL history +.node_repl_history + +# Output of 'npm pack' +*.tgz + +# Yarn Integrity file +.yarn-integrity + +# dotenv environment variables file +.env + + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Folder config file +Desktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msm +*.msp + +# Windows shortcuts +*.lnk + +# End of https://www.gitignore.io/api/macos,windows,linux,jetbrains,cmake,c++,node diff --git a/.gitmodules b/.gitmodules index 109e496..68ecb8a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "googletest"] path = Libraries/googletest url = https://github.com/google/googletest.git +[submodule "Libraries/Catch"] + path = Libraries/Catch + url = https://github.com/philsquared/Catch.git diff --git a/ArduinoController/ArduinoController.ino b/ArduinoController/ArduinoController.ino index 0cc4161..cc69aba 100644 --- a/ArduinoController/ArduinoController.ino +++ b/ArduinoController/ArduinoController.ino @@ -1,5 +1,3 @@ -#include - #include "ThrustController.h" #include "EscController.h" #include "LedController.h" @@ -8,7 +6,6 @@ #include "KillSwitchController.h" #include "VoltageController.h" #include "StartController.h" -#include "PinJSON.h" const uint32_t CONTROLLER_CNT = 13u; class IController* controllers[CONTROLLER_CNT]; diff --git a/ArduinoController/EscController.h b/ArduinoController/EscController.h index dd90274..bd286ac 100644 --- a/ArduinoController/EscController.h +++ b/ArduinoController/EscController.h @@ -13,6 +13,7 @@ class EscController : public IController { GpioPin::ESC_S5_PIN, GpioPin::ESC_S6_PIN }; + public: EscController() { for(uint32_t i = 0; i < GPIO_CNT; i++) { @@ -20,22 +21,7 @@ class EscController : public IController { digitalWrite(GPIO_PINS[i], HIGH); } } - - void setupPins_() { - for(int i = 0; i < pinCount; i++) { - pinMode(pins_[i], OUTPUT); - digitalWrite(pins_[i], HIGH); - } - } - -public: - - EscController(JsonObject& pins) { - pinCount = pins["gpio"].size(); - assignPins_(pins["gpio"]); - setupPins_(); - } - + void execute() { uint8_t toggle = SerialTools::readByte(); for(uint32_t i = 0; i < GPIO_CNT; i++) { @@ -50,4 +36,4 @@ class EscController : public IController { } }; -#undef GPIO_CNT \ No newline at end of file +#undef GPIO_CNT diff --git a/ArduinoController/PinJSON.h b/ArduinoController/PinJSON.h deleted file mode 100644 index 8737713..0000000 --- a/ArduinoController/PinJSON.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef PIN_JSON_H -#define PIN_JSON_H - -namespace PinJSON { - char json[] = "{" - "\"voltage\" : {" - "\"volt\" : 10" - "}," - "\"thrust\" : {" - "\"left_forward\" : 13," - "\"right_forward\" : 12," - "\"left_strafe\" : 11," - "\"right_strafe\" : 9," - "\"front_dive\" : 8," - "\"back_dive\" : 7" - "}," - "\"light\" : {" - "\"lights\" : 45" - "}," - "\"led\" : {" - "\"green\" : 22," - "\"white\" : 52" - "}," - "\"kill_switch\" : {" - "\"stat_led\" : 22" - "}," - "\"esc\" : {" - "\"gpio\" : [36, 39, 41, 47, 49, 53]" - "}" - "}"; -} - -#endif // PIN_JSON_H diff --git a/BoneCentral/Brain/CppInterface/ThrustController.js b/BoneCentral/Brain/CppInterface/ThrustController.js index 264b492..6016163 100644 --- a/BoneCentral/Brain/CppInterface/ThrustController.js +++ b/BoneCentral/Brain/CppInterface/ThrustController.js @@ -8,45 +8,50 @@ module.exports = (function(){ this._cmdOut = cmdOut; } - ThrustController.prototype.goDirection = function(forward, strafe, dive) { + ThrustController.prototype.goDirection = function(move, secondDive, dive) { var cmdString = - "goDirection " + forward + " " + strafe + " " + " " + dive + "\n"; + "goDirection " + move + " " + secondDive + " " + " " + dive + "\n"; this._cmdOut.write(cmdString); }; - ThrustController.prototype.faceDirection = function(yaw, dive){ + ThrustController.prototype.rotate = function(yaw, pitch, roll){ var cmdString = - "faceDirection " + yaw + " " + dive + "\n"; + "rotate " + yaw + " " + pitch + " " + roll + "\n"; this._cmdOut.write(cmdString); }; - ThrustController.prototype.setDiveOffset = function(front, back) { - var cmdString = "setDiveOffset " + front + " " + back + "\n"; + ThrustController.prototype.move = function (throttle) { + var cmdString = "move " + throttle + "\n"; this._cmdOut.write(cmdString); }; - ThrustController.prototype.setForwardTrim = function(left, right) { - var cmdString = "setForwardTrim " + left + " " + right + "\n"; + ThrustController.prototype.secondaryDive = function (throttle) { + var cmdString = "secondaryDive " + throttle + "\n"; this._cmdOut.write(cmdString); }; - ThrustController.prototype.setStrafeTrim = function(left, right) { - var cmdString = "setStrafeTrim " + left + " " + right + "\n"; + ThrustController.prototype.dive = function (throttle) { + var cmdString = "primaryDive " + throttle + "\n"; this._cmdOut.write(cmdString); }; - ThrustController.prototype.setDiveTrim = function(front, back) { - var cmdString = "setDiveTrim " + front + " " + back + "\n"; + ThrustController.prototype.yaw = function (throttle) { + var cmdString = "yaw " + throttle + "\n"; this._cmdOut.write(cmdString); }; - ThrustController.prototype.thrustForward = function (left, right) { - var cmdString = "thrustForward " + left + " " + right + "\n"; + ThrustController.prototype.pitch = function (throttle) { + var cmdString = "pitch " + throttle + "\n"; this._cmdOut.write(cmdString); }; - ThrustController.prototype.dive = function (forward, rear) { - var cmdString = "dive " + forward + " " + rear + "\n"; + ThrustController.prototype.roll = function (throttle) { + var cmdString = "roll " + throttle + "\n"; + this._cmdOut.write(cmdString); + }; + + ThrustController.prototype.kill = function () { + var cmdString = "killThrust\n"; this._cmdOut.write(cmdString); }; diff --git a/BoneCentral/Peripherals/Peripherals.Core/include/CommandDispatcher.h b/BoneCentral/Peripherals/Peripherals.Core/include/CommandDispatcher.h index 1c17d4f..cfd21bb 100644 --- a/BoneCentral/Peripherals/Peripherals.Core/include/CommandDispatcher.h +++ b/BoneCentral/Peripherals/Peripherals.Core/include/CommandDispatcher.h @@ -29,15 +29,16 @@ class CommandDispatcher { bool shouldExit_; void dispatchCommand(std::stringstream& cmd); + void goDirection(std::stringstream& cmdString); - void faceDirection(std::stringstream& cmdString); - void thrustForward(std::stringstream& cmdString); + void rotate(std::stringstream& cmdString); + void move(std::stringstream& cmdString); + void strafe(std::stringstream& cmdString); void dive(std::stringstream& cmdString); - - void setForwardTrim(std::stringstream& cmdString); - void setStrafeTrim(std::stringstream& cmdString); - void setDiveTrim(std::stringstream& cmdString); - void setDiveOffset(std::stringstream& cmdString); + void yaw(std::stringstream& cmdString); + void pitch(std::stringstream& cmdString); + void roll(std::stringstream& cmdString); + void kill(); void _getAcceleration(); void _getAngularAcceleration(); diff --git a/BoneCentral/Peripherals/Peripherals.Core/include/Interfaces/IThrusterFactory.h b/BoneCentral/Peripherals/Peripherals.Core/include/Interfaces/IThrusterFactory.h index 949b23b..e92f362 100644 --- a/BoneCentral/Peripherals/Peripherals.Core/include/Interfaces/IThrusterFactory.h +++ b/BoneCentral/Peripherals/Peripherals.Core/include/Interfaces/IThrusterFactory.h @@ -10,12 +10,12 @@ class IThrusterFactory { public: - virtual std::shared_ptr createLeftForwardThruster() = 0; - virtual std::shared_ptr createRightForwardThruster() = 0; - virtual std::shared_ptr createLeftStrafeThruster() = 0; - virtual std::shared_ptr createRightStrafeThruster() = 0; - virtual std::shared_ptr createForwardDiveThruster() = 0; - virtual std::shared_ptr createRearDiveThruster() = 0; + virtual std::shared_ptr createMoveThruster() = 0; + virtual std::shared_ptr createStrafeThruster() = 0; + virtual std::shared_ptr createDiveThruster() = 0; + virtual std::shared_ptr createYawThruster() = 0; + virtual std::shared_ptr createPitchThruster() = 0; + virtual std::shared_ptr createRollThruster() = 0; }; #endif //PERIPHERALS_I_THRUSTER_FACTORY_H diff --git a/BoneCentral/Peripherals/Peripherals.Core/include/ThrustController.h b/BoneCentral/Peripherals/Peripherals.Core/include/ThrustController.h index a0e1b6a..37aa1b6 100644 --- a/BoneCentral/Peripherals/Peripherals.Core/include/ThrustController.h +++ b/BoneCentral/Peripherals/Peripherals.Core/include/ThrustController.h @@ -16,42 +16,33 @@ class ThrustController { public: ThrustController(IThrusterFactory& thrusterFactory, std::shared_ptr logger); - void goDirection(float forward, float strafe, float dive); - void faceDirection(float yaw, float dive); - void thrustForward(float left, float right); - void dive(float front, float rear); - void setForwardTrim(float left, float right); - void setStrafeTrim(float left, float right); - void setDiveTrim(float front, float back); - void setDiveOffset(float front, float back); - void killAllThruster(); + void goDirection(float _move_, float _strafe_, float _dive_); + void rotate(float _yaw_, float _pitch_, float _roll_); + void move(float throttle); + void strafe(float throttle); + void dive(float throttle); + void yaw(float throttle); + void pitch(float throttle); + void roll(float throttle); + void killAllThrusters(); ~ThrustController(); private: - std::shared_ptr leftForwardThruster_; - std::shared_ptr rightForwardThruster_; - std::shared_ptr leftStrafeThruster_; - std::shared_ptr rightStrafeThruster_; - std::shared_ptr forwardDiveThruster_; - std::shared_ptr rearDiveThruster_; + std::shared_ptr moveThruster_; + std::shared_ptr strafeThruster_; + std::shared_ptr diveThruster_; + std::shared_ptr yawThruster_; + std::shared_ptr pitchThruster_; + std::shared_ptr rollThruster_; std::shared_ptr logger_; - const float maxPower = 1.0f; - const float minPower = 0.0f; - const float reverseRatio = 0.84507f; - const float strafeRatio = 0.7f; - - FloatPair forwardTrim; - FloatPair diveTrim; - FloatPair diveOffset; - FloatPair strafeTrim; - - FloatPair getReciprocalValues(float value); - float getSafeOffset(float a, float b); - float getScaleToMaxPower(float left, float right); - float getMaxMag(float left, float right); - void setThrust(FloatPair forwardPair, FloatPair strafePair, FloatPair divePair); + const float MAX_THROTTLE = 1.0f; + const float MIN_THROTTLE = -1.0f; + // const float reverseRatio = 0.84507f; + // const float strafeRatio = 0.7f; + + float getSafeThrottle(float throttle); }; #endif diff --git a/BoneCentral/Peripherals/Peripherals.Core/src/CommandDispatcher.cpp b/BoneCentral/Peripherals/Peripherals.Core/src/CommandDispatcher.cpp index 213fc2f..f45bcc3 100644 --- a/BoneCentral/Peripherals/Peripherals.Core/src/CommandDispatcher.cpp +++ b/BoneCentral/Peripherals/Peripherals.Core/src/CommandDispatcher.cpp @@ -34,29 +34,33 @@ void CommandDispatcher::runLoop() { void CommandDispatcher::dispatchCommand(std::stringstream& cmdString) { std::string cmd; cmdString >> cmd; - if(cmd == "goDirection") goDirection(cmdString); - else if(cmd == "faceDirection") faceDirection(cmdString); - else if(cmd == "thrustForward") thrustForward(cmdString); - else if(cmd == "dive") dive(cmdString); - else if(cmd == "turnOnEscs") powerManager_.turnOnEscs(); - else if(cmd == "turnOffEscs") powerManager_.turnOffEscs(); - else if(cmd == "switchLights") lights_.switchLights(); - else if(cmd == "turnOnImu") powerManager_.turnOnImuSensor(); - else if(cmd == "turnOffImu") powerManager_.turnOffImuSensor(); - else if(cmd == "setForwardTrim") setForwardTrim(cmdString); - else if(cmd == "setStrafeTrim") setStrafeTrim(cmdString); - else if(cmd == "setDiveTrim") setDiveTrim(cmdString); - else if(cmd == "setDiveOffset") setDiveOffset(cmdString); - else if(cmd == "getAcceleration") _getAcceleration(); - else if(cmd == "getAngularAcceleration") _getAngularAcceleration(); - else if(cmd == "getHeading") _getHeading(); - else if(cmd == "getInternalTemperature") _getInternalTemperature(); - else if(cmd == "getInternalPressure") _getInternalPressure(); - else if(cmd == "getExternalTemperature") _getExternalTemperature(); - else if(cmd == "getExternalPressure") _getExternalPressure(); - else if(cmd == "exit") shouldExit_ = true; + if(cmd == "goDirection") goDirection(cmdString); + else if(cmd == "rotate") rotate(cmdString); + else if(cmd == "move") move(cmdString); + else if(cmd == "secondaryDive") strafe(cmdString); + else if(cmd == "primaryDive") dive(cmdString); + else if(cmd == "yaw") yaw(cmdString); + else if(cmd == "pitch") pitch(cmdString); + else if(cmd == "roll") roll(cmdString); + else if(cmd == "killThrust") kill(); + else if(cmd == "turnOnEscs") powerManager_.turnOnEscs(); + else if(cmd == "turnOffEscs") powerManager_.turnOffEscs(); + else if(cmd == "switchLights") lights_.switchLights(); + else if(cmd == "turnOnImu") powerManager_.turnOnImuSensor(); + else if(cmd == "turnOffImu") powerManager_.turnOffImuSensor(); + else if(cmd == "getAcceleration") _getAcceleration(); + else if(cmd == "getAngularAcceleration") _getAngularAcceleration(); + else if(cmd == "getHeading") _getHeading(); + else if(cmd == "getInternalTemperature") _getInternalTemperature(); + else if(cmd == "getInternalPressure") _getInternalPressure(); + else if(cmd == "getExternalTemperature") _getExternalTemperature(); + else if(cmd == "getExternalPressure") _getExternalPressure(); + else if(cmd == "exit") shouldExit_ = true; } + + + void CommandDispatcher::goDirection(std::stringstream& cmdString) { float move, strafe, dive; cmdString >> move >> strafe >> dive; @@ -140,6 +144,9 @@ void CommandDispatcher::kill() { thrustController_.killAllThrusters(); } + + + void CommandDispatcher::_getAcceleration() { auto data = imuSensor_.getAcceleration(); auto accelJson = json{ diff --git a/BoneCentral/Peripherals/Peripherals.Core/src/ThrustController.cpp b/BoneCentral/Peripherals/Peripherals.Core/src/ThrustController.cpp index 7818093..9f58431 100644 --- a/BoneCentral/Peripherals/Peripherals.Core/src/ThrustController.cpp +++ b/BoneCentral/Peripherals/Peripherals.Core/src/ThrustController.cpp @@ -1,148 +1,65 @@ #include "ThrustController.h" ThrustController::ThrustController(IThrusterFactory& thrusterFactory, std::shared_ptr logger) : - leftForwardThruster_(thrusterFactory.createLeftForwardThruster()), - rightForwardThruster_(thrusterFactory.createRightForwardThruster()), - leftStrafeThruster_(thrusterFactory.createLeftStrafeThruster()), - rightStrafeThruster_(thrusterFactory.createRightStrafeThruster()), - forwardDiveThruster_(thrusterFactory.createForwardDiveThruster()), - rearDiveThruster_(thrusterFactory.createRearDiveThruster()), - logger_(logger) -{ - // sets default trim - forwardTrim.first = 1; - forwardTrim.second = 1; - diveTrim.first = 1; - diveTrim.second = 1; - strafeTrim.first = 1; - strafeTrim.second = 1; -} - -void ThrustController::goDirection(float forward, float strafe, float dive) { - std::stringstream ss; - ss << "Thrusting... F: " << forward << " S: " << strafe << " D: " << dive; - logger_->info(ss.str().c_str()); - auto strafeCorrection = getReciprocalValues(strafe * strafeRatio); - auto leftForward = strafeCorrection.first + forward; - auto rightForward = strafeCorrection.second + forward; - float powerScale = getScaleToMaxPower(leftForward, rightForward); - auto forwardPair = std::make_pair(leftForward*powerScale, rightForward*powerScale); - auto strafePair = getReciprocalValues(strafe*powerScale); + moveThruster_(thrusterFactory.createMoveThruster()), + strafeThruster_(thrusterFactory.createStrafeThruster()), + diveThruster_(thrusterFactory.createDiveThruster()), + yawThruster_(thrusterFactory.createYawThruster()), + pitchThruster_(thrusterFactory.createPitchThruster()), + rollThruster_(thrusterFactory.createRollThruster()), + logger_(logger) { } - float scaledDive = dive * powerScale; - FloatPair divePair; - divePair.first = getSafeOffset(scaledDive, diveOffset.first); - divePair.second = getSafeOffset(scaledDive, diveOffset.second); - setThrust(forwardPair, strafePair, divePair); +void ThrustController::goDirection(float _move_, float _strafe_, float _dive_) { + move(_move_); + strafe(_strafe_); + dive(_dive_); } -void ThrustController::faceDirection(float yaw, float dive) { - logger_->info("Yawing..."); - auto yawPair = getReciprocalValues(yaw); - - float scaledDive = dive; - FloatPair divePair; - divePair.first = getSafeOffset(scaledDive, diveOffset.first); - divePair.second = getSafeOffset(scaledDive, diveOffset.second); - setThrust(yawPair, std::make_pair(0.0f, 0.0f), divePair); +void ThrustController::move(float throttle) { + moveThruster_->Thrust(getSafeThrottle(throttle)); } -void ThrustController::setForwardTrim(float left, float right) { - std::stringstream ss; - ss << "Setting forward trim: L " << left << " R " << right; - logger_->info(ss.str().c_str()); - - forwardTrim.first = left; - forwardTrim.second = right; +void ThrustController::strafe(float throttle) { + strafeThruster_->Thrust(getSafeThrottle(throttle)); } -void ThrustController::setStrafeTrim(float left, float right) { - std::stringstream ss; - ss << "Setting strafe trim: L " << left << " R " << right; - logger_->info(ss.str().c_str()); - - strafeTrim.first = right; - strafeTrim.second = left; +void ThrustController::dive(float throttle) { + diveThruster_->Thrust(getSafeThrottle(throttle)); } -void ThrustController::setDiveTrim(float front, float back) { - std::stringstream ss; - ss << "Setting dive trim: F " << front << " B " << back; - logger_->info(ss.str().c_str()); - - diveTrim.first = front; - diveTrim.second = back; -} - -void ThrustController::setDiveOffset(float front, float back) { - std::stringstream ss; - ss << "Setting dive Offset: F " << front << " B " << back; - logger_->info(ss.str().c_str()); - - diveOffset.first = front; - diveOffset.second = back; +void ThrustController::rotate(float _yaw_, float _pitch_, float _roll_) { + yaw(_yaw_); + pitch(_pitch_); + roll(_roll_); } -float ThrustController::getSafeOffset(float a, float b) { - float safe = a + b; - if(safe > 1) - safe = 1; - else if(safe < -1) - safe = -1; - return safe; +void ThrustController::yaw(float throttle) { + yawThruster_->Thrust(getSafeThrottle(throttle)); } -void ThrustController::killAllThruster() { - setThrust(std::make_pair(0.0f, 0.0f), std::make_pair(0.0f, 0.0f), std::make_pair(0.0f, 0.0f)); +void ThrustController::pitch(float throttle) { + pitchThruster_->Thrust(getSafeThrottle(throttle)); } -std::pair ThrustController::getReciprocalValues(float value) { - float left, right; - if(value < 0.0f) { - left = value; - right = -1 * value * reverseRatio; - } - else { - left = value * reverseRatio; - right = -1 * value; - } - return std::make_pair(left, right); +void ThrustController::roll(float throttle) { + rollThruster_->Thrust(getSafeThrottle(throttle)); } -void ThrustController::thrustForward(float left, float right) { - auto scaleToMax = getScaleToMaxPower(left, right); - leftForwardThruster_->Thrust(left*scaleToMax); - rightForwardThruster_->Thrust(right*scaleToMax); +void ThrustController::killAllThrusters() { + move(0); + strafe(0); + dive(0); + yaw(0); + pitch(0); + roll(0); } -float ThrustController::getScaleToMaxPower(float left, float right) { - auto power = getMaxMag(left, right); - return power > maxPower ? maxPower / power : 1; -} - -float ThrustController::getMaxMag(float left, float right) { - if(left < 0) left *= -1; - if(right < 0) left *= -1; - return right > left ? right : left; -} - -void ThrustController::dive(float front, float rear) { - forwardDiveThruster_->Thrust(front); - rearDiveThruster_->Thrust(rear); -} - -void ThrustController::setThrust(FloatPair forwardPair, FloatPair strafePair, FloatPair divePair) { - leftForwardThruster_->Thrust(forwardPair.first * forwardTrim.first); - rightForwardThruster_->Thrust(forwardPair.second * forwardTrim.second); - - leftStrafeThruster_->Thrust(strafePair.first * strafeTrim.first); - rightStrafeThruster_->Thrust(strafePair.second * strafeTrim.second); - - forwardDiveThruster_->Thrust(divePair.first * diveTrim.first); - rearDiveThruster_->Thrust(divePair.second * diveTrim.second); +float ThrustController::getSafeThrottle(float throttle) { + if(throttle > MAX_THROTTLE || throttle < MIN_THROTTLE) + return 0; + return throttle; } -ThrustController::~ThrustController() -{ +ThrustController::~ThrustController() { logger_->info("Stopping Thrusters..."); } diff --git a/BoneCentral/Peripherals/Peripherals.SerialAdaptors/include/SerialFactory.h b/BoneCentral/Peripherals/Peripherals.SerialAdaptors/include/SerialFactory.h index 7e96037..88ffa97 100644 --- a/BoneCentral/Peripherals/Peripherals.SerialAdaptors/include/SerialFactory.h +++ b/BoneCentral/Peripherals/Peripherals.SerialAdaptors/include/SerialFactory.h @@ -18,12 +18,13 @@ class SerialFactory : public IThrusterFactory { private: Serial serial_; public: - std::shared_ptr createLeftForwardThruster(); - std::shared_ptr createRightForwardThruster(); - std::shared_ptr createLeftStrafeThruster(); - std::shared_ptr createRightStrafeThruster(); - std::shared_ptr createForwardDiveThruster(); - std::shared_ptr createRearDiveThruster(); + SerialFactory(); + std::shared_ptr createMoveThruster(); + std::shared_ptr createStrafeThruster(); + std::shared_ptr createDiveThruster(); + std::shared_ptr createYawThruster(); + std::shared_ptr createPitchThruster(); + std::shared_ptr createRollThruster(); std::shared_ptr createExternalTemperatureSensor(); std::shared_ptr createExternalPressureSensor(); std::shared_ptr createEscPower(); diff --git a/BoneCentral/Peripherals/Peripherals.SerialAdaptors/src/Serial.cpp b/BoneCentral/Peripherals/Peripherals.SerialAdaptors/src/Serial.cpp index 255b211..ebbcc60 100644 --- a/BoneCentral/Peripherals/Peripherals.SerialAdaptors/src/Serial.cpp +++ b/BoneCentral/Peripherals/Peripherals.SerialAdaptors/src/Serial.cpp @@ -5,20 +5,18 @@ #include "Serial.h" -#ifdef DEBUG -#define IFDEBUG if(true) -#else -#define IFDEBUG if(false) -#endif - #define DMSG(x) std::cerr << x std::mutex Serial::serialLock_; Serial::Serial(std::string device) { - IFDEBUG { - DMSG("\nReceived Device Name: " << device); - DMSG("\nEntering Serial Debug Mode\n"); +#ifdef DEBUG + DMSG("\nReceived Device Name: " << device); + DMSG("\nEntering Serial Debug Mode\n"); + fd = 0; +#else + if((fd = open(device.c_str(), O_RDWR)) < 0) { + DMSG("Device failed to open... entering dummy mode.\n"); fd = 0; } else { if((fd = open(device.c_str(), O_RDWR)) < 0) { @@ -69,9 +67,9 @@ void Serial::configure() { void Serial::acknowledge() { std::string response = readString(); - IFDEBUG { - DMSG("Arduino Message: " << response << "\n"); - } +#ifdef DEBUG + DMSG("Arduino Message: " << response << "\n"); +#endif writeByte('R'); } @@ -175,14 +173,13 @@ void Serial::writeByte(unsigned char value) { } void Serial::writeData(char* ptr, size_t size) { - IFDEBUG { - DMSG("Serial Write: " << std::hex << std::setw(2)); - for(size_t i = 0; i < size; i++) { - DMSG((unsigned short)ptr[i]); - } - DMSG(std::dec << std::endl); - return; +#ifdef DEBUG + DMSG("Serial Write: " << std::hex << std::setw(2)); + for(size_t i = 0; i < size; i++) { + DMSG((unsigned short)ptr[i]); } + DMSG(std::dec << std::endl); +#else std::lock_guard guard(serialLock_); if(fd == 0) return; write(fd, ptr, size); @@ -192,4 +189,7 @@ void Serial::writeData(char* ptr, size_t size) { #undef IFDEBUG #undef DMSG -#endif \ No newline at end of file +#endif +} + +#undef DMSG \ No newline at end of file diff --git a/BoneCentral/Peripherals/Peripherals.SerialAdaptors/src/SerialFactory.cpp b/BoneCentral/Peripherals/Peripherals.SerialAdaptors/src/SerialFactory.cpp index 0f931dc..be6bede 100644 --- a/BoneCentral/Peripherals/Peripherals.SerialAdaptors/src/SerialFactory.cpp +++ b/BoneCentral/Peripherals/Peripherals.SerialAdaptors/src/SerialFactory.cpp @@ -4,28 +4,30 @@ #include "SerialFactory.h" -std::shared_ptr SerialFactory::createLeftForwardThruster() { - return std::make_shared(serial_, LEFT_FORWARD); +SerialFactory::SerialFactory() : serial_("/dev/ttyACM0") { } + +std::shared_ptr SerialFactory::createMoveThruster() { + return std::make_shared(serial_, MOVE_IDX); } -std::shared_ptr SerialFactory::createRightForwardThruster() { - return std::make_shared(serial_, RIGHT_FORWARD); +std::shared_ptr SerialFactory::createStrafeThruster() { + return std::make_shared(serial_, STRAFE_IDX); } -std::shared_ptr SerialFactory::createLeftStrafeThruster() { - return std::make_shared(serial_, LEFT_STRAFE); +std::shared_ptr SerialFactory::createDiveThruster() { + return std::make_shared(serial_, DIVE_IDX); } -std::shared_ptr SerialFactory::createRightStrafeThruster() { - return std::make_shared(serial_, RIGHT_STRAFE); +std::shared_ptr SerialFactory::createYawThruster() { + return std::make_shared(serial_, YAW_IDX); } -std::shared_ptr SerialFactory::createForwardDiveThruster() { - return std::make_shared(serial_, FRONT_DIVE); +std::shared_ptr SerialFactory::createPitchThruster() { + return std::make_shared(serial_, PITCH_IDX); } -std::shared_ptr SerialFactory::createRearDiveThruster() { - return std::make_shared(serial_, BACK_DIVE); +std::shared_ptr SerialFactory::createRollThruster() { + return std::make_shared(serial_, ROLL_IDX); } std::shared_ptr SerialFactory::createEscPower() { diff --git a/BoneCentral/WebApplication/index.js b/BoneCentral/WebApplication/index.js index 4ddf3b1..6c0632f 100644 --- a/BoneCentral/WebApplication/index.js +++ b/BoneCentral/WebApplication/index.js @@ -53,65 +53,70 @@ app.post('/thrust', function(req, res) { res.send('thrust ' + req.body.powerLevel); }); - app.get('/stdoutData', function(req, res) { res.send(webLogger.pull()); }); -app.post('/thrustForward', function (req, res) { - var params = req.body; - thrustController.thrustForward(params.left, params.right); - res.send(''); +// From IThrustController +app.post('/move', function (req, res) { + var params = req.body; + thrustController.move(params.throttle); + res.send(''); }); -app.post('/dive', function (req, res) { +// From IThrustController +app.post('/secondaryDive', function (req, res) { var params = req.body; - thrustController.dive(params.forward, params.rear); + thrustController.secondaryDive(params.throttle); res.send(''); }); // From IThrustController -app.post('/goDirection', function(req, res) { - var params = req.body; - thrustController.goDirection(params.forward, params.strafe, params.dive); - res.send(''); +app.post('/dive', function (req, res) { + var params = req.body; + thrustController.dive(params.throttle); + res.send(''); }); -app.post('/faceDirection', function(req, res) { - thrustController.faceDirection(req.body.yaw, req.body.dive); - res.send(''); +// From IThrustController +app.post('/yaw', function (req, res) { + var params = req.body; + thrustController.yaw(params.throttle); + res.send(''); }); -// From setForwardTrim - -app.post('/setForwardTrim', function(req, res) { - var params = req.body; - thrustController.setForwardTrim(params.left, params.right); - res.send(''); +// From IThrustController +app.post('/pitch', function (req, res) { + var params = req.body; + thrustController.pitch(params.throttle); + res.send(''); }); -// From setStrafeTrim +// From IThrustController +app.post('/roll', function (req, res) { + var params = req.body; + thrustController.roll(params.throttle); + res.send(''); +}); -app.post('/setStrafeTrim', function(req, res) { +// From IThrustController +app.post('/goDirection', function(req, res) { var params = req.body; - thrustController.setStrafeTrim(params.left, params.right); + thrustController.goDirection(params.move, params.secondaryDive, params.dive); res.send(''); }); -// From setDiveTrim - -app.post('/setDiveTrim', function(req, res) { +// From IThrustController +app.post('/rotate', function(req, res) { var params = req.body; - thrustController.setDiveTrim(params.front, params.back); + thrustController.rotate(params.yaw, params.pitch, params.roll); res.send(''); }); -// From diveOffset - -app.post('/setDiveOffset', function(req, res) { - var params = req.body; - thrustController.setDiveOffset(params.front, params.back); - res.send(''); +// From IThrustController +app.get('/killThrust', function(req, res) { + thrustController.kill(); + res.send('killThrust'); }); // From Imu diff --git a/BoneCentral/WebApplication/static/index.html b/BoneCentral/WebApplication/static/index.html index b414869..70a56bd 100644 --- a/BoneCentral/WebApplication/static/index.html +++ b/BoneCentral/WebApplication/static/index.html @@ -127,14 +127,15 @@
- - - - - - - - + + + + + + + + +
diff --git a/Libraries/Catch b/Libraries/Catch new file mode 160000 index 0000000..4838039 --- /dev/null +++ b/Libraries/Catch @@ -0,0 +1 @@ +Subproject commit 4838039b65e6852c4029cedfd385611566318370 diff --git a/SLAM/.gitignore b/SLAM/.gitignore new file mode 100644 index 0000000..ef072c5 --- /dev/null +++ b/SLAM/.gitignore @@ -0,0 +1,3 @@ +slam_tests +libslam.a +test/_config.h diff --git a/SLAM/CMakeLists.txt b/SLAM/CMakeLists.txt new file mode 100644 index 0000000..5d38bce --- /dev/null +++ b/SLAM/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 2.8.9) +project(SLAM) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra -pedantic") # -Werror +set(TEST_RESOURCES_DIR "${PROJECT_SOURCE_DIR}/test_resources") + +configure_file(test/_config.h.in ${PROJECT_SOURCE_DIR}/test/_config.h ) + +file(GLOB_RECURSE HEADER_FILES "include/*.h" "include/*.hpp") +file(GLOB_RECURSE SOURCE_FILES "src/*.cpp" "src/*.hpp" "src/*.h") +file(GLOB_RECURSE TEST_SOURCE_FILES "test/*.cpp" "test/*.hpp" "test/*.h") + +# Declare the libraries and executable targets built from your sources +add_library(slam ${HEADER_FILES} ${SOURCE_FILES}) +add_executable(slam_tests ${TEST_SOURCE_FILES}) + +# Link the slam library to the slam_tests executable +target_link_libraries(slam_tests slam) + +include(catch.cmake) +include(opencv.cmake) + diff --git a/SLAM/INSTALLING_CMAKE.md b/SLAM/INSTALLING_CMAKE.md new file mode 100644 index 0000000..c4a161a --- /dev/null +++ b/SLAM/INSTALLING_CMAKE.md @@ -0,0 +1,15 @@ +##### Unix/Linux Users: +These instructions are for Ubuntu 16.04. You can substitute the appropriate +command for your system. Open a Terminal window and execute the following +command: + +```bash +sudo apt-get install build-essential cmake +``` +##### macOS Users: +CMake comes installed with macOS in most cases. If you're having trouble +finding it, make sure you have Xcode installed. + +##### Windows Users: +Follow [these instructions](https://cmake.org/install/) to install CMake +on your machine. \ No newline at end of file diff --git a/SLAM/INSTALLING_OPENCV.md b/SLAM/INSTALLING_OPENCV.md new file mode 100644 index 0000000..8528a2a --- /dev/null +++ b/SLAM/INSTALLING_OPENCV.md @@ -0,0 +1,21 @@ +##### Unix/Linux Users: +These instructions are for Ubuntu 16.04. You can substitute the appropriate +command for your system. Open a Terminal window and execute the following +command: + +```bash +sudo apt-get install libopencv-dev +``` + +##### macOS Users: +1. Make sure you have [Homebrew](https://brew.sh) installed. +2. Open a Terminal window and execute the following commands: + + ```bash + brew tap homebrew/science + brew install opencv + ``` + +##### Windows Users: +Follow [these instructions](http://docs.opencv.org/2.4/doc/tutorials/introduction/windows_install/windows_install.html) +to install OpenCV on your machine. \ No newline at end of file diff --git a/SLAM/README.md b/SLAM/README.md new file mode 100644 index 0000000..3f25c33 --- /dev/null +++ b/SLAM/README.md @@ -0,0 +1,64 @@ + +# SLAM +Simultaneous Location and Mapping for the USU Autonomous RoboSub. + +--- + +## Dependencies + +#### CMake +CMake is required to compile and build the library. Follow +[these instructions](INSTALLING_CMAKE.md) to get CMake up and +running on your machine. + +#### OpenCV 2.4 +OpenCV is required to build or use this library. Any version +in the `2.4.x.x` range should work. At the ime of this writing, +the most recent stable release was `2.4.13.2`. Follow +[these instructions](INSTALLING_OPENCV.md) to get OpenCV +up and running on your machine. + +--- + +## Contributing + +#### Basics + +- Please place all headers `.h` and `.hpp` files in `include/`. +- Please place all source `.cpp` files in `src/`. + +#### Namespace `slam` + +Please place everything in `namespace slam`. For example: + +```c++ +namespace slam { + class MyClass {...} + struct MyStruct {...} + void doSomething() {...} +} +``` + +#### Tests + +Use `test/` folder for writing unit tests. Every concrete class +should have a set of associated unit tests. Structs should also +probably have unit tests for sanity's sake. Take a look at the +existing tests to figure out how to write them. + +#### How to build +```bash +mkdir out +cd out +cmake .. +make slam # <-- libslam.a is created +``` + +#### How to run tests +```bash +mkdir out +cd out +cmake .. +make slam_tests # <-- slam_tests is created +./slam_tests +``` diff --git a/SLAM/catch.cmake b/SLAM/catch.cmake new file mode 100644 index 0000000..05acacb --- /dev/null +++ b/SLAM/catch.cmake @@ -0,0 +1,14 @@ +set(CATCH_INCLUDE_DIR ../Libraries/Catch/include) +get_filename_component(CATCH_INCLUDE_DIR ${CATCH_INCLUDE_DIR} ABSOLUTE) + +if((NOT CATCH_INCLUDE_DIR) OR (NOT EXISTS ${CATCH_INCLUDE_DIR})) + message(WARNING "Library 'Catch' could not be found at: ${CATCH_INCLUDE_DIR}.\n" + "Please initialize its git submodule by running the following command:\n" + " `git submodule update --init -- ../Libraries/Catch`\n" + "or its Windows equivalent.") + message(STATUS "SLAM unit tests will not be built.") +else() + message(STATUS "Library 'Catch' was found at ${CATCH_INCLUDE_DIR}.") + message(STATUS "SLAM unit tests will be built.") + include_directories(${CATCH_INCLUDE_DIR}) +endif() diff --git a/SLAM/include/_types.h b/SLAM/include/_types.h new file mode 100644 index 0000000..94c6133 --- /dev/null +++ b/SLAM/include/_types.h @@ -0,0 +1,53 @@ +// +// Created by Kyler Jensen on 2/15/17. +// + +#ifndef SLAM_EXTRAS_H +#define SLAM_EXTRAS_H + +#include +#include +#include +#include + +namespace slam { + + /** + * An alias for std::chrono::steady_clock::time_point + * */ + typedef std::chrono::steady_clock::time_point chrono; + + /** + * A one-byte unsigned integer + * Range: 0 to 255 + * */ + typedef uint8_t uchar; + + /** + * A two-byte unsigned integer + * Range: 0 to 65,535 + * */ + typedef uint16_t ushort; + + /** + * A four-byte unsigned integer + * Range: 0 to 4,294,967,295 + * */ + typedef uint32_t uint; + + /** + * An eight-byte unsigned integer + * Range: 0 to 18,446,744,073,709,551,615 + * */ + typedef uint64_t ulong; + + inline std::ostream& operator <<(std::ostream& os, const uchar it) { + os << (int)it; + return os; + } + + class Output3DWrapper {}; //TODO: remove this and replace usages with the real dso::Output3DWrapper + +} + +#endif //SLAM_EXTRAS_H diff --git a/SLAM/include/camera_frame/ColorSpace.h b/SLAM/include/camera_frame/ColorSpace.h new file mode 100644 index 0000000..53148bf --- /dev/null +++ b/SLAM/include/camera_frame/ColorSpace.h @@ -0,0 +1,35 @@ +// +// Created by Kyler Jensen on 3/28/17. +// + +#ifndef SLAM_COLORSPACE_H +#define SLAM_COLORSPACE_H + +#include + +namespace slam { + + enum ColorSpace : int { + GRAYSCALE = 1, + RGB = 3 + }; + + inline std::string to_string(const slam::ColorSpace &it) { + switch(it) { + case GRAYSCALE: + return "GRAYSCALE(" + std::to_string((int)it) + ")"; + case RGB: + return "RGB(" + std::to_string((int)it) + ")"; + default: + return "UNSUPPORTED(" + std::to_string((int)it) + ")"; + } + } + + inline std::ostream& operator <<(std::ostream& os, const slam::ColorSpace &it) { + os << to_string(it); + return os; + } + +} + +#endif //SLAM_COLORSPACE_H diff --git a/SLAM/include/camera_frame/GrayscaleCameraFrame.h b/SLAM/include/camera_frame/GrayscaleCameraFrame.h new file mode 100644 index 0000000..1624acb --- /dev/null +++ b/SLAM/include/camera_frame/GrayscaleCameraFrame.h @@ -0,0 +1,32 @@ +// +// Created by Kyler Jensen on 3/28/17. +// + +#ifndef SLAM_GRAYSCALECAMERAFRAME_H +#define SLAM_GRAYSCALECAMERAFRAME_H + + +#include "../_types.h" +#include "ICameraFrameCVMatBased.h" + +namespace slam { + + class GrayscaleCameraFrame : public TCameraFrame, + public ICameraFrameCVMatBased { + + public: + GrayscaleCameraFrame(int rows, int cols, const chrono& timestamp); + + GrayscaleCameraFrame(const cv::Mat& data, const chrono& timestamp); + + GrayscaleCameraFrame(const std::vector>& data, const chrono& timestamp); + + void setPixel(int row, int col, const uchar& value) override; + + uchar getPixel(int row, int col) const override; + + }; + +} + +#endif //SLAM_GRAYSCALECAMERAFRAME_H diff --git a/SLAM/include/camera_frame/ICameraFrame.h b/SLAM/include/camera_frame/ICameraFrame.h new file mode 100644 index 0000000..dad0e98 --- /dev/null +++ b/SLAM/include/camera_frame/ICameraFrame.h @@ -0,0 +1,37 @@ +// +// Created by Kyler Jensen on 3/29/17. +// + +#ifndef SLAM_ICAMERAFRAME_H +#define SLAM_ICAMERAFRAME_H + +#include +#include "ColorSpace.h" +#include "../_types.h" + +namespace slam { + + class ICameraFrame { + + protected: + chrono timestamp; + + ICameraFrame(const chrono ×tamp); + + public: + const chrono& getTimestamp() const; + + virtual int getRows() const = 0; + + virtual int getCols() const = 0; + + virtual const cv::Mat& toMat() const = 0; + + virtual ColorSpace getColorSpace() const = 0; + + }; + +} + + +#endif //SLAM_ICAMERAFRAME_H diff --git a/SLAM/include/camera_frame/ICameraFrameCVMatBased.h b/SLAM/include/camera_frame/ICameraFrameCVMatBased.h new file mode 100644 index 0000000..3b34e45 --- /dev/null +++ b/SLAM/include/camera_frame/ICameraFrameCVMatBased.h @@ -0,0 +1,35 @@ +// +// Created by Kyler Jensen on 3/29/17. +// + +#ifndef SLAM_ICAMERAFRAMECVMATBASED_H +#define SLAM_ICAMERAFRAMECVMATBASED_H + +#include "TCameraFrame.h" +#include "ColorSpace.h" + +namespace slam { + + class ICameraFrameCVMatBased : public ICameraFrame { + + protected: + cv::Mat data; + + ICameraFrameCVMatBased(int rows, int cols, int mode, const cv::Scalar& fill, const chrono& timestamp); + + ICameraFrameCVMatBased(const cv::Mat& data, const chrono& timestamp); + + public: + int getRows() const override; + + int getCols() const override; + + const cv::Mat &toMat() const override; + + ColorSpace getColorSpace() const override; + + }; + +} + +#endif //SLAM_ICAMERAFRAMECVMATBASED_H diff --git a/SLAM/include/camera_frame/RGBCameraFrame.h b/SLAM/include/camera_frame/RGBCameraFrame.h new file mode 100644 index 0000000..6ff440c --- /dev/null +++ b/SLAM/include/camera_frame/RGBCameraFrame.h @@ -0,0 +1,32 @@ +// +// Created by Kyler Jensen on 3/28/17. +// + +#ifndef SLAM_RGBCAMERAFRAME_H +#define SLAM_RGBCAMERAFRAME_H + + +#include "RGBColor.h" +#include "ICameraFrameCVMatBased.h" + +namespace slam { + + class RGBCameraFrame : public TCameraFrame, + public ICameraFrameCVMatBased { + + public: + RGBCameraFrame(int rows, int cols, const chrono ×tamp); + + RGBCameraFrame(const std::vector> &data, const chrono ×tamp); + + RGBCameraFrame(const cv::Mat &data, const chrono ×tamp); + + void setPixel(int row, int col, const RGBColor &value) override; + + RGBColor getPixel(int row, int col) const override; + + }; + +} + +#endif //SLAM_RGBCAMERAFRAME_H diff --git a/SLAM/include/camera_frame/RGBColor.h b/SLAM/include/camera_frame/RGBColor.h new file mode 100644 index 0000000..809152b --- /dev/null +++ b/SLAM/include/camera_frame/RGBColor.h @@ -0,0 +1,73 @@ +// +// Created by Kyler Jensen on 2/22/17. +// + +#ifndef SLAM_RGBCOLOR_H +#define SLAM_RGBCOLOR_H + +#include +#include "../_types.h" + +namespace slam { + + struct RGBColor { + + uchar red; + uchar green; + uchar blue; + + bool operator ==(const RGBColor other) const { + return this->red == other.red && + this->blue == other.blue && + this->green == other.green; + } + + bool operator !=(const RGBColor other) const { + return !(*this == other); + } + + /** + * Returns false if the difference in intensity of + * any of the 3 color channels is greater than or + * equal to 2. + * + * Returns true for the following pair: + * @code + * this: { other: { + * red: 100, red: 101, + * green: 100, green: 100, + * blue: 100 blue: 99 + * } } + * @endcode + * Returns false for the following pair: + * @code + * this: { other: { + * red: 100, red: 100, + * green: 100, green: 102, + * blue: 100 blue: 100 + * } } + * @endcode + * */ + bool almostEquals(const RGBColor &other) const { + return abs((int)this->red - (int)other.red) < 2 && + abs((int)this->blue - (int)other.blue) < 2 && + abs((int)this->green - (int)other.green) < 2; + } + + }; + + inline std::string to_string(const RGBColor &it) { + return "rgb(" + + std::to_string((int)it.red) + "," + + std::to_string((int)it.green) + "," + + std::to_string((int)it.blue) + ")"; + } + + inline std::ostream& operator <<(std::ostream& os, const RGBColor &it) { + os << to_string(it); + return os; + } + +} + +#endif //SLAM_RGBCOLOR_H diff --git a/SLAM/include/camera_frame/TCameraFrame.h b/SLAM/include/camera_frame/TCameraFrame.h new file mode 100644 index 0000000..321b4d5 --- /dev/null +++ b/SLAM/include/camera_frame/TCameraFrame.h @@ -0,0 +1,52 @@ +// +// Created by Kyler Jensen on 2/15/17. +// + +#ifndef SLAM_TCAMERAFRAME_H +#define SLAM_TCAMERAFRAME_H + +#include "../_types.h" +#include "ColorSpace.h" +#include "ICameraFrame.h" +#include +#include + +namespace slam { + + template + class TCameraFrame { + + protected: + virtual void setPixel(int row, int col, const T& value) = 0; + + virtual T getPixel(int row, int col) const = 0; + + void loadFromVector(const std::vector> &data) { + for(uint row = 0; row < data.size(); row++) { + for(uint col = 0; col < data[row].size(); col++) { + setPixel(row, col, data[row][col]); + } + } + } + + }; + + template + int get_rows(const std::vector> &data) { + return (int) data.size(); + } + + template + int get_cols(const std::vector> &data) { + size_t max = 0; + for(auto row : data) { + max = row.size() > max ? row.size() : max; + } + return (int) max; + } + +} + + + +#endif //SLAM_TCAMERAFRAME_H diff --git a/SLAM/include/camera_frame_stream/IFrameStream.h b/SLAM/include/camera_frame_stream/IFrameStream.h new file mode 100644 index 0000000..e4bb752 --- /dev/null +++ b/SLAM/include/camera_frame_stream/IFrameStream.h @@ -0,0 +1,22 @@ +// +// Created by Kyler Jensen on 2/28/17. +// + +#ifndef SLAM_IFRAMESTREAM_H +#define SLAM_IFRAMESTREAM_H + +#include "../camera_frame/TCameraFrame.h" +#include "../camera_frame/GrayscaleCameraFrame.h" + +namespace slam { + + class IFrameStream { + + public: + virtual const ICameraFrame& getFrame() = 0; + + }; + +} + +#endif //SLAM_IFRAMESTREAM_H diff --git a/SLAM/include/telemetry/DepthMap.h b/SLAM/include/telemetry/DepthMap.h new file mode 100644 index 0000000..1fe3fc9 --- /dev/null +++ b/SLAM/include/telemetry/DepthMap.h @@ -0,0 +1,25 @@ +#ifndef SLAM_DEPTHMAP_H +#define SLAM_DEPTHMAP_H +#define UNKNOWN_DEPTH -1.0f + +#include +#include +#include + +namespace slam { + + class DepthMap{ + + private: + std::vector> m_depth; //vector of vector of floats + + public: + DepthMap(ushort x, ushort y); + void setDepth(ushort x, ushort y, float depth); + double getDepth(ushort x, ushort y); + + + }; + +} +#endif //SLAM_DEPTHMAP_H diff --git a/SLAM/include/telemetry/Pose.h b/SLAM/include/telemetry/Pose.h new file mode 100644 index 0000000..35f5bee --- /dev/null +++ b/SLAM/include/telemetry/Pose.h @@ -0,0 +1,17 @@ +// +// Created by Kyler Jensen on 2/15/17. +// + +#ifndef SLAM_POSE_H +#define SLAM_POSE_H + +namespace slam { + + struct Pose { + //TODO + }; + +} + + +#endif //SLAM_POSE_H diff --git a/SLAM/include/telemetry/TelemetryListener.h b/SLAM/include/telemetry/TelemetryListener.h new file mode 100644 index 0000000..ccc83ff --- /dev/null +++ b/SLAM/include/telemetry/TelemetryListener.h @@ -0,0 +1,16 @@ +// +// Created by Kyler Jensen on 4/5/17. +// + +#ifndef SLAM_TELEMETRYLISTENER_H +#define SLAM_TELEMETRYLISTENER_H + +#include "TelemetryPacket.h" + +namespace slam { + + typedef void (*TelemetryListener)(slam::TelemetryPacket); + +} + +#endif //SLAM_TELEMETRYLISTENER_H diff --git a/SLAM/include/telemetry/TelemetryPacket.h b/SLAM/include/telemetry/TelemetryPacket.h new file mode 100644 index 0000000..5030e83 --- /dev/null +++ b/SLAM/include/telemetry/TelemetryPacket.h @@ -0,0 +1,26 @@ +// +// Created by Kyler Jensen on 2/15/17. +// + +#ifndef SLAM_TELEMETRYPACKET_H +#define SLAM_TELEMETRYPACKET_H + +#include "../camera_frame/TCameraFrame.h" +#include "DepthMap.h" +#include "Pose.h" + +namespace slam { + + struct TelemetryPacket { + + ICameraFrame& cameraFrame; + DepthMap& depthMap; + Pose& pose; + + }; + +} + + + +#endif //SLAM_TELEMETRYPACKET_H diff --git a/SLAM/include/telemetry/TelemetryService.h b/SLAM/include/telemetry/TelemetryService.h new file mode 100644 index 0000000..543d6a7 --- /dev/null +++ b/SLAM/include/telemetry/TelemetryService.h @@ -0,0 +1,31 @@ +// +// Created by Kyler Jensen on 3/29/17. +// + +#ifndef SLAM_TELEMETRYSERVICE_H +#define SLAM_TELEMETRYSERVICE_H + +#include +#include "TelemetryPacket.h" +#include "../camera_frame_stream/IFrameStream.h" +#include "TelemetryListener.h" + +namespace slam { + + class TelemetryService { + + private: + std::unordered_set listeners = std::unordered_set(); + + public: + TelemetryPacket getCurrentTelemetry(); + void subscribe(TelemetryListener listener); + void unsubscribe(TelemetryListener listener); + int subscribers() const; + + }; + +} + + +#endif //SLAM_TELEMETRYSERVICE_H diff --git a/SLAM/opencv.cmake b/SLAM/opencv.cmake new file mode 100644 index 0000000..855d598 --- /dev/null +++ b/SLAM/opencv.cmake @@ -0,0 +1,22 @@ +# Find OpenCV, you may need to set OpenCV_DIR variable +# to the absolute path to the directory containing OpenCVConfig.cmake file +# via the command line or GUI +find_package(OpenCV REQUIRED) + +# If the package has been found, several variables will +# be set, you can find the full list with descriptions +# in the OpenCVConfig.cmake file. +# Print some message showing some of them +message(STATUS "OpenCV library status:") +message(STATUS " version: ${OpenCV_VERSION}") +message(STATUS " libraries: ${OpenCV_LIBS}") +message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}") + +if(CMAKE_VERSION VERSION_LESS "2.8.11") + # Add OpenCV headers location to your include paths + include_directories(${OpenCV_INCLUDE_DIRS}) +endif() + +# Link your application with OpenCV libraries +target_link_libraries(slam ${OpenCV_LIBS}) +target_link_libraries(slam_tests ${OpenCV_LIBS}) \ No newline at end of file diff --git a/SLAM/src/DepthMap.cpp b/SLAM/src/DepthMap.cpp new file mode 100644 index 0000000..9aaa831 --- /dev/null +++ b/SLAM/src/DepthMap.cpp @@ -0,0 +1,21 @@ +#include "../include/telemetry/DepthMap.h" + +using namespace slam; + +DepthMap::DepthMap(unsigned short x, unsigned short y) { + for(int i = 0; i < x; i++) { + std::vector temp; + for(int k = 0; k < y; k++) { + temp.push_back(UNKNOWN_DEPTH); + } + m_depth.push_back(temp); + } +} + +void DepthMap::setDepth(unsigned short x, unsigned short y, float depth) { + m_depth[x][y] = depth; +} + +double DepthMap::getDepth(unsigned short x, unsigned short y) { + return m_depth[x][y]; +} diff --git a/SLAM/src/GrayscaleCameraFrame.cpp b/SLAM/src/GrayscaleCameraFrame.cpp new file mode 100644 index 0000000..a24af43 --- /dev/null +++ b/SLAM/src/GrayscaleCameraFrame.cpp @@ -0,0 +1,26 @@ +// +// Created by Kyler Jensen on 3/28/17. +// + +#include "../include/camera_frame/GrayscaleCameraFrame.h" + +using namespace slam; + +GrayscaleCameraFrame::GrayscaleCameraFrame(const cv::Mat &data, const chrono ×tamp) + : ICameraFrameCVMatBased(data, timestamp) {} + +GrayscaleCameraFrame::GrayscaleCameraFrame(int rows, int cols, const chrono& timestamp) + : ICameraFrameCVMatBased(rows, cols, CV_8U, cv::Scalar(0), timestamp) {} + +GrayscaleCameraFrame::GrayscaleCameraFrame(const std::vector>& data, const chrono& timestamp) + : ICameraFrameCVMatBased(get_rows(data), get_cols(data), CV_8U, cv::Scalar(0), timestamp) { loadFromVector(data); } + +void GrayscaleCameraFrame::setPixel(int row, int col, const uchar& value) { + data.at(row, col) = value; +} + +uchar GrayscaleCameraFrame::getPixel(int row, int col) const { + return data.at(row, col); +} + + diff --git a/SLAM/src/ICameraFrame.cpp b/SLAM/src/ICameraFrame.cpp new file mode 100644 index 0000000..72c8728 --- /dev/null +++ b/SLAM/src/ICameraFrame.cpp @@ -0,0 +1,13 @@ +// +// Created by Kyler Jensen on 3/29/17. +// + +#include "../include/camera_frame/ICameraFrame.h" + +using namespace slam; + +ICameraFrame::ICameraFrame(const chrono ×tamp) : timestamp(timestamp) {} + +const chrono &ICameraFrame::getTimestamp() const { + return timestamp; +} \ No newline at end of file diff --git a/SLAM/src/ICameraFrameCVMatBased.cpp b/SLAM/src/ICameraFrameCVMatBased.cpp new file mode 100644 index 0000000..9314399 --- /dev/null +++ b/SLAM/src/ICameraFrameCVMatBased.cpp @@ -0,0 +1,31 @@ +// +// Created by Kyler Jensen on 3/29/17. +// + +#include "../include/camera_frame/ICameraFrameCVMatBased.h" + +using namespace slam; + +ICameraFrameCVMatBased::ICameraFrameCVMatBased(int rows, int cols, int mode, const cv::Scalar &fill, const chrono ×tamp) + : ICameraFrame(timestamp), + data(cv::Mat(rows, cols, mode, fill)) {} + +ICameraFrameCVMatBased::ICameraFrameCVMatBased(const cv::Mat &data, const chrono ×tamp) + : ICameraFrame(timestamp), + data(data) {} + +int ICameraFrameCVMatBased::getRows() const { + return data.rows; +} + +int ICameraFrameCVMatBased::getCols() const { + return data.cols; +} + +const cv::Mat &ICameraFrameCVMatBased::toMat() const { + return data; +} + +ColorSpace ICameraFrameCVMatBased::getColorSpace() const { + return (ColorSpace) data.channels(); +} diff --git a/SLAM/src/RGBCameraFrame.cpp b/SLAM/src/RGBCameraFrame.cpp new file mode 100644 index 0000000..bd92616 --- /dev/null +++ b/SLAM/src/RGBCameraFrame.cpp @@ -0,0 +1,31 @@ +// +// Created by Kyler Jensen on 3/28/17. +// + +#include "../include/camera_frame/RGBCameraFrame.h" + +using namespace slam; + +RGBCameraFrame::RGBCameraFrame(const cv::Mat &data, const chrono& timestamp) + : ICameraFrameCVMatBased(data, timestamp) {} + +RGBCameraFrame::RGBCameraFrame(int rows, int cols, const chrono& timestamp) + : ICameraFrameCVMatBased(rows, cols, CV_8UC3, cv::Scalar(0, 0, 0), timestamp) {} + +RGBCameraFrame::RGBCameraFrame(const std::vector> &data, const chrono& timestamp) + : ICameraFrameCVMatBased(get_rows(data), get_cols(data), CV_8UC3, cv::Scalar(0, 0, 0), timestamp) { loadFromVector(data); } + +void RGBCameraFrame::setPixel(int row, int col, const RGBColor& value) { + auto &pixel = data.at(row, col); + pixel[2] = value.red; + pixel[1] = value.green; + pixel[0] = value.blue; +} + +RGBColor RGBCameraFrame::getPixel(int row, int col) const { + auto pixel = data.at(row, col); + auto r = pixel[2]; + auto g = pixel[1]; + auto b = pixel[0]; + return { r, g, b }; +} diff --git a/SLAM/src/TelemetryService.cpp b/SLAM/src/TelemetryService.cpp new file mode 100644 index 0000000..35242b9 --- /dev/null +++ b/SLAM/src/TelemetryService.cpp @@ -0,0 +1,24 @@ +// +// Created by Kyler Jensen on 3/29/17. +// + +#include "../include/telemetry/TelemetryService.h" + +using namespace slam; + +TelemetryPacket TelemetryService::getCurrentTelemetry() { + //TODO Implement this + throw std::runtime_error("Not implemented."); +} + +void TelemetryService::subscribe(TelemetryListener listener) { + listeners.insert(listener); +} + +void TelemetryService::unsubscribe(TelemetryListener listener) { + listeners.erase(listener); +} + +int TelemetryService::subscribers() const { + return (int) listeners.size(); +} diff --git a/SLAM/test/_config.h.in b/SLAM/test/_config.h.in new file mode 100644 index 0000000..cacd362 --- /dev/null +++ b/SLAM/test/_config.h.in @@ -0,0 +1,21 @@ +// +// Created by Kyler Jensen on 3/27/17. +// + +#ifndef CONFIG_H +#define CONFIG_H + +#include + +class TestConfig { + +public: + static std::string testResourcesDirectory() { + // This gets filled in automatically with the appropriate directory by CMake. + return "@TEST_RESOURCES_DIR@"; + } + +}; + + +#endif diff --git a/SLAM/test/_test_main.cpp b/SLAM/test/_test_main.cpp new file mode 100644 index 0000000..8b57393 --- /dev/null +++ b/SLAM/test/_test_main.cpp @@ -0,0 +1,6 @@ +// +// Created by Kyler Jensen on 2/15/17. +// + +#define CATCH_CONFIG_MAIN +#include diff --git a/SLAM/test/camera_frame/GrayscaleCameraFrameRealFilesTest.cpp b/SLAM/test/camera_frame/GrayscaleCameraFrameRealFilesTest.cpp new file mode 100644 index 0000000..ac3c17b --- /dev/null +++ b/SLAM/test/camera_frame/GrayscaleCameraFrameRealFilesTest.cpp @@ -0,0 +1,73 @@ +// +// Created by Kyler Jensen on 3/28/17. +// + +#include +#include +#include "../_config.h" +#include "../../include/camera_frame/GrayscaleCameraFrame.h" + +using namespace slam; + +std::vector expected_intensities_by_row = { + 54, + 182, + 18, + 100, + 0, + 255, + 191, + 255, + 255, + 255 +}; + +void test_exact_match_gs(std::string filename) { + auto img = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE); + auto frame = GrayscaleCameraFrame(img, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 10); + REQUIRE(frame.getCols() == 10); + REQUIRE(frame.toMat().rows == frame.getRows()); + REQUIRE(frame.toMat().cols == frame.getCols()); + REQUIRE(frame.getColorSpace() == GRAYSCALE); + for (auto row = 0; row < frame.getRows(); row++) { + for (auto col = 0; col < frame.getCols(); col++) { + auto expected = expected_intensities_by_row[row]; + auto actual = frame.getPixel(row, col); + REQUIRE((int)actual == (int)expected); + } + } +} + +void test_near_match_gs(std::string filename) { + auto img = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE); + auto frame = GrayscaleCameraFrame(img, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 10); + REQUIRE(frame.getCols() == 10); + REQUIRE(frame.toMat().rows == frame.getRows()); + REQUIRE(frame.toMat().cols == frame.getCols()); + REQUIRE(frame.getColorSpace() == GRAYSCALE); + for (auto row = 0; row < frame.getRows(); row++) { + for (auto col = 0; col < frame.getCols(); col++) { + auto expected = expected_intensities_by_row[row]; + auto actual = frame.getPixel(row, col); + REQUIRE((int)expected - (int)actual < 2); + REQUIRE((int)expected - (int)actual > -2); + } + } +} + +TEST_CASE("GrayscaleCameraFrame can be initialized with a real cv::Mat") { + SECTION("Works with a real .bmp file") { + auto filename = TestConfig::testResourcesDirectory() + "/GRAYSCALE10x10.bmp"; + test_exact_match_gs(filename); + } + SECTION("Works with a real .png file") { + auto filename = TestConfig::testResourcesDirectory() + "/GRAYSCALE10x10.png"; + test_exact_match_gs(filename); + } + SECTION("Works with a real .jpg file") { + auto filename = TestConfig::testResourcesDirectory() + "/GRAYSCALE10x10.jpg"; + test_near_match_gs(filename); + } +} diff --git a/SLAM/test/camera_frame/GrayscaleCameraFrameTest.cpp b/SLAM/test/camera_frame/GrayscaleCameraFrameTest.cpp new file mode 100644 index 0000000..1b393d9 --- /dev/null +++ b/SLAM/test/camera_frame/GrayscaleCameraFrameTest.cpp @@ -0,0 +1,54 @@ +// +// Created by Kyler Jensen on 3/29/17. +// + +#include +#include "../../include/camera_frame/GrayscaleCameraFrame.h" + +using namespace slam; + +TEST_CASE("TCameraFrame can be instantiated with a 2D vector of uchar") { + SECTION("A dense uchar vector works as expected") { + std::vector> data = { + {255, 254}, + {253, 252} + }; + auto frame = GrayscaleCameraFrame(data, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 2); + REQUIRE(frame.getCols() == 2); + REQUIRE(frame.getColorSpace() == GRAYSCALE); + REQUIRE(frame.getPixel(0, 0) == data[0][0]); + REQUIRE(frame.getPixel(0, 1) == data[0][1]); + REQUIRE(frame.getPixel(1, 0) == data[1][0]); + REQUIRE(frame.getPixel(1, 1) == data[1][1]); + } + SECTION("A ragged uchar vector is padded with black pixels") { + std::vector> data = { + {255}, + {254, 253}, + {252, 251, 250} + }; + uchar BLACK = 0; + auto frame = GrayscaleCameraFrame(data, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 3); + REQUIRE(frame.getCols() == 3); + REQUIRE(frame.getColorSpace() == GRAYSCALE); + REQUIRE(frame.getPixel(0, 0) == data[0][0]); + REQUIRE(frame.getPixel(0, 1) == BLACK); + REQUIRE(frame.getPixel(0, 2) == BLACK); + REQUIRE(frame.getPixel(1, 0) == data[1][0]); + REQUIRE(frame.getPixel(1, 1) == data[1][1]); + REQUIRE(frame.getPixel(1, 2) == BLACK); + REQUIRE(frame.getPixel(2, 0) == data[2][0]); + REQUIRE(frame.getPixel(2, 1) == data[2][1]); + REQUIRE(frame.getPixel(2, 2) == data[2][2]); + } + SECTION("An empty uchar vector produces an empty TCameraFrame") { + std::vector> data = {}; + auto frame = GrayscaleCameraFrame(data, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 0); + REQUIRE(frame.getCols() == 0); + REQUIRE(frame.getColorSpace() == GRAYSCALE); + } + +} \ No newline at end of file diff --git a/SLAM/test/camera_frame/RGBCameraFrameRealFilesTest.cpp b/SLAM/test/camera_frame/RGBCameraFrameRealFilesTest.cpp new file mode 100644 index 0000000..5167dc7 --- /dev/null +++ b/SLAM/test/camera_frame/RGBCameraFrameRealFilesTest.cpp @@ -0,0 +1,72 @@ +// +// Created by Kyler Jensen on 3/28/17. +// + +#include +#include +#include "../_config.h" +#include "../../include/camera_frame/RGBCameraFrame.h" + +using namespace slam; + +std::vector expected_colors_by_row = { + { 255, 0, 0 }, + { 0, 255, 0 }, + { 0, 0, 255 }, + { 100, 100, 100 }, + { 0, 0, 0 }, + { 255, 255, 255 }, + { 120, 210, 212 }, + { 255, 255, 255 }, + { 255, 255, 255 }, + { 255, 255, 255 }, +}; + +void test_exact_match_rgb(std::string filename) { + auto img = cv::imread(filename, CV_LOAD_IMAGE_COLOR); + auto frame = RGBCameraFrame(img, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 10); + REQUIRE(frame.getCols() == 10); + REQUIRE(frame.toMat().rows == frame.getRows()); + REQUIRE(frame.toMat().cols == frame.getCols()); + REQUIRE(frame.getColorSpace() == RGB); + for (auto row = 0; row < frame.getRows(); row++) { + for (auto col = 0; col < frame.getCols(); col++) { + auto expected = expected_colors_by_row[row]; + auto actual = frame.getPixel(row, col); + REQUIRE(actual == expected); + } + } +} + +void test_near_match_rgb(std::string filename) { + auto img = cv::imread(filename, CV_LOAD_IMAGE_COLOR); + auto frame = RGBCameraFrame(img, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 10); + REQUIRE(frame.getCols() == 10); + REQUIRE(frame.toMat().rows == frame.getRows()); + REQUIRE(frame.toMat().cols == frame.getCols()); + REQUIRE(frame.getColorSpace() == RGB); + for (auto row = 0; row < frame.getRows(); row++) { + for (auto col = 0; col < frame.getCols(); col++) { + auto expected = expected_colors_by_row[row]; + auto actual = frame.getPixel(row, col); + REQUIRE(actual.almostEquals(expected)); + } + } +} + +TEST_CASE("RGBCameraFrame can be initialized with a real cv::Mat") { + SECTION("Works with a real .bmp file") { + auto filename = TestConfig::testResourcesDirectory() + "/RGB10x10.bmp"; + test_exact_match_rgb(filename); + } + SECTION("Works with a real .png file") { + auto filename = TestConfig::testResourcesDirectory() + "/RGB10x10.png"; + test_exact_match_rgb(filename); + } + SECTION("Works with a real .jpg file") { + auto filename = TestConfig::testResourcesDirectory() + "/RGB10x10.jpg"; + test_near_match_rgb(filename); + } +} diff --git a/SLAM/test/camera_frame/RGBCameraFrameTest.cpp b/SLAM/test/camera_frame/RGBCameraFrameTest.cpp new file mode 100644 index 0000000..f2d11b8 --- /dev/null +++ b/SLAM/test/camera_frame/RGBCameraFrameTest.cpp @@ -0,0 +1,54 @@ +// +// Created by Kyler Jensen on 2/28/17. +// + +#include +#include +#include "../../include/camera_frame/RGBCameraFrame.h" + +using namespace slam; + +TEST_CASE("TCameraFrame can be instantiated with a 2D vector of RGB") { + SECTION("A dense RGB vector works as expected") { + std::vector> data = { + {{255, 254, 253}, {252, 251, 250}}, + {{249, 248, 247}, {246, 245, 244}} + }; + auto frame = RGBCameraFrame(data, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 2); + REQUIRE(frame.getCols() == 2); + REQUIRE(frame.getColorSpace() == RGB); + REQUIRE(frame.getPixel(0, 0) == data[0][0]); + REQUIRE(frame.getPixel(0, 1) == data[0][1]); + REQUIRE(frame.getPixel(1, 0) == data[1][0]); + REQUIRE(frame.getPixel(1, 1) == data[1][1]); + } + SECTION("A ragged RGB vector is padded with black pixels") { + std::vector> data = { + {{255, 254, 253}}, + {{249, 248, 247}, {246, 245, 244}}, + {{243, 242, 241}, {240, 239, 238}, {237, 236, 235}} + }; + RGBColor BLACK = { 0, 0, 0 }; + auto frame = RGBCameraFrame(data, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 3); + REQUIRE(frame.getCols() == 3); + REQUIRE(frame.getColorSpace() == RGB); + REQUIRE(frame.getPixel(0, 0) == data[0][0]); + REQUIRE(frame.getPixel(0, 1) == BLACK); + REQUIRE(frame.getPixel(0, 2) == BLACK); + REQUIRE(frame.getPixel(1, 0) == data[1][0]); + REQUIRE(frame.getPixel(1, 1) == data[1][1]); + REQUIRE(frame.getPixel(1, 2) == BLACK); + REQUIRE(frame.getPixel(2, 0) == data[2][0]); + REQUIRE(frame.getPixel(2, 1) == data[2][1]); + REQUIRE(frame.getPixel(2, 2) == data[2][2]); + } + SECTION("An empty RGB vector produces an empty TCameraFrame") { + std::vector> data = {}; + auto frame = RGBCameraFrame(data, std::chrono::steady_clock::now()); + REQUIRE(frame.getRows() == 0); + REQUIRE(frame.getCols() == 0); + REQUIRE(frame.getColorSpace() == RGB); + } +} diff --git a/SLAM/test/telemetry/DepthMapTest.cpp b/SLAM/test/telemetry/DepthMapTest.cpp new file mode 100644 index 0000000..b21f9eb --- /dev/null +++ b/SLAM/test/telemetry/DepthMapTest.cpp @@ -0,0 +1,29 @@ +// +// structDepthMap.cpp +// StructureForDepthMap +// +// Created by Aubrey Parker on 2/8/17. + + +#include "../../include/telemetry/DepthMap.h" +#include "catch.hpp" + +using namespace slam; + +TEST_CASE("DepthMap works as expected") { + auto test1 = DepthMap(2, 4); + + SECTION("The DepthMap constructor works as expected") { + REQUIRE(test1.getDepth(0, 0) == UNKNOWN_DEPTH); + REQUIRE(test1.getDepth(0, 3) == UNKNOWN_DEPTH); + REQUIRE(test1.getDepth(1, 0) == UNKNOWN_DEPTH); + REQUIRE(test1.getDepth(1, 3) == UNKNOWN_DEPTH); + } + + SECTION("getDepth() and setDepth() work as expected") { + test1.setDepth(0, 3, 2); + REQUIRE(test1.getDepth(0, 3) == 2); + + } +} + diff --git a/SLAM/test/telemetry/TelemetryPacketTest.cpp b/SLAM/test/telemetry/TelemetryPacketTest.cpp new file mode 100644 index 0000000..2eee891 --- /dev/null +++ b/SLAM/test/telemetry/TelemetryPacketTest.cpp @@ -0,0 +1,39 @@ +// +// Created by Kyler Jensen on 2/15/17. +// + +#include +#include "../../include/telemetry/TelemetryPacket.h" +#include "../../include/camera_frame/GrayscaleCameraFrame.h" + +using namespace slam; + +TEST_CASE("TelemetryPacket works as expected") { + + auto now = std::chrono::steady_clock::now(); + auto frame = GrayscaleCameraFrame(640, 480, now); + auto map = DepthMap(0, 0); + auto pose = Pose(); + + TelemetryPacket packet = { frame, map, pose }; + + SECTION("All of TelemetryPacket's members were initialized") { + + REQUIRE(packet.cameraFrame.getRows() == 640); + REQUIRE(packet.cameraFrame.getCols() == 480); + REQUIRE(packet.cameraFrame.getColorSpace() == GRAYSCALE); + REQUIRE(packet.cameraFrame.getTimestamp() == now); + + } + + SECTION("Chrono works as expected") { + + std::chrono::time_point ts = frame.getTimestamp(); + + REQUIRE(ts == frame.getTimestamp()); + + REQUIRE(ts == packet.cameraFrame.getTimestamp()); + + } + +} diff --git a/SLAM/test/telemetry/TelemetryServiceTest.cpp b/SLAM/test/telemetry/TelemetryServiceTest.cpp new file mode 100644 index 0000000..e7f6684 --- /dev/null +++ b/SLAM/test/telemetry/TelemetryServiceTest.cpp @@ -0,0 +1,51 @@ +// +// Created by Kyler Jensen on 4/5/17. +// + + +#include +#include "../../include/telemetry/TelemetryService.h" + +using namespace slam; + +void doSomething(TelemetryPacket) { + +} + +TEST_CASE("TelemetryService works as expected") { + + auto telemetryService = TelemetryService(); + + REQUIRE(telemetryService.subscribers() == 0); + + SECTION("The user is able to subscribe to and unsubscribe from telemetry updates") { + + auto listener = [](TelemetryPacket) { + std::cout << "Received telemetry!" << std::endl; + }; + + REQUIRE(telemetryService.subscribers() == 0); + + telemetryService.subscribe(listener); + + REQUIRE(telemetryService.subscribers() == 1); + + telemetryService.subscribe(listener); + + REQUIRE(telemetryService.subscribers() == 1); + + telemetryService.subscribe(&doSomething); + + REQUIRE(telemetryService.subscribers() == 2); + + telemetryService.unsubscribe(listener); + + REQUIRE(telemetryService.subscribers() == 1); + + telemetryService.unsubscribe(&doSomething); + + REQUIRE(telemetryService.subscribers() == 0); + + } + +} \ No newline at end of file diff --git a/SLAM/test_resources/GRAYSCALE10x10.bmp b/SLAM/test_resources/GRAYSCALE10x10.bmp new file mode 100644 index 0000000..e8f3fdf Binary files /dev/null and b/SLAM/test_resources/GRAYSCALE10x10.bmp differ diff --git a/SLAM/test_resources/GRAYSCALE10x10.jpg b/SLAM/test_resources/GRAYSCALE10x10.jpg new file mode 100644 index 0000000..9d49b2d Binary files /dev/null and b/SLAM/test_resources/GRAYSCALE10x10.jpg differ diff --git a/SLAM/test_resources/GRAYSCALE10x10.png b/SLAM/test_resources/GRAYSCALE10x10.png new file mode 100644 index 0000000..cad226c Binary files /dev/null and b/SLAM/test_resources/GRAYSCALE10x10.png differ diff --git a/SLAM/test_resources/RGB10x10.bmp b/SLAM/test_resources/RGB10x10.bmp new file mode 100644 index 0000000..04e0f1f Binary files /dev/null and b/SLAM/test_resources/RGB10x10.bmp differ diff --git a/SLAM/test_resources/RGB10x10.jpg b/SLAM/test_resources/RGB10x10.jpg new file mode 100644 index 0000000..dbb2bce Binary files /dev/null and b/SLAM/test_resources/RGB10x10.jpg differ diff --git a/SLAM/test_resources/RGB10x10.png b/SLAM/test_resources/RGB10x10.png new file mode 100644 index 0000000..5902d5b Binary files /dev/null and b/SLAM/test_resources/RGB10x10.png differ diff --git a/Tests/README.txt b/Tests/README.txt new file mode 100644 index 0000000..561fcb3 --- /dev/null +++ b/Tests/README.txt @@ -0,0 +1,39 @@ +In order to quickly test Arduino code without unpluging and repluging the +Arduino, these tests have been writen to use PlatformIO. PlatformIO enables +combiling and uploading Arduino code, directly from the terminal without ever +leaving your chair. + + + +* Installing PlatformIO * + +In a Linux terminal, run the following commands: +> python -c "$(curl -fsSL https://raw.githubusercontent.com/platformio/platformio/develop/scripts/get-platformio.py)" +> python get-platformio.py + +To confirm PlatformIO is installed sucessfully, run: +> platformio --help + + + +* Compiling and Uploading with PlatformIO * + +> # list serial ports +> platformio serialports list + +> # upload code +> platformio run -e due -t upload + +> # compile code +> platformio run -e due + + + +* Running Tests * + +Step 1. Upload the corresponding Arduino code with PlatformIO (as needed) +Step 2. Compile test w/dependencies: +> g++ --std=c++11 Test_.cpp [dependencies] -o run +Step 3. Run the test: +> ./run + diff --git a/Tests/SerialTest/PIO_ArduinoToBone/.gitignore b/Tests/SerialTest/PIO_ArduinoToBone/.gitignore new file mode 100644 index 0000000..6c69f4c --- /dev/null +++ b/Tests/SerialTest/PIO_ArduinoToBone/.gitignore @@ -0,0 +1,2 @@ +.pioenvs +.piolibdeps diff --git a/Tests/SerialTest/PIO_ArduinoToBone/.travis.yml b/Tests/SerialTest/PIO_ArduinoToBone/.travis.yml new file mode 100644 index 0000000..2c4ff5c --- /dev/null +++ b/Tests/SerialTest/PIO_ArduinoToBone/.travis.yml @@ -0,0 +1,65 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < http://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < http://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < http://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choice one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# install: +# - pip install -U platformio +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to by used as a library with examples +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/Tests/SerialTest/PIO_ArduinoToBone/lib/readme.txt b/Tests/SerialTest/PIO_ArduinoToBone/lib/readme.txt new file mode 100644 index 0000000..dbadc3d --- /dev/null +++ b/Tests/SerialTest/PIO_ArduinoToBone/lib/readme.txt @@ -0,0 +1,36 @@ + +This directory is intended for the project specific (private) libraries. +PlatformIO will compile them to static libraries and link to executable file. + +The source code of each library should be placed in separate directory, like +"lib/private_lib/[here are source files]". + +For example, see how can be organized `Foo` and `Bar` libraries: + +|--lib +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| |--Foo +| | |- Foo.c +| | |- Foo.h +| |- readme.txt --> THIS FILE +|- platformio.ini +|--src + |- main.c + +Then in `src/main.c` you should use: + +#include +#include + +// rest H/C/CPP code + +PlatformIO will find your libraries automatically, configure preprocessor's +include paths and build them. + +More information about PlatformIO Library Dependency Finder +- http://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Tests/SerialTest/PIO_ArduinoToBone/platformio.ini b/Tests/SerialTest/PIO_ArduinoToBone/platformio.ini new file mode 100644 index 0000000..b4fb831 --- /dev/null +++ b/Tests/SerialTest/PIO_ArduinoToBone/platformio.ini @@ -0,0 +1,19 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; http://docs.platformio.org/page/projectconf.html + +[env:uno] +platform = atmelavr +framework = arduino +board = uno + +[env:due] +platform = atmelsam +framework = arduino +board = due \ No newline at end of file diff --git a/Tests/SerialTest/PIO_ArduinoToBone/src/Arduino.ino b/Tests/SerialTest/PIO_ArduinoToBone/src/Arduino.ino new file mode 100644 index 0000000..38d4756 --- /dev/null +++ b/Tests/SerialTest/PIO_ArduinoToBone/src/Arduino.ino @@ -0,0 +1,23 @@ +#include +#include "SerialTools.h" + +#define B115200 115200 +#define GARBAGE 0 + +void setup() { + Serial.begin(B115200); + SerialTools::writeString("Ready!", 6); + while((!Serial.available())||(Serial.read()==GARBAGE)); + + int test = 0x0A0B0C0D; + SerialTools::writeData((char*)&test, 4); + SerialTools::writeByte(0xFF); + SerialTools::writeFloat(0.1F); + SerialTools::writeDouble(0.2); + SerialTools::writeInt(-3); + SerialTools::writeUInt(4); + SerialTools::writeShort(-5); + SerialTools::writeUShort(6); +} + +void loop() { } \ No newline at end of file diff --git a/Tests/SerialTest/PIO_ArduinoToBone/src/SerialTools.h b/Tests/SerialTest/PIO_ArduinoToBone/src/SerialTools.h new file mode 100644 index 0000000..c1fe220 --- /dev/null +++ b/Tests/SerialTest/PIO_ArduinoToBone/src/SerialTools.h @@ -0,0 +1,145 @@ +/** + * Created by TekuConcept on 8/3/2016. + */ +#ifndef SERIAL_TOOLS_H +#define SERIAL_TOOLS_H + +/** + * Extend the functionality of the Serial object, + * providing various useful type value streams. + */ +class SerialTools{ +public: + + /* - - - - - - - - - - - - - - - - - - - - - - - - - - *\ + * READ FUNCTIONS * + \* - - - - - - - - - - - - - - - - - - - - - - - - - - */ + + /* reading strings is not yet implemented here */ + static uint8_t readByte() { + uint8_t result = 0; + readData((char*)&result, sizeof(uint8_t)); + return result; + } + + static float readFloat() { + float result = 0; + readData((char*)&result, sizeof(float)); + return result; + } + + static double readDouble() { + double result = 0; + readData((char*)&result, sizeof(double)); + return result; + } + + static int32_t readInt() { + int result = 0; + readData((char*)&result, sizeof(int32_t)); + return result; + } + + static uint32_t readUInt() { + uint32_t result = 0; + readData((char*)&result, sizeof(uint32_t)); + return result; + } + + static int16_t readShort() { + int16_t result = 0; + readData((char*)&result, sizeof(int16_t)); + return result; + } + + static uint16_t readUShort() { + uint16_t result = 0; + readData((char*)&result, sizeof(uint16_t)); + return result; + } + + static void readData(char* ptr, int size) { + while(Serial.available() < size); + for(int i = 0; i < size; i++) { + ptr[i] = Serial.read(); + } + } + + /* - - - - - - - - - - - - - - - - - - - - - - - - - - *\ + * WRITE FUNCTIONS * + \* - - - - - - - - - - - - - - - - - - - - - - - - - - */ + + /** + * Writes a null-terminated string to the Serial's + * out buffer. + */ + static void writeString(const char* data, int size) { + for(int i = 0; i < size; i++) { + Serial.print(data[i]); + } + Serial.print('\0'); + } + + static void writeByte(uint8_t out) { + writeData((char*)&out, sizeof(uint8_t)); + } + + static void writeFloat(float out) { + writeData((char*)&out, sizeof(float)); + } + + static void writeDouble(double out) { + writeData((char*)&out, sizeof(double)); + } + + static void writeInt(int32_t out) { + writeData((char*)&out, sizeof(int32_t)); + } + + static void writeUInt(uint32_t out) { + writeData((char*)&out, sizeof(uint32_t)); + } + + static void writeShort(int16_t out) { + writeData((char*)&out, sizeof(int16_t)); + } + + static void writeUShort(uint16_t out) { + writeData((char*)&out, sizeof(uint16_t)); + } + + static void writeData(char* ptr, int size) { + for(int i = 0; i < size; i++){ + // #ifdef DEBUG + // printHex((uint8_t)ptr[i]); + // #else + Serial.print(ptr[i]); + // #endif + } + } + + static void printHex(uint8_t c) { + printHexChar((uint8_t)((c&0xF0) >> 4)); + printHexChar((uint8_t)(c&0x0F)); + } + +private: + SerialTools(){} + static void printHexChar(uint8_t val) { + char res = '0'; + if(val < 10) + Serial.print(val); + else + switch(val-10) { + case 0: res = 'A'; break; + case 1: res = 'B'; break; + case 2: res = 'C'; break; + case 3: res = 'D'; break; + case 4: res = 'E'; break; + case 5: res = 'F'; break; + } + Serial.print(res); + } +}; + +#endif \ No newline at end of file diff --git a/Tests/SerialTest/PIO_BoneToArduino/.gitignore b/Tests/SerialTest/PIO_BoneToArduino/.gitignore new file mode 100644 index 0000000..6c69f4c --- /dev/null +++ b/Tests/SerialTest/PIO_BoneToArduino/.gitignore @@ -0,0 +1,2 @@ +.pioenvs +.piolibdeps diff --git a/Tests/SerialTest/PIO_BoneToArduino/.travis.yml b/Tests/SerialTest/PIO_BoneToArduino/.travis.yml new file mode 100644 index 0000000..2c4ff5c --- /dev/null +++ b/Tests/SerialTest/PIO_BoneToArduino/.travis.yml @@ -0,0 +1,65 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < http://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < http://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < http://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choice one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# install: +# - pip install -U platformio +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to by used as a library with examples +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/Tests/SerialTest/PIO_BoneToArduino/lib/readme.txt b/Tests/SerialTest/PIO_BoneToArduino/lib/readme.txt new file mode 100644 index 0000000..dbadc3d --- /dev/null +++ b/Tests/SerialTest/PIO_BoneToArduino/lib/readme.txt @@ -0,0 +1,36 @@ + +This directory is intended for the project specific (private) libraries. +PlatformIO will compile them to static libraries and link to executable file. + +The source code of each library should be placed in separate directory, like +"lib/private_lib/[here are source files]". + +For example, see how can be organized `Foo` and `Bar` libraries: + +|--lib +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| |--Foo +| | |- Foo.c +| | |- Foo.h +| |- readme.txt --> THIS FILE +|- platformio.ini +|--src + |- main.c + +Then in `src/main.c` you should use: + +#include +#include + +// rest H/C/CPP code + +PlatformIO will find your libraries automatically, configure preprocessor's +include paths and build them. + +More information about PlatformIO Library Dependency Finder +- http://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Tests/SerialTest/PIO_BoneToArduino/platformio.ini b/Tests/SerialTest/PIO_BoneToArduino/platformio.ini new file mode 100644 index 0000000..b4fb831 --- /dev/null +++ b/Tests/SerialTest/PIO_BoneToArduino/platformio.ini @@ -0,0 +1,19 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; http://docs.platformio.org/page/projectconf.html + +[env:uno] +platform = atmelavr +framework = arduino +board = uno + +[env:due] +platform = atmelsam +framework = arduino +board = due \ No newline at end of file diff --git a/Tests/SerialTest/PIO_BoneToArduino/src/Arduino.ino b/Tests/SerialTest/PIO_BoneToArduino/src/Arduino.ino new file mode 100644 index 0000000..179ff4a --- /dev/null +++ b/Tests/SerialTest/PIO_BoneToArduino/src/Arduino.ino @@ -0,0 +1,29 @@ +#include +#include "SerialTools.h" + +#define m_assert(T,V) if(T != V) { result = (result<<1)|1; } else { result <<= 1; } +#define B115200 115200 +#define GARBAGE 0 + +void setup() { + Serial.begin(B115200); + SerialTools::writeString("Ready!", 6); + while((!Serial.available())||(Serial.read()==GARBAGE)); + + uint16_t result = 0; + int test = 0; + + SerialTools::readData((char*)&test, sizeof(int)); + m_assert(test, 0x0A0B0C0D) + m_assert(SerialTools::readByte(), 0xFF) + m_assert(SerialTools::readFloat(), 0.1F) + m_assert(SerialTools::readDouble(), 0.2) + m_assert(SerialTools::readInt(), -3) + m_assert(SerialTools::readUInt(), 4) + m_assert(SerialTools::readShort(), -5) + m_assert(SerialTools::readUShort(), 6) + + SerialTools::writeInt(result); +} + +void loop() { } \ No newline at end of file diff --git a/Tests/SerialTest/PIO_BoneToArduino/src/SerialTools.h b/Tests/SerialTest/PIO_BoneToArduino/src/SerialTools.h new file mode 100644 index 0000000..c1fe220 --- /dev/null +++ b/Tests/SerialTest/PIO_BoneToArduino/src/SerialTools.h @@ -0,0 +1,145 @@ +/** + * Created by TekuConcept on 8/3/2016. + */ +#ifndef SERIAL_TOOLS_H +#define SERIAL_TOOLS_H + +/** + * Extend the functionality of the Serial object, + * providing various useful type value streams. + */ +class SerialTools{ +public: + + /* - - - - - - - - - - - - - - - - - - - - - - - - - - *\ + * READ FUNCTIONS * + \* - - - - - - - - - - - - - - - - - - - - - - - - - - */ + + /* reading strings is not yet implemented here */ + static uint8_t readByte() { + uint8_t result = 0; + readData((char*)&result, sizeof(uint8_t)); + return result; + } + + static float readFloat() { + float result = 0; + readData((char*)&result, sizeof(float)); + return result; + } + + static double readDouble() { + double result = 0; + readData((char*)&result, sizeof(double)); + return result; + } + + static int32_t readInt() { + int result = 0; + readData((char*)&result, sizeof(int32_t)); + return result; + } + + static uint32_t readUInt() { + uint32_t result = 0; + readData((char*)&result, sizeof(uint32_t)); + return result; + } + + static int16_t readShort() { + int16_t result = 0; + readData((char*)&result, sizeof(int16_t)); + return result; + } + + static uint16_t readUShort() { + uint16_t result = 0; + readData((char*)&result, sizeof(uint16_t)); + return result; + } + + static void readData(char* ptr, int size) { + while(Serial.available() < size); + for(int i = 0; i < size; i++) { + ptr[i] = Serial.read(); + } + } + + /* - - - - - - - - - - - - - - - - - - - - - - - - - - *\ + * WRITE FUNCTIONS * + \* - - - - - - - - - - - - - - - - - - - - - - - - - - */ + + /** + * Writes a null-terminated string to the Serial's + * out buffer. + */ + static void writeString(const char* data, int size) { + for(int i = 0; i < size; i++) { + Serial.print(data[i]); + } + Serial.print('\0'); + } + + static void writeByte(uint8_t out) { + writeData((char*)&out, sizeof(uint8_t)); + } + + static void writeFloat(float out) { + writeData((char*)&out, sizeof(float)); + } + + static void writeDouble(double out) { + writeData((char*)&out, sizeof(double)); + } + + static void writeInt(int32_t out) { + writeData((char*)&out, sizeof(int32_t)); + } + + static void writeUInt(uint32_t out) { + writeData((char*)&out, sizeof(uint32_t)); + } + + static void writeShort(int16_t out) { + writeData((char*)&out, sizeof(int16_t)); + } + + static void writeUShort(uint16_t out) { + writeData((char*)&out, sizeof(uint16_t)); + } + + static void writeData(char* ptr, int size) { + for(int i = 0; i < size; i++){ + // #ifdef DEBUG + // printHex((uint8_t)ptr[i]); + // #else + Serial.print(ptr[i]); + // #endif + } + } + + static void printHex(uint8_t c) { + printHexChar((uint8_t)((c&0xF0) >> 4)); + printHexChar((uint8_t)(c&0x0F)); + } + +private: + SerialTools(){} + static void printHexChar(uint8_t val) { + char res = '0'; + if(val < 10) + Serial.print(val); + else + switch(val-10) { + case 0: res = 'A'; break; + case 1: res = 'B'; break; + case 2: res = 'C'; break; + case 3: res = 'D'; break; + case 4: res = 'E'; break; + case 5: res = 'F'; break; + } + Serial.print(res); + } +}; + +#endif \ No newline at end of file diff --git a/Tests/SerialTest/PIO_StructDataType/.gitignore b/Tests/SerialTest/PIO_StructDataType/.gitignore new file mode 100644 index 0000000..6c69f4c --- /dev/null +++ b/Tests/SerialTest/PIO_StructDataType/.gitignore @@ -0,0 +1,2 @@ +.pioenvs +.piolibdeps diff --git a/Tests/SerialTest/PIO_StructDataType/.travis.yml b/Tests/SerialTest/PIO_StructDataType/.travis.yml new file mode 100644 index 0000000..2c4ff5c --- /dev/null +++ b/Tests/SerialTest/PIO_StructDataType/.travis.yml @@ -0,0 +1,65 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < http://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < http://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < http://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choice one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# install: +# - pip install -U platformio +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to by used as a library with examples +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/Tests/SerialTest/PIO_StructDataType/lib/readme.txt b/Tests/SerialTest/PIO_StructDataType/lib/readme.txt new file mode 100644 index 0000000..dbadc3d --- /dev/null +++ b/Tests/SerialTest/PIO_StructDataType/lib/readme.txt @@ -0,0 +1,36 @@ + +This directory is intended for the project specific (private) libraries. +PlatformIO will compile them to static libraries and link to executable file. + +The source code of each library should be placed in separate directory, like +"lib/private_lib/[here are source files]". + +For example, see how can be organized `Foo` and `Bar` libraries: + +|--lib +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| |--Foo +| | |- Foo.c +| | |- Foo.h +| |- readme.txt --> THIS FILE +|- platformio.ini +|--src + |- main.c + +Then in `src/main.c` you should use: + +#include +#include + +// rest H/C/CPP code + +PlatformIO will find your libraries automatically, configure preprocessor's +include paths and build them. + +More information about PlatformIO Library Dependency Finder +- http://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Tests/SerialTest/PIO_StructDataType/platformio.ini b/Tests/SerialTest/PIO_StructDataType/platformio.ini new file mode 100644 index 0000000..b4fb831 --- /dev/null +++ b/Tests/SerialTest/PIO_StructDataType/platformio.ini @@ -0,0 +1,19 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; http://docs.platformio.org/page/projectconf.html + +[env:uno] +platform = atmelavr +framework = arduino +board = uno + +[env:due] +platform = atmelsam +framework = arduino +board = due \ No newline at end of file diff --git a/Tests/SerialTest/PIO_StructDataType/src/Arduino.ino b/Tests/SerialTest/PIO_StructDataType/src/Arduino.ino new file mode 100644 index 0000000..200f4ff --- /dev/null +++ b/Tests/SerialTest/PIO_StructDataType/src/Arduino.ino @@ -0,0 +1,47 @@ +#include +#include "SerialTools.h" + +#define B115200 115200 +#define GARBAGE 0 +#define m_assert(T,V) if(T != V) { result = (result<<1)|1; } else { result <<= 1; } + +typedef struct Data { + float a; + double b; + int32_t c; + uint32_t d; + int16_t e; + uint16_t f; +} Data; + +void setup() { + Serial.begin(B115200); + SerialTools::writeString("Ready!", 6); + while((!Serial.available())||(Serial.read()==GARBAGE)); + + uint16_t result = 0; + + Data data; + data.a = 0.1F; + data.b = 0.2; + data.c = -3; + data.d = 4; + data.e = -5; + data.f = 6; + + SerialTools::writeData((char*)&data, sizeof(Data)); + + Data dmod; + SerialTools::readData((char*)&dmod, sizeof(Data)); + + m_assert(dmod.a, data.a*2) + m_assert(dmod.b, data.b*2) + m_assert(dmod.c, data.c*2) + m_assert(dmod.d, data.d*2) + m_assert(dmod.e, data.e*2) + m_assert(dmod.f, data.f*2) + + SerialTools::writeShort(result); +} + +void loop() { } \ No newline at end of file diff --git a/Tests/SerialTest/PIO_StructDataType/src/SerialTools.h b/Tests/SerialTest/PIO_StructDataType/src/SerialTools.h new file mode 100644 index 0000000..c1fe220 --- /dev/null +++ b/Tests/SerialTest/PIO_StructDataType/src/SerialTools.h @@ -0,0 +1,145 @@ +/** + * Created by TekuConcept on 8/3/2016. + */ +#ifndef SERIAL_TOOLS_H +#define SERIAL_TOOLS_H + +/** + * Extend the functionality of the Serial object, + * providing various useful type value streams. + */ +class SerialTools{ +public: + + /* - - - - - - - - - - - - - - - - - - - - - - - - - - *\ + * READ FUNCTIONS * + \* - - - - - - - - - - - - - - - - - - - - - - - - - - */ + + /* reading strings is not yet implemented here */ + static uint8_t readByte() { + uint8_t result = 0; + readData((char*)&result, sizeof(uint8_t)); + return result; + } + + static float readFloat() { + float result = 0; + readData((char*)&result, sizeof(float)); + return result; + } + + static double readDouble() { + double result = 0; + readData((char*)&result, sizeof(double)); + return result; + } + + static int32_t readInt() { + int result = 0; + readData((char*)&result, sizeof(int32_t)); + return result; + } + + static uint32_t readUInt() { + uint32_t result = 0; + readData((char*)&result, sizeof(uint32_t)); + return result; + } + + static int16_t readShort() { + int16_t result = 0; + readData((char*)&result, sizeof(int16_t)); + return result; + } + + static uint16_t readUShort() { + uint16_t result = 0; + readData((char*)&result, sizeof(uint16_t)); + return result; + } + + static void readData(char* ptr, int size) { + while(Serial.available() < size); + for(int i = 0; i < size; i++) { + ptr[i] = Serial.read(); + } + } + + /* - - - - - - - - - - - - - - - - - - - - - - - - - - *\ + * WRITE FUNCTIONS * + \* - - - - - - - - - - - - - - - - - - - - - - - - - - */ + + /** + * Writes a null-terminated string to the Serial's + * out buffer. + */ + static void writeString(const char* data, int size) { + for(int i = 0; i < size; i++) { + Serial.print(data[i]); + } + Serial.print('\0'); + } + + static void writeByte(uint8_t out) { + writeData((char*)&out, sizeof(uint8_t)); + } + + static void writeFloat(float out) { + writeData((char*)&out, sizeof(float)); + } + + static void writeDouble(double out) { + writeData((char*)&out, sizeof(double)); + } + + static void writeInt(int32_t out) { + writeData((char*)&out, sizeof(int32_t)); + } + + static void writeUInt(uint32_t out) { + writeData((char*)&out, sizeof(uint32_t)); + } + + static void writeShort(int16_t out) { + writeData((char*)&out, sizeof(int16_t)); + } + + static void writeUShort(uint16_t out) { + writeData((char*)&out, sizeof(uint16_t)); + } + + static void writeData(char* ptr, int size) { + for(int i = 0; i < size; i++){ + // #ifdef DEBUG + // printHex((uint8_t)ptr[i]); + // #else + Serial.print(ptr[i]); + // #endif + } + } + + static void printHex(uint8_t c) { + printHexChar((uint8_t)((c&0xF0) >> 4)); + printHexChar((uint8_t)(c&0x0F)); + } + +private: + SerialTools(){} + static void printHexChar(uint8_t val) { + char res = '0'; + if(val < 10) + Serial.print(val); + else + switch(val-10) { + case 0: res = 'A'; break; + case 1: res = 'B'; break; + case 2: res = 'C'; break; + case 3: res = 'D'; break; + case 4: res = 'E'; break; + case 5: res = 'F'; break; + } + Serial.print(res); + } +}; + +#endif \ No newline at end of file diff --git a/Tests/SerialTest/Serial.cpp b/Tests/SerialTest/Serial.cpp new file mode 100644 index 0000000..3660af4 --- /dev/null +++ b/Tests/SerialTest/Serial.cpp @@ -0,0 +1,179 @@ +// +// Created by Nathan Copier on 1/28/2016. +// Refactored by TekuConcept on 2/13/2017. +// + +#include "Serial.h" + +std::mutex Serial::serialLock_; + +Serial::Serial(std::string device) { +#ifdef DEBUG + LOG("\nReceived Device Name: " << device); + LOG("\nEntering Serial Debug Mode\n"); + fd = 0; +#else + if((fd = open(device.c_str(), O_RDWR)) < 0) { + LOG("Device failed to open... entering dummy mode.\n"); + fd = 0; + return; + } + + configure(); +#endif +} + +Serial::~Serial() { + if(fd != 0) close(fd); +} + +void Serial::configure() { + struct termios topts; + if(tcgetattr(fd, &topts)) { + LOG("Failed to get terminal ios options from file descriptor.\n"); + throw 1; + } + if(cfsetispeed(&topts, B115200)) { + LOG("Failed to set input baud rate.\n"); + throw 1; + } + if(cfsetospeed(&topts, B115200)) { + LOG("Failed to set output baud rate.\n"); + throw 1; + } + topts.c_cflag &= ~(PARENB|CSTOPB|CSIZE|CRTSCTS); + topts.c_cflag |= CS8|CREAD|CLOCAL; + topts.c_iflag &= ~(IXON | IXOFF | IXANY); + topts.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + topts.c_oflag &= ~OPOST; + topts.c_cc[VMIN] = 1; + topts.c_cc[VTIME] = 0; + if(tcsetattr(fd, TCSANOW, &topts)) { + LOG("Failed to set terminal ios options for file descriptor.\n"); + throw 1; + } + if(tcflush(fd, TCIFLUSH)) { + LOG("Failed to flush file descriptor.\n"); + throw 1; + } +} + +std::string Serial::readString() { +#ifdef DEBUG + return "Dummy String\0"; +#else + if(fd==0) return ""; + std::stringstream ss; + char c[1]; + + do{ + read(fd, c, 1); + ss << c[0]; + }while(c[0] != '\0'); + return ss.str(); +#endif +} + +unsigned char Serial::readByte() { + unsigned char result = 0; + readData((char*)&result, 1); + return result; +} + +float Serial::readFloat() { + float result = 0; + readData((char*)&result, 4); + return result; +} + +double Serial::readDouble() { + double result = 0; +#ifdef DUE + readData((char*)&result, 8); +#else + float data = 0; + readData((char*)&data, 4); + result = (double)data; +#endif + return result; +} + +int Serial::readInt() { + int result = 0; + readData((char*)&result, 4); + return result; +} + +unsigned int Serial::readUInt() { + unsigned int result = 0; + readData((char*)&result, 4); + return result; +} + +short Serial::readShort() { + short result = 0; + readData((char*)&result, 2); + return result; +} + +unsigned short Serial::readUShort() { + unsigned short result = 0; + readData((char*)&result, 2); + return result; +} + +void Serial::readData(char* ptr, size_t size) { + std::lock_guard guard(serialLock_); + if(fd!=0) read(fd, ptr, size); + else for(unsigned int i = 0; i < size; i++) ptr[i] = 0; +} + + + + +void Serial::writeFloat(float value) { + writeData((char*)&value, 4); +} + +void Serial::writeDouble(double value) { +#ifdef DUE + writeData((char*)&value, 8); +#else + float val2 = (float)value; + writeData((char*)&value, 4); +#endif +} + +void Serial::writeInt(int value) { + writeData((char*)&value, 4); +} + +void Serial::writeUInt(unsigned int value) { + writeData((char*)&value, 4); +} + +void Serial::writeShort(short value) { + writeData((char*)&value, 2); +} + +void Serial::writeUShort(unsigned short value) { + writeData((char*)&value, 2); +} + +void Serial::writeByte(unsigned char value) { + writeData((char*)&value, 1); +} + +void Serial::writeData(char* ptr, size_t size) { +#ifdef DEBUG + LOG("Serial Write: " << std::hex << std::setw(2)); + for(size_t i = 0; i < size; i++) { + LOG((unsigned short)ptr[i]); + } + LOG(std::dec << std::endl); +#else + std::lock_guard guard(serialLock_); + if(fd == 0) return; + write(fd, ptr, size); +#endif +} \ No newline at end of file diff --git a/Tests/SerialTest/Serial.h b/Tests/SerialTest/Serial.h new file mode 100644 index 0000000..f3aa4ed --- /dev/null +++ b/Tests/SerialTest/Serial.h @@ -0,0 +1,58 @@ +// +// Created by Nathan Copier on 1/28/2016. +// Refactored by TekuConcept on 8/3/2016. +// + +#ifndef PERIPHERALS_SERIAL_H +#define PERIPHERALS_SERIAL_H + +#define DUE + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define LOG(x) std::cerr << x + +class Serial { +private: + static std::mutex serialLock_; + int fd; + + void configure(); + +public: + Serial(std::string device); + ~Serial(); + + std::string readString(); + unsigned char readByte(); + float readFloat(); + double readDouble(); + int readInt(); + unsigned int readUInt(); + short readShort(); + unsigned short readUShort(); + void readData(char* ptr, size_t size); + + void writeFloat(float value); + void writeDouble(double value); + void writeInt(int value); + void writeUInt(unsigned int value); + void writeShort(short value); + void writeUShort(unsigned short value); + void writeByte(unsigned char value); + void writeData(char* ptr, size_t size); +}; + + +#endif //PERIPHERALS_SERIAL_H \ No newline at end of file diff --git a/Tests/SerialTest/Test_ArduinoToBone.cpp b/Tests/SerialTest/Test_ArduinoToBone.cpp new file mode 100644 index 0000000..f1f6dc6 --- /dev/null +++ b/Tests/SerialTest/Test_ArduinoToBone.cpp @@ -0,0 +1,51 @@ +#include +#include +#include "Serial.h" + +#define MSG(x) std::cout << x +#define MSGN(x) std::cout << x << "\n" +#define ERR(x) std::cerr << x << "\n" +#define m_assert(x,y) (x) != (y) ? MSGN("FAIL") : MSGN("PASS") + +int main() { + MSGN("-- BEGIN TEST --"); + + MSG("\nCreating Serial Object... "); + Serial serial("/dev/ttyACM0"); + MSGN("Done!\n"); + + std::string res = serial.readString(); + MSG("Message: "); MSGN(res); + serial.writeByte('R'); + + MSGN("Receiving Static Values from Arduino..."); + + int bytes = 0; + MSG("> Receiving Bytes...\t\t"); + serial.readData((char*)&bytes, 4); + m_assert(bytes, 0x0A0B0C0D); + + MSG("> Receiving Byte...\t\t"); + m_assert(serial.readByte(), 0xFF); + + MSG("> Receiving Float...\t\t"); + m_assert(serial.readFloat(), 0.1F); + + MSG("> Receiving Double...\t\t"); + m_assert(serial.readDouble(), 0.2); + + MSG("> Receiving Int...\t\t"); + m_assert(serial.readInt(), -3); + + MSG("> Receiving Unsigned Int...\t"); + m_assert(serial.readUInt(), 4); + + MSG("> Receiving Short...\t\t"); + m_assert(serial.readShort(), -5); + + MSG("> Receiving Unsigned Short...\t"); + m_assert(serial.readUShort(), 6); + + MSGN("\n-- END TEST --"); + return 0; +} \ No newline at end of file diff --git a/Tests/SerialTest/Test_BoneToArduino.cpp b/Tests/SerialTest/Test_BoneToArduino.cpp new file mode 100644 index 0000000..5a73ca7 --- /dev/null +++ b/Tests/SerialTest/Test_BoneToArduino.cpp @@ -0,0 +1,48 @@ +#include +#include +#include "Serial.h" + +#define MSG(x) std::cout << x +#define MSGN(x) std::cout << x << "\n" +#define ERR(x) std::cerr << x << "\n" +#define m_assert(x) (x)&0x01 == 1 ? MSGN("FAIL") : MSGN("PASS") + +int main() { + MSGN("-- BEGIN TEST --"); + + MSG("\nCreating Serial Object... "); + Serial serial("/dev/ttyACM0"); + MSGN("Done!\n"); + + std::string res = serial.readString(); + MSG("Message: "); MSGN(res); + serial.writeByte('R'); + + MSGN("Sending Static Values to Arduino..."); + int bytes = 0x0A0B0C0D; + + serial.writeData((char*)&bytes, 4); + serial.writeByte(0xFF); + serial.writeFloat(0.1F); + serial.writeDouble(0.2); + serial.writeInt(-3); + serial.writeUInt(4); + serial.writeShort(-5); + serial.writeUShort(6); + + MSG("Results: "); + int ires = serial.readInt(); + + MSGN(ires); + MSG("> Sending Bytes\t\t\t"); m_assert(ires>>7); + MSG("> Sending Byte\t\t\t"); m_assert(ires>>6); + MSG("> Sending Float\t\t\t"); m_assert(ires>>5); + MSG("> Sending Double\t\t"); m_assert(ires>>4); + MSG("> Sending Int\t\t\t"); m_assert(ires>>3); + MSG("> Sending Unsigned Int\t\t"); m_assert(ires>>2); + MSG("> Sending Short\t\t\t"); m_assert(ires>>1); + MSG("> Sending Unsigned Short\t"); m_assert(ires>>0); + + MSGN("\n-- END TEST --"); + return 0; +} \ No newline at end of file diff --git a/Tests/SerialTest/Test_StructDataType.cpp b/Tests/SerialTest/Test_StructDataType.cpp new file mode 100644 index 0000000..2f2c652 --- /dev/null +++ b/Tests/SerialTest/Test_StructDataType.cpp @@ -0,0 +1,76 @@ +#include +#include +#include "Serial.h" + +#define MSG(x) std::cout << x +#define MSGN(x) std::cout << x << "\n" +#define ERR(x) std::cerr << x << "\n" +#define m_assert(x,y) (x) != (y) ? MSGN("FAIL") : MSGN("PASS") + +typedef struct Data { + float a; + double b; + int c; + unsigned d; + short e; + unsigned short f; +} Data; + + + +int main() { + MSGN("-- BEGIN TEST --"); + + MSG("\nCreating Serial Object... "); + Serial serial("/dev/ttyACM0"); + MSGN("Done!\n"); + + std::string res = serial.readString(); + MSG("Message: "); MSGN(res); + serial.writeByte('R'); + + Data data; + data.a = 0.0F; + data.b = 0.0; + data.c = 0; + data.d = 0; + data.e = 0; + data.f = 0; + + MSGN("Receiving Static Values from Arduino..."); + serial.readData((char*)&data, sizeof(Data)); + // MSG("> "); MSGN(data.a); + // MSG("> "); MSGN(data.b); + // MSG("> "); MSGN(data.c); + // MSG("> "); MSGN(data.d); + // MSG("> "); MSGN(data.e); + // MSG("> "); MSGN(data.f); + MSG("> Float:\t"); m_assert(data.a, 0.1F); + MSG("> Double:\t"); m_assert(data.b, 0.2); + MSG("> Int:\t\t"); m_assert(data.c, -3); + MSG("> UInt:\t\t"); m_assert(data.d, 4); + MSG("> Short:\t"); m_assert(data.e, -5); + MSG("> UShort:\t"); m_assert(data.f, 6); + + data.a *= 2; + data.b *= 2; + data.c *= 2; + data.d *= 2; + data.e *= 2; + data.f *= 2; + + MSGN("Sending Modified Static Values to Arduino..."); + serial.writeData((char*)&data, sizeof(Data)); + short err = serial.readShort(); + MSG("Result: "); MSGN(err); + + MSG("> Float:\t"); m_assert((err>>5)&1, 0); + MSG("> Double:\t"); m_assert((err>>4)&1, 0); + MSG("> Int:\t\t"); m_assert((err>>3)&1, 0); + MSG("> UInt:\t\t"); m_assert((err>>2)&1, 0); + MSG("> Short:\t"); m_assert((err>>1)&1, 0); + MSG("> UShort:\t"); m_assert((err>>0)&1, 0); + + MSGN("\n-- END TEST --"); + return 0; +} \ No newline at end of file