Rework library without unix sockets for now
This commit is contained in:
Konstantin Lampalzer
parent
e9ae1a320a
commit
0bef6035ae
30 changed files with 987 additions and 136 deletions
187
server/.idea/workspace.xml
generated
Normal file
187
server/.idea/workspace.xml
generated
Normal file
|
|
@ -0,0 +1,187 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<project version="4">
|
||||||
|
<component name="CMakeRunConfigurationManager" shouldGenerate="true" shouldDeleteObsolete="true">
|
||||||
|
<generated>
|
||||||
|
<config projectName="compLib_server" targetName="compLib_server" />
|
||||||
|
</generated>
|
||||||
|
</component>
|
||||||
|
<component name="CMakeSettings" AUTO_RELOAD="true">
|
||||||
|
<configurations>
|
||||||
|
<configuration PROFILE_NAME="Debug" CONFIG_NAME="Debug" />
|
||||||
|
</configurations>
|
||||||
|
</component>
|
||||||
|
<component name="ChangeListManager">
|
||||||
|
<list default="true" id="8af7440c-54ee-4d31-9382-d9c6297ebc16" name="Default Changelist" comment="">
|
||||||
|
<change beforePath="$PROJECT_DIR$/CMakeLists.txt" beforeDir="false" afterPath="$PROJECT_DIR$/CMakeLists.txt" afterDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeCache.txt" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/3.17.3/CMakeCCompiler.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/3.17.3/CMakeCXXCompiler.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/3.17.3/CMakeDetermineCompilerABI_C.bin" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/3.17.3/CMakeDetermineCompilerABI_CXX.bin" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/3.17.3/CMakeSystem.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdC/CMakeCCompilerId.c" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdCXX/CMakeCXXCompilerId.cpp" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/CMakeDirectoryInformation.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/CMakeError.log" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/CMakeOutput.log" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/CMakeRuleHashes.txt" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/Makefile.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/Makefile2" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/TargetDirectories.txt" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/clion-environment.txt" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/clion-log.txt" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/cmake.check_cache" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/CXX.includecache" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/DependInfo.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/build.make" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/cmake_clean.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/depend.internal" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/depend.make" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/flags.make" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/link.txt" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/compLib_server.dir/progress.make" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/progress.marks" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/CXX.includecache" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/DependInfo.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/build.make" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/cmake_clean.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/depend.internal" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/depend.make" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/flags.make" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/link.txt" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CMakeFiles/server.dir/progress.make" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CompLib.pb.cc" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/CompLib.pb.h" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/Makefile" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/cmake_install.cmake" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/compLib_server" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/compLib_server.cbp" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/server" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/cmake-build-debug/server.cbp" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/src/Spi.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/Spi.cpp" afterDir="false" />
|
||||||
|
</list>
|
||||||
|
<option name="SHOW_DIALOG" value="false" />
|
||||||
|
<option name="HIGHLIGHT_CONFLICTS" value="true" />
|
||||||
|
<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
|
||||||
|
<option name="LAST_RESOLUTION" value="IGNORE" />
|
||||||
|
</component>
|
||||||
|
<component name="ClangdSettings">
|
||||||
|
<option name="formatViaClangd" value="false" />
|
||||||
|
</component>
|
||||||
|
<component name="ExecutionTargetManager" SELECTED_TARGET="CMakeBuildProfile:Debug" />
|
||||||
|
<component name="Git.Settings">
|
||||||
|
<option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$/.." />
|
||||||
|
</component>
|
||||||
|
<component name="ProjectId" id="260hg7Cr430WVTtVvH0hiiD9T6r" />
|
||||||
|
<component name="ProjectLevelVcsManager" settingsEditedManually="true" />
|
||||||
|
<component name="ProjectViewState">
|
||||||
|
<option name="autoscrollFromSource" value="true" />
|
||||||
|
<option name="hideEmptyMiddlePackages" value="true" />
|
||||||
|
<option name="showLibraryContents" value="true" />
|
||||||
|
</component>
|
||||||
|
<component name="PropertiesComponent">
|
||||||
|
<property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
|
||||||
|
<property name="WebServerToolWindowFactoryState" value="false" />
|
||||||
|
<property name="cf.first.check.clang-format" value="false" />
|
||||||
|
<property name="last_opened_file_path" value="$PROJECT_DIR$/third_party/asio" />
|
||||||
|
<property name="settings.editor.selected.configurable" value="settings.saveactions" />
|
||||||
|
</component>
|
||||||
|
<component name="RecentsManager">
|
||||||
|
<key name="MoveFile.RECENT_KEYS">
|
||||||
|
<recent name="$PROJECT_DIR$" />
|
||||||
|
<recent name="$PROJECT_DIR$/src" />
|
||||||
|
</key>
|
||||||
|
<key name="CopyFile.RECENT_KEYS">
|
||||||
|
<recent name="$PROJECT_DIR$/third_party/asio" />
|
||||||
|
</key>
|
||||||
|
</component>
|
||||||
|
<component name="RunManager">
|
||||||
|
<configuration default="true" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" PASS_PARENT_ENVS_2="true">
|
||||||
|
<method v="2">
|
||||||
|
<option name="CLION.EXTERNAL.BUILD" enabled="true" />
|
||||||
|
</method>
|
||||||
|
</configuration>
|
||||||
|
<configuration name="compLib_server" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="compLib_server" TARGET_NAME="compLib_server" CONFIG_NAME="Debug" RUN_TARGET_PROJECT_NAME="compLib_server" RUN_TARGET_NAME="compLib_server">
|
||||||
|
<method v="2">
|
||||||
|
<option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
|
||||||
|
</method>
|
||||||
|
</configuration>
|
||||||
|
<configuration default="true" type="GradleAppRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" PASS_PARENT_ENVS_2="true">
|
||||||
|
<method v="2">
|
||||||
|
<option name="com.jetbrains.cidr.cpp.gradle.execution.GradleNativeBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
|
||||||
|
</method>
|
||||||
|
</configuration>
|
||||||
|
</component>
|
||||||
|
<component name="SpellCheckerSettings" RuntimeDictionaries="0" Folders="0" CustomDictionaries="0" DefaultDictionary="application-level" UseSingleDictionary="true" transferred="true" />
|
||||||
|
<component name="WindowStateProjectService">
|
||||||
|
<state x="440" y="187" key="#New_File_Extensions" timestamp="1647623370330">
|
||||||
|
<screen x="0" y="0" width="1680" height="1050" />
|
||||||
|
</state>
|
||||||
|
<state x="440" y="187" key="#New_File_Extensions/0.0.1680.1050@0.0.1680.1050" timestamp="1647623370330" />
|
||||||
|
<state x="995" y="526" key="#com.intellij.fileTypes.FileTypeChooser" timestamp="1646566673308">
|
||||||
|
<screen x="0" y="0" width="2560" height="1440" />
|
||||||
|
</state>
|
||||||
|
<state x="995" y="526" key="#com.intellij.fileTypes.FileTypeChooser/2560.25.2560.1415/-1680.415.1680.1025/0.0.2560.1440@0.0.2560.1440" timestamp="1646566673308" />
|
||||||
|
<state width="2518" height="314" key="GridCell.Tab.0.bottom" timestamp="1646668312027">
|
||||||
|
<screen x="0" y="0" width="2560" height="1440" />
|
||||||
|
</state>
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.bottom/0.0.1680.1050@0.0.1680.1050" timestamp="1646595844143" />
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.bottom/0.25.1680.1025@0.25.1680.1025" timestamp="1646594478732" />
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.bottom/2560.25.2560.1415/-1680.390.1680.1050/0.25.2560.1415@-1680.390.1680.1050" timestamp="1646576156561" />
|
||||||
|
<state width="2518" height="314" key="GridCell.Tab.0.bottom/2560.25.2560.1415/-1680.415.1680.1025/0.0.2560.1440@0.0.2560.1440" timestamp="1646668312027" />
|
||||||
|
<state width="2518" height="314" key="GridCell.Tab.0.center" timestamp="1646668312026">
|
||||||
|
<screen x="0" y="0" width="2560" height="1440" />
|
||||||
|
</state>
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.center/0.0.1680.1050@0.0.1680.1050" timestamp="1646595844142" />
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.center/0.25.1680.1025@0.25.1680.1025" timestamp="1646594478731" />
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.center/2560.25.2560.1415/-1680.390.1680.1050/0.25.2560.1415@-1680.390.1680.1050" timestamp="1646576156560" />
|
||||||
|
<state width="2518" height="314" key="GridCell.Tab.0.center/2560.25.2560.1415/-1680.415.1680.1025/0.0.2560.1440@0.0.2560.1440" timestamp="1646668312026" />
|
||||||
|
<state width="2518" height="314" key="GridCell.Tab.0.left" timestamp="1646668312026">
|
||||||
|
<screen x="0" y="0" width="2560" height="1440" />
|
||||||
|
</state>
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.left/0.0.1680.1050@0.0.1680.1050" timestamp="1646595844141" />
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.left/0.25.1680.1025@0.25.1680.1025" timestamp="1646594478730" />
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.left/2560.25.2560.1415/-1680.390.1680.1050/0.25.2560.1415@-1680.390.1680.1050" timestamp="1646576156559" />
|
||||||
|
<state width="2518" height="314" key="GridCell.Tab.0.left/2560.25.2560.1415/-1680.415.1680.1025/0.0.2560.1440@0.0.2560.1440" timestamp="1646668312026" />
|
||||||
|
<state width="2518" height="314" key="GridCell.Tab.0.right" timestamp="1646668312027">
|
||||||
|
<screen x="0" y="0" width="2560" height="1440" />
|
||||||
|
</state>
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.right/0.0.1680.1050@0.0.1680.1050" timestamp="1646595844143" />
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.right/0.25.1680.1025@0.25.1680.1025" timestamp="1646594478731" />
|
||||||
|
<state width="1638" height="314" key="GridCell.Tab.0.right/2560.25.2560.1415/-1680.390.1680.1050/0.25.2560.1415@-1680.390.1680.1050" timestamp="1646576156560" />
|
||||||
|
<state width="2518" height="314" key="GridCell.Tab.0.right/2560.25.2560.1415/-1680.415.1680.1025/0.0.2560.1440@0.0.2560.1440" timestamp="1646668312027" />
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.bottom" timestamp="1646581971712">
|
||||||
|
<screen x="0" y="0" width="1680" height="1050" />
|
||||||
|
</state>
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.bottom/0.0.1680.1050@0.0.1680.1050" timestamp="1646581971712" />
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.bottom/2560.25.2560.1415/-1680.390.1680.1050/0.25.2560.1415@-1680.390.1680.1050" timestamp="1646574297963" />
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.center" timestamp="1646581971711">
|
||||||
|
<screen x="0" y="0" width="1680" height="1050" />
|
||||||
|
</state>
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.center/0.0.1680.1050@0.0.1680.1050" timestamp="1646581971711" />
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.center/2560.25.2560.1415/-1680.390.1680.1050/0.25.2560.1415@-1680.390.1680.1050" timestamp="1646574297962" />
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.left" timestamp="1646581971711">
|
||||||
|
<screen x="0" y="0" width="1680" height="1050" />
|
||||||
|
</state>
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.left/0.0.1680.1050@0.0.1680.1050" timestamp="1646581971711" />
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.left/2560.25.2560.1415/-1680.390.1680.1050/0.25.2560.1415@-1680.390.1680.1050" timestamp="1646574297962" />
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.right" timestamp="1646581971711">
|
||||||
|
<screen x="0" y="0" width="1680" height="1050" />
|
||||||
|
</state>
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.right/0.0.1680.1050@0.0.1680.1050" timestamp="1646581971711" />
|
||||||
|
<state width="1638" height="364" key="GridCell.Tab.1.right/2560.25.2560.1415/-1680.390.1680.1050/0.25.2560.1415@-1680.390.1680.1050" timestamp="1646574297963" />
|
||||||
|
<state x="349" y="161" key="SettingsEditor" timestamp="1646594428197">
|
||||||
|
<screen x="0" y="0" width="1680" height="1050" />
|
||||||
|
</state>
|
||||||
|
<state x="349" y="161" key="SettingsEditor/0.0.1680.1050@0.0.1680.1050" timestamp="1646594428197" />
|
||||||
|
<state width="600" height="428" key="javadoc.popup" timestamp="1646570231808">
|
||||||
|
<screen x="0" y="0" width="2560" height="1440" />
|
||||||
|
</state>
|
||||||
|
<state width="600" height="428" key="javadoc.popup/2560.25.2560.1415/-1680.415.1680.1025/0.0.2560.1440@0.0.2560.1440" timestamp="1646570231808" />
|
||||||
|
<state x="620" y="241" width="670" height="493" key="search.everywhere.popup" timestamp="1646595572573">
|
||||||
|
<screen x="0" y="0" width="1680" height="1050" />
|
||||||
|
</state>
|
||||||
|
<state x="620" y="241" width="670" height="493" key="search.everywhere.popup/0.0.1680.1050@0.0.1680.1050" timestamp="1646595572573" />
|
||||||
|
<state x="945" y="331" width="670" height="676" key="search.everywhere.popup/2560.25.2560.1415/-1680.415.1680.1025/0.0.2560.1440@0.0.2560.1440" timestamp="1646568792293" />
|
||||||
|
</component>
|
||||||
|
</project>
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
2
server_v2/.gitignore
vendored
2
server_v2/.gitignore
vendored
|
|
@ -103,4 +103,6 @@ Temporary Items
|
||||||
|
|
||||||
# End of https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode
|
# End of https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode
|
||||||
|
|
||||||
|
|
||||||
|
cmake-build*
|
||||||
build
|
build
|
||||||
|
|
|
||||||
18
server_v2/.idea/deployment.xml
generated
Normal file
18
server_v2/.idea/deployment.xml
generated
Normal file
|
|
@ -0,0 +1,18 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<project version="4">
|
||||||
|
<component name="PublishConfigData" remoteFilesAllowedToDisappearOnAutoupload="false">
|
||||||
|
<serverData>
|
||||||
|
<paths name="Judge01 (9c7ab821-1501-483e-beda-814d54656ca2)">
|
||||||
|
<serverdata>
|
||||||
|
<mappings>
|
||||||
|
<mapping deploy="/tmp/tmp.vU8xaArBs9" local="$PROJECT_DIR$" />
|
||||||
|
</mappings>
|
||||||
|
<excludedPaths>
|
||||||
|
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugremote" />
|
||||||
|
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debug" />
|
||||||
|
</excludedPaths>
|
||||||
|
</serverdata>
|
||||||
|
</paths>
|
||||||
|
</serverData>
|
||||||
|
</component>
|
||||||
|
</project>
|
||||||
10
server_v2/.idea/other.xml
generated
Normal file
10
server_v2/.idea/other.xml
generated
Normal file
|
|
@ -0,0 +1,10 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<project version="4">
|
||||||
|
<component name="AutoUploadManager">
|
||||||
|
<option name="hosts">
|
||||||
|
<list>
|
||||||
|
<option value="9c7ab821-1501-483e-beda-814d54656ca2" />
|
||||||
|
</list>
|
||||||
|
</option>
|
||||||
|
</component>
|
||||||
|
</project>
|
||||||
|
|
@ -25,13 +25,28 @@ set(PROTO_FILES
|
||||||
set(SRC_FILES
|
set(SRC_FILES
|
||||||
src/main.cpp
|
src/main.cpp
|
||||||
src/spi.cpp
|
src/spi.cpp
|
||||||
)
|
src/IRSensors.cpp
|
||||||
|
src/Cache.cpp
|
||||||
|
src/Motors.cpp
|
||||||
|
src/Encoders.cpp
|
||||||
|
src/ClosedLoopMotorController.cpp
|
||||||
|
src/PID.cpp
|
||||||
|
src/OdometryController.cpp
|
||||||
|
src/Odometry.cpp)
|
||||||
|
|
||||||
set(HDR_FILES
|
set(HDR_FILES
|
||||||
include/spi.hpp
|
include/spi.hpp
|
||||||
include/reset.hpp
|
include/reset.hpp
|
||||||
include/mathUtils.hpp
|
include/mathUtils.hpp
|
||||||
)
|
include/IRSensors.hpp
|
||||||
|
include/Robot.hpp
|
||||||
|
include/Cache.hpp
|
||||||
|
include/Motors.hpp
|
||||||
|
include/Encoders.hpp
|
||||||
|
include/ClosedLoopMotorController.hpp
|
||||||
|
include/PID.hpp
|
||||||
|
include/OdometryController.hpp
|
||||||
|
include/Odometry.hpp)
|
||||||
|
|
||||||
include_directories(third_party/asio)
|
include_directories(third_party/asio)
|
||||||
|
|
||||||
|
|
|
||||||
23
server_v2/include/Cache.hpp
Normal file
23
server_v2/include/Cache.hpp
Normal file
|
|
@ -0,0 +1,23 @@
|
||||||
|
#ifndef COMPLIB_SERVER_CACHE_HPP
|
||||||
|
#define COMPLIB_SERVER_CACHE_HPP
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
|
||||||
|
class Cache {
|
||||||
|
public:
|
||||||
|
explicit Cache(int rate_hz);
|
||||||
|
|
||||||
|
bool is_expired();
|
||||||
|
|
||||||
|
void update();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
typedef std::chrono::steady_clock clock;
|
||||||
|
|
||||||
|
std::chrono::microseconds cache_duration{};
|
||||||
|
std::chrono::time_point<std::chrono::steady_clock> last_update{};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif //COMPLIB_SERVER_CACHE_HPP
|
||||||
46
server_v2/include/ClosedLoopMotorController.hpp
Normal file
46
server_v2/include/ClosedLoopMotorController.hpp
Normal file
|
|
@ -0,0 +1,46 @@
|
||||||
|
#ifndef COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
|
||||||
|
#define COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
|
||||||
|
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <array>
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
|
#include "include/PID.hpp"
|
||||||
|
#include "include/Robot.hpp"
|
||||||
|
|
||||||
|
class ClosedLoopMotorController {
|
||||||
|
public:
|
||||||
|
static ClosedLoopMotorController &getInstance() {
|
||||||
|
static ClosedLoopMotorController instance;
|
||||||
|
return instance;
|
||||||
|
}
|
||||||
|
|
||||||
|
ClosedLoopMotorController(ClosedLoopMotorController const &) = delete;
|
||||||
|
|
||||||
|
void operator=(ClosedLoopMotorController const &) = delete;
|
||||||
|
|
||||||
|
void set_power(uint8_t port, double power);
|
||||||
|
|
||||||
|
void set_speed(uint8_t port, double speed_rpm);
|
||||||
|
|
||||||
|
private:
|
||||||
|
enum ControlMode : uint8_t {
|
||||||
|
NONE = 0,
|
||||||
|
POWER = 1,
|
||||||
|
SPEED = 2
|
||||||
|
};
|
||||||
|
|
||||||
|
ClosedLoopMotorController();
|
||||||
|
|
||||||
|
[[noreturn]] void speed_control_loop();
|
||||||
|
|
||||||
|
std::array<PID, ROBOT_MOTOR_COUNT> pids;
|
||||||
|
std::array<ControlMode, ROBOT_MOTOR_COUNT> control_modes{ControlMode::NONE};
|
||||||
|
std::array<double, ROBOT_MOTOR_COUNT> speed_targets{0};
|
||||||
|
|
||||||
|
std::thread speed_control_thread;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif //COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
|
||||||
35
server_v2/include/Encoders.hpp
Normal file
35
server_v2/include/Encoders.hpp
Normal file
|
|
@ -0,0 +1,35 @@
|
||||||
|
#ifndef COMPLIB_SERVER_ENCODERS_HPP
|
||||||
|
#define COMPLIB_SERVER_ENCODERS_HPP
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <cstdint>
|
||||||
|
#include "include/Cache.hpp"
|
||||||
|
#include "include/Robot.hpp"
|
||||||
|
|
||||||
|
class Encoders {
|
||||||
|
public:
|
||||||
|
static Encoders &getInstance() {
|
||||||
|
static Encoders instance;
|
||||||
|
return instance;
|
||||||
|
}
|
||||||
|
|
||||||
|
Encoders(Encoders const &) = delete;
|
||||||
|
|
||||||
|
void operator=(Encoders const &) = delete;
|
||||||
|
|
||||||
|
std::array<int32_t, ROBOT_MOTOR_COUNT> get_positions();
|
||||||
|
|
||||||
|
std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rpm();
|
||||||
|
|
||||||
|
private:
|
||||||
|
Encoders() = default;
|
||||||
|
|
||||||
|
Cache position_cache{ROBOT_ENCODER_RATE_HZ};
|
||||||
|
Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
|
||||||
|
|
||||||
|
std::array<int32_t, ROBOT_MOTOR_COUNT> cached_positions = {0};
|
||||||
|
std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rpm = {0};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif //COMPLIB_SERVER_ENCODERS_HPP
|
||||||
37
server_v2/include/IRSensors.hpp
Normal file
37
server_v2/include/IRSensors.hpp
Normal file
|
|
@ -0,0 +1,37 @@
|
||||||
|
#ifndef COMPLIB_SERVER_IRSENSORS_HPP
|
||||||
|
#define COMPLIB_SERVER_IRSENSORS_HPP
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
#include "include/Cache.hpp"
|
||||||
|
#include "include/Robot.hpp"
|
||||||
|
|
||||||
|
class IRSensors {
|
||||||
|
public:
|
||||||
|
static IRSensors &getInstance() {
|
||||||
|
static IRSensors instance;
|
||||||
|
return instance;
|
||||||
|
}
|
||||||
|
|
||||||
|
IRSensors(IRSensors const &) = delete;
|
||||||
|
|
||||||
|
void operator=(IRSensors const &) = delete;
|
||||||
|
|
||||||
|
static void enable();
|
||||||
|
|
||||||
|
static void disable();
|
||||||
|
|
||||||
|
std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> read();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
IRSensors() = default;
|
||||||
|
|
||||||
|
Cache cache{ROBOT_IR_RATE_HZ};
|
||||||
|
|
||||||
|
std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> cached_values = {0};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif //COMPLIB_SERVER_IRSENSORS_HPP
|
||||||
26
server_v2/include/Motors.hpp
Normal file
26
server_v2/include/Motors.hpp
Normal file
|
|
@ -0,0 +1,26 @@
|
||||||
|
#ifndef COMPLIB_SERVER_MOTORS_HPP
|
||||||
|
#define COMPLIB_SERVER_MOTORS_HPP
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
class Motors {
|
||||||
|
public:
|
||||||
|
enum Mode : uint8_t {
|
||||||
|
COAST = 0,
|
||||||
|
FORWARD = 1,
|
||||||
|
BACKWARD = 2,
|
||||||
|
BREAK = 3,
|
||||||
|
SERVO = 4,
|
||||||
|
NONE = 5
|
||||||
|
};
|
||||||
|
|
||||||
|
static void set_power(uint8_t port, double percent);
|
||||||
|
|
||||||
|
static void set_pwm(uint8_t port, uint16_t pwm, Mode mode);
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
Motors() = default;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif //COMPLIB_SERVER_MOTORS_HPP
|
||||||
24
server_v2/include/Odometry.hpp
Normal file
24
server_v2/include/Odometry.hpp
Normal file
|
|
@ -0,0 +1,24 @@
|
||||||
|
#ifndef COMPLIB_SERVER_ODOMETRY_HPP
|
||||||
|
#define COMPLIB_SERVER_ODOMETRY_HPP
|
||||||
|
|
||||||
|
class Odometry {
|
||||||
|
|
||||||
|
public:
|
||||||
|
Odometry();
|
||||||
|
|
||||||
|
Odometry(double x_position, double y_position, double angular_orientation);
|
||||||
|
|
||||||
|
double get_x_position() const;
|
||||||
|
|
||||||
|
double get_y_position() const;
|
||||||
|
|
||||||
|
double get_angular_orientation() const;
|
||||||
|
|
||||||
|
|
||||||
|
private:
|
||||||
|
double x_position;
|
||||||
|
double y_position;
|
||||||
|
double angular_orientation;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif //COMPLIB_SERVER_ODOMETRY_HPP
|
||||||
46
server_v2/include/OdometryController.hpp
Normal file
46
server_v2/include/OdometryController.hpp
Normal file
|
|
@ -0,0 +1,46 @@
|
||||||
|
#ifndef COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
|
||||||
|
#define COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
|
||||||
|
|
||||||
|
|
||||||
|
#include <thread>
|
||||||
|
#include <chrono>
|
||||||
|
|
||||||
|
#include "include/Odometry.hpp"
|
||||||
|
|
||||||
|
class OdometryController {
|
||||||
|
public:
|
||||||
|
static OdometryController &getInstance() {
|
||||||
|
static OdometryController instance;
|
||||||
|
return instance;
|
||||||
|
}
|
||||||
|
|
||||||
|
OdometryController(OdometryController const &) = delete;
|
||||||
|
|
||||||
|
void operator=(OdometryController const &) = delete;
|
||||||
|
|
||||||
|
void enable();
|
||||||
|
|
||||||
|
void disable();
|
||||||
|
|
||||||
|
void reset();
|
||||||
|
|
||||||
|
Odometry get();
|
||||||
|
|
||||||
|
private:
|
||||||
|
typedef std::chrono::steady_clock clock;
|
||||||
|
|
||||||
|
OdometryController();
|
||||||
|
|
||||||
|
std::thread odometry_thread;
|
||||||
|
|
||||||
|
[[noreturn]] void odometry_loop();
|
||||||
|
|
||||||
|
Odometry current_odometry{};
|
||||||
|
bool enabled = false;
|
||||||
|
double last_position_left{0};
|
||||||
|
double last_position_right{0};
|
||||||
|
std::chrono::time_point<clock> last_run;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif //COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
|
||||||
32
server_v2/include/PID.hpp
Normal file
32
server_v2/include/PID.hpp
Normal file
|
|
@ -0,0 +1,32 @@
|
||||||
|
#ifndef COMPLIB_SERVER_PID_HPP
|
||||||
|
#define COMPLIB_SERVER_PID_HPP
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
|
||||||
|
class PID {
|
||||||
|
public:
|
||||||
|
PID();
|
||||||
|
|
||||||
|
PID(double P, double I, double D, double ramp, double limit);
|
||||||
|
|
||||||
|
~PID() = default;
|
||||||
|
|
||||||
|
double operator()(double setpoint, double process_variable);
|
||||||
|
|
||||||
|
double P = 1;
|
||||||
|
double I = 0;
|
||||||
|
double D = 0;
|
||||||
|
double setpoint_ramp = 0;
|
||||||
|
double limit = 0;
|
||||||
|
|
||||||
|
private:
|
||||||
|
typedef std::chrono::steady_clock clock;
|
||||||
|
#
|
||||||
|
|
||||||
|
double error_prev = 0;
|
||||||
|
double setpoint_prev = 0;
|
||||||
|
double integral_prev = 0;
|
||||||
|
clock::time_point timestamp_prev = clock::now();
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // COMPLIB_SERVER_PID_HPP
|
||||||
31
server_v2/include/Robot.hpp
Normal file
31
server_v2/include/Robot.hpp
Normal file
|
|
@ -0,0 +1,31 @@
|
||||||
|
#ifndef COMPLIB_SERVER_ROBOT_HPP
|
||||||
|
#define COMPLIB_SERVER_ROBOT_HPP
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
#define ROBOT_IR_SENSOR_COUNT 5
|
||||||
|
#define ROBOT_IR_RATE_HZ 250
|
||||||
|
|
||||||
|
#define ROBOT_MOTOR_COUNT 4
|
||||||
|
#define ROBOT_MOTOR_SPEED_CONTROL_KP 0.5
|
||||||
|
#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0
|
||||||
|
#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.025
|
||||||
|
#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 100.0
|
||||||
|
#define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
|
||||||
|
|
||||||
|
#define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250
|
||||||
|
#define ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT 3
|
||||||
|
#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT 0
|
||||||
|
#define ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT 1.0
|
||||||
|
#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0
|
||||||
|
|
||||||
|
#define ROBOT_ENCODER_RATE_HZ 250
|
||||||
|
|
||||||
|
#define ROBOT_WHEEL_CIRCUMFERENCE_MM (71.0 * M_PI)
|
||||||
|
#define ROBOT_TICKS_PER_TURN (27.7 * 100.0)
|
||||||
|
#define ROBOT_ARBOR_LENGTH_MM 139.0
|
||||||
|
#define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0)
|
||||||
|
#define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN)
|
||||||
|
|
||||||
|
|
||||||
|
#endif //COMPLIB_SERVER_ROBOT_HPP
|
||||||
|
|
@ -4,24 +4,36 @@
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
|
|
||||||
namespace mathUtils {
|
namespace mathUtils {
|
||||||
inline int int_from_bytes(uint8_t *data, int length) {
|
inline int int_from_bytes(uint8_t *data, int length) {
|
||||||
int ret = 0;
|
int ret = 0;
|
||||||
|
|
||||||
int i = 0;
|
int i = 0;
|
||||||
for (int j = length -1; j >= 0; j--) {
|
for (int j = length - 1; j >= 0; j--) {
|
||||||
ret = ret | (data[i] << (j * 8));
|
ret = ret | (data[i] << (j * 8));
|
||||||
i++;
|
i++;
|
||||||
}
|
|
||||||
return ret;
|
|
||||||
}
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
inline void bytes_from_int(int data, int length, uint8_t *result) {
|
template<class T>
|
||||||
int i = 0;
|
inline T from_bytes(const uint8_t *data, int length) {
|
||||||
for (int j = length -1; j >= 0; j--) {
|
T ret = 0;
|
||||||
result[i] = ((data >> (j * 8)) & 0xffu);
|
|
||||||
i++;
|
int i = 0;
|
||||||
}
|
for (int j = length - 1; j >= 0; j--) {
|
||||||
|
ret = ret | (data[i] << (j * 8));
|
||||||
|
i++;
|
||||||
}
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void bytes_from_int(int data, int length, uint8_t *result) {
|
||||||
|
int i = 0;
|
||||||
|
for (int j = length - 1; j >= 0; j--) {
|
||||||
|
result[i] = ((data >> (j * 8)) & 0xffu);
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // COMPLIB_SERVER_MATHUTILS_HPP
|
#endif // COMPLIB_SERVER_MATHUTILS_HPP
|
||||||
|
|
@ -14,126 +14,129 @@
|
||||||
|
|
||||||
class Spi {
|
class Spi {
|
||||||
public:
|
public:
|
||||||
static Spi& getInstance()
|
static Spi &getInstance() {
|
||||||
{
|
static Spi instance;
|
||||||
static Spi instance;
|
return instance;
|
||||||
return instance;
|
}
|
||||||
}
|
|
||||||
|
|
||||||
Spi(Spi const&) = delete;
|
Spi(Spi const &) = delete;
|
||||||
void operator=(Spi const&) = delete;
|
|
||||||
|
|
||||||
int read(uint8_t reg, uint8_t length);
|
void operator=(Spi const &) = delete;
|
||||||
void read_array(uint8_t reg, uint8_t length, uint8_t* data);
|
|
||||||
|
|
||||||
void write(uint8_t reg, uint8_t length, int value);
|
|
||||||
void write_array(uint8_t reg, uint8_t length, const uint8_t* data);
|
|
||||||
|
|
||||||
enum Register : uint8_t {
|
|
||||||
IDENTIFICATION_MODEL_ID = 1,
|
|
||||||
IDENTIFICATION_MODEL_REV_MAJOR = 2,
|
|
||||||
IDENTIFICATION_MODEL_REV_MINOR = 3,
|
|
||||||
IDENTIFICATION_MODEL_REV_PATCH = 4,
|
|
||||||
|
|
||||||
// Motor encoder positions
|
int read(uint8_t reg, uint8_t length);
|
||||||
MOTOR_1_POS_B3 = 10,
|
|
||||||
MOTOR_1_POS_B2 = 11,
|
|
||||||
MOTOR_1_POS_B1 = 12,
|
|
||||||
MOTOR_1_POS_B0 = 13,
|
|
||||||
MOTOR_2_POS_B3 = 14,
|
|
||||||
MOTOR_2_POS_B2 = 15,
|
|
||||||
MOTOR_2_POS_B1 = 16,
|
|
||||||
MOTOR_2_POS_B0 = 17,
|
|
||||||
MOTOR_3_POS_B3 = 18,
|
|
||||||
MOTOR_3_POS_B2 = 19,
|
|
||||||
MOTOR_3_POS_B1 = 20,
|
|
||||||
MOTOR_3_POS_B0 = 21,
|
|
||||||
MOTOR_4_POS_B3 = 22,
|
|
||||||
MOTOR_4_POS_B2 = 23,
|
|
||||||
MOTOR_4_POS_B1 = 24,
|
|
||||||
MOTOR_4_POS_B0 = 25,
|
|
||||||
|
|
||||||
// PWM Control Modes
|
void read_array(uint8_t reg, uint8_t length, uint8_t *data);
|
||||||
PWM_1_CTRL = 26,
|
|
||||||
PWM_2_CTRL = 27,
|
|
||||||
PWM_3_CTRL = 28,
|
|
||||||
PWM_4_CTRL = 29,
|
|
||||||
|
|
||||||
// Motor pwm speed
|
void write(uint8_t reg, uint8_t length, int value);
|
||||||
MOTOR_1_PWM_H = 30,
|
|
||||||
MOTOR_1_PWM_L = 31,
|
|
||||||
MOTOR_2_PWM_H = 32,
|
|
||||||
MOTOR_2_PWM_L = 33,
|
|
||||||
MOTOR_3_PWM_H = 34,
|
|
||||||
MOTOR_3_PWM_L = 35,
|
|
||||||
MOTOR_4_PWM_H = 36,
|
|
||||||
MOTOR_4_PWM_L = 37,
|
|
||||||
|
|
||||||
// Servo goal position
|
void write_array(uint8_t reg, uint8_t length, const uint8_t *data);
|
||||||
SERVO_1_PWM_H = 38,
|
|
||||||
SERVO_1_PWM_L = 39,
|
|
||||||
SERVO_2_PWM_H = 40,
|
|
||||||
SERVO_2_PWM_L = 41,
|
|
||||||
SERVO_3_PWM_H = 42,
|
|
||||||
SERVO_3_PWM_L = 43,
|
|
||||||
SERVO_4_PWM_H = 44,
|
|
||||||
SERVO_4_PWM_L = 45,
|
|
||||||
SERVO_5_PWM_H = 46,
|
|
||||||
SERVO_5_PWM_L = 47,
|
|
||||||
SERVO_6_PWM_H = 48,
|
|
||||||
SERVO_6_PWM_L = 49,
|
|
||||||
SERVO_7_PWM_H = 50,
|
|
||||||
SERVO_7_PWM_L = 51,
|
|
||||||
SERVO_8_PWM_H = 52,
|
|
||||||
SERVO_8_PWM_L = 53,
|
|
||||||
|
|
||||||
// IR Sensor value
|
enum Register : uint8_t {
|
||||||
IR_1_H = 54,
|
IDENTIFICATION_MODEL_ID = 1,
|
||||||
IR_1_L = 55,
|
IDENTIFICATION_MODEL_REV_MAJOR = 2,
|
||||||
IR_2_H = 56,
|
IDENTIFICATION_MODEL_REV_MINOR = 3,
|
||||||
IR_2_L = 57,
|
IDENTIFICATION_MODEL_REV_PATCH = 4,
|
||||||
IR_3_H = 58,
|
|
||||||
IR_3_L = 59,
|
|
||||||
IR_4_H = 60,
|
|
||||||
IR_4_L = 61,
|
|
||||||
IR_5_H = 62,
|
|
||||||
IR_5_L = 63,
|
|
||||||
IR_1_LED = 64,
|
|
||||||
IR_2_LED = 65,
|
|
||||||
IR_3_LED = 66,
|
|
||||||
IR_4_LED = 67,
|
|
||||||
IR_5_LED = 68,
|
|
||||||
|
|
||||||
// Display registers
|
// Motor encoder positions
|
||||||
DISPLAY_LINE_1_C0 = 69,
|
MOTOR_1_POS_B3 = 10,
|
||||||
DISPLAY_LINE_2_C0 = 85,
|
MOTOR_1_POS_B2 = 11,
|
||||||
DISPLAY_LINE_3_C0 = 101,
|
MOTOR_1_POS_B1 = 12,
|
||||||
DISPLAY_LINE_4_C0 = 117,
|
MOTOR_1_POS_B0 = 13,
|
||||||
|
MOTOR_2_POS_B3 = 14,
|
||||||
|
MOTOR_2_POS_B2 = 15,
|
||||||
|
MOTOR_2_POS_B1 = 16,
|
||||||
|
MOTOR_2_POS_B0 = 17,
|
||||||
|
MOTOR_3_POS_B3 = 18,
|
||||||
|
MOTOR_3_POS_B2 = 19,
|
||||||
|
MOTOR_3_POS_B1 = 20,
|
||||||
|
MOTOR_3_POS_B0 = 21,
|
||||||
|
MOTOR_4_POS_B3 = 22,
|
||||||
|
MOTOR_4_POS_B2 = 23,
|
||||||
|
MOTOR_4_POS_B1 = 24,
|
||||||
|
MOTOR_4_POS_B0 = 25,
|
||||||
|
|
||||||
// Motor encoder velocities
|
// PWM Control Modes
|
||||||
MOTOR_1_VEL_H = 118,
|
PWM_1_CTRL = 26,
|
||||||
MOTOR_1_VEL_L = 119,
|
PWM_2_CTRL = 27,
|
||||||
MOTOR_2_VEL_H = 120,
|
PWM_3_CTRL = 28,
|
||||||
MOTOR_2_VEL_L = 121,
|
PWM_4_CTRL = 29,
|
||||||
MOTOR_3_VEL_H = 122,
|
|
||||||
MOTOR_3_VEL_L = 123,
|
// Motor pwm speed
|
||||||
MOTOR_4_VEL_H = 124,
|
MOTOR_1_PWM_H = 30,
|
||||||
MOTOR_4_VEL_L = 125
|
MOTOR_1_PWM_L = 31,
|
||||||
};
|
MOTOR_2_PWM_H = 32,
|
||||||
|
MOTOR_2_PWM_L = 33,
|
||||||
|
MOTOR_3_PWM_H = 34,
|
||||||
|
MOTOR_3_PWM_L = 35,
|
||||||
|
MOTOR_4_PWM_H = 36,
|
||||||
|
MOTOR_4_PWM_L = 37,
|
||||||
|
|
||||||
|
// Servo goal position
|
||||||
|
SERVO_1_PWM_H = 38,
|
||||||
|
SERVO_1_PWM_L = 39,
|
||||||
|
SERVO_2_PWM_H = 40,
|
||||||
|
SERVO_2_PWM_L = 41,
|
||||||
|
SERVO_3_PWM_H = 42,
|
||||||
|
SERVO_3_PWM_L = 43,
|
||||||
|
SERVO_4_PWM_H = 44,
|
||||||
|
SERVO_4_PWM_L = 45,
|
||||||
|
SERVO_5_PWM_H = 46,
|
||||||
|
SERVO_5_PWM_L = 47,
|
||||||
|
SERVO_6_PWM_H = 48,
|
||||||
|
SERVO_6_PWM_L = 49,
|
||||||
|
SERVO_7_PWM_H = 50,
|
||||||
|
SERVO_7_PWM_L = 51,
|
||||||
|
SERVO_8_PWM_H = 52,
|
||||||
|
SERVO_8_PWM_L = 53,
|
||||||
|
|
||||||
|
// IR Sensor value
|
||||||
|
IR_1_H = 54,
|
||||||
|
IR_1_L = 55,
|
||||||
|
IR_2_H = 56,
|
||||||
|
IR_2_L = 57,
|
||||||
|
IR_3_H = 58,
|
||||||
|
IR_3_L = 59,
|
||||||
|
IR_4_H = 60,
|
||||||
|
IR_4_L = 61,
|
||||||
|
IR_5_H = 62,
|
||||||
|
IR_5_L = 63,
|
||||||
|
IR_1_LED = 64,
|
||||||
|
IR_2_LED = 65,
|
||||||
|
IR_3_LED = 66,
|
||||||
|
IR_4_LED = 67,
|
||||||
|
IR_5_LED = 68,
|
||||||
|
|
||||||
|
// Display registers
|
||||||
|
DISPLAY_LINE_1_C0 = 69,
|
||||||
|
DISPLAY_LINE_2_C0 = 85,
|
||||||
|
DISPLAY_LINE_3_C0 = 101,
|
||||||
|
DISPLAY_LINE_4_C0 = 117,
|
||||||
|
|
||||||
|
// Motor encoder velocities
|
||||||
|
MOTOR_1_VEL_H = 133,
|
||||||
|
MOTOR_1_VEL_L = 134,
|
||||||
|
MOTOR_2_VEL_H = 135,
|
||||||
|
MOTOR_2_VEL_L = 136,
|
||||||
|
MOTOR_3_VEL_H = 137,
|
||||||
|
MOTOR_3_VEL_L = 138,
|
||||||
|
MOTOR_4_VEL_H = 139,
|
||||||
|
MOTOR_4_VEL_L = 140
|
||||||
|
};
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Spi();
|
Spi();
|
||||||
|
|
||||||
int spi_file_descriptor{};
|
int spi_file_descriptor{};
|
||||||
uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
|
uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
|
||||||
uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};
|
uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};
|
||||||
|
|
||||||
std::recursive_mutex spi_mutex;
|
std::recursive_mutex spi_mutex;
|
||||||
|
|
||||||
void transfer();
|
void transfer();
|
||||||
void clear_buffers();
|
|
||||||
|
|
||||||
uint8_t calculate_checksum(uint8_t* data, uint8_t length);
|
void clear_buffers();
|
||||||
|
|
||||||
|
uint8_t calculate_checksum(uint8_t *data, uint8_t length);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
16
server_v2/src/Cache.cpp
Normal file
16
server_v2/src/Cache.cpp
Normal file
|
|
@ -0,0 +1,16 @@
|
||||||
|
#include "include/Cache.hpp"
|
||||||
|
|
||||||
|
#define MS_IN_S 1000
|
||||||
|
#define US_IN_S (1000 * MS_IN_S)
|
||||||
|
|
||||||
|
Cache::Cache(int rate_hz) {
|
||||||
|
cache_duration = std::chrono::microseconds(US_IN_S / rate_hz);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Cache::is_expired() {
|
||||||
|
return ((clock::now() - last_update) > cache_duration);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Cache::update() {
|
||||||
|
last_update = clock::now();
|
||||||
|
}
|
||||||
49
server_v2/src/ClosedLoopMotorController.cpp
Normal file
49
server_v2/src/ClosedLoopMotorController.cpp
Normal file
|
|
@ -0,0 +1,49 @@
|
||||||
|
#include <algorithm>
|
||||||
|
#include "include/ClosedLoopMotorController.hpp"
|
||||||
|
#include "include/Motors.hpp"
|
||||||
|
#include "include/Encoders.hpp"
|
||||||
|
|
||||||
|
#define MS_IN_S 1000
|
||||||
|
#define US_IN_S (1000 * MS_IN_S)
|
||||||
|
|
||||||
|
void ClosedLoopMotorController::set_power(uint8_t port, double power) {
|
||||||
|
control_modes.at(port) = ControlMode::POWER;
|
||||||
|
Motors::set_power(port, power);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ClosedLoopMotorController::set_speed(uint8_t port, double speed_rpm) {
|
||||||
|
speed_targets.at(port) = speed_rpm;
|
||||||
|
if (control_modes.at(port) != ControlMode::SPEED) {
|
||||||
|
pids.at(port) = PID(
|
||||||
|
ROBOT_MOTOR_SPEED_CONTROL_KP, ROBOT_MOTOR_SPEED_CONTROL_KI, ROBOT_MOTOR_SPEED_CONTROL_KD,
|
||||||
|
ROBOT_MOTOR_SPEED_CONTROL_RAMP, 100.0);
|
||||||
|
control_modes.at(port) = ControlMode::SPEED;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ClosedLoopMotorController::ClosedLoopMotorController() {
|
||||||
|
speed_control_thread = std::thread(&ClosedLoopMotorController::speed_control_loop, this);
|
||||||
|
speed_control_thread.detach();
|
||||||
|
}
|
||||||
|
|
||||||
|
[[noreturn]] void ClosedLoopMotorController::speed_control_loop() {
|
||||||
|
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
|
||||||
|
while (true) {
|
||||||
|
std::this_thread::sleep_for(sleep_duration);
|
||||||
|
|
||||||
|
bool should_control_speed = std::find(control_modes.begin(), control_modes.end(), ControlMode::SPEED) != control_modes.end();
|
||||||
|
if (!should_control_speed) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto speeds = Encoders::getInstance().get_velocities_rpm();
|
||||||
|
for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) {
|
||||||
|
if (control_modes.at(i) == ControlMode::SPEED) {
|
||||||
|
double power = pids.at(i)(speed_targets.at(i), speeds.at(i));
|
||||||
|
Motors::set_power(i, power);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
29
server_v2/src/Encoders.cpp
Normal file
29
server_v2/src/Encoders.cpp
Normal file
|
|
@ -0,0 +1,29 @@
|
||||||
|
#include "include/Encoders.hpp"
|
||||||
|
#include "include/spi.hpp"
|
||||||
|
#include "include/mathUtils.hpp"
|
||||||
|
#include <spdlog/spdlog.h>
|
||||||
|
|
||||||
|
std::array<int32_t, ROBOT_MOTOR_COUNT> Encoders::get_positions() {
|
||||||
|
if (position_cache.is_expired()) {
|
||||||
|
uint8_t data[ROBOT_MOTOR_COUNT * 4] = {0};
|
||||||
|
Spi::getInstance().read_array(Spi::MOTOR_1_POS_B3, 4 * ROBOT_MOTOR_COUNT, data);
|
||||||
|
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
|
||||||
|
cached_positions.at(i) = mathUtils::from_bytes<int32_t>(data + i * 4, 4);
|
||||||
|
}
|
||||||
|
position_cache.update();
|
||||||
|
}
|
||||||
|
return cached_positions;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rpm() {
|
||||||
|
if (velocity_cache.is_expired()) {
|
||||||
|
uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
|
||||||
|
Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
|
||||||
|
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
|
||||||
|
auto velocity_tps = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
|
||||||
|
cached_velocities_rpm.at(i) = velocity_tps * 60.0 / ROBOT_TICKS_PER_TURN;
|
||||||
|
}
|
||||||
|
velocity_cache.update();
|
||||||
|
}
|
||||||
|
return cached_velocities_rpm;
|
||||||
|
}
|
||||||
29
server_v2/src/IRSensors.cpp
Normal file
29
server_v2/src/IRSensors.cpp
Normal file
|
|
@ -0,0 +1,29 @@
|
||||||
|
#include "include/IRSensors.hpp"
|
||||||
|
#include "include/spi.hpp"
|
||||||
|
#include "include/Robot.hpp"
|
||||||
|
#include "include/mathUtils.hpp"
|
||||||
|
|
||||||
|
void IRSensors::enable() {
|
||||||
|
uint8_t data[ROBOT_IR_SENSOR_COUNT] = {};
|
||||||
|
std::fill_n(data, ROBOT_IR_SENSOR_COUNT, 1);
|
||||||
|
Spi::getInstance().write_array(Spi::IR_1_LED, ROBOT_IR_SENSOR_COUNT, data);
|
||||||
|
}
|
||||||
|
|
||||||
|
void IRSensors::disable() {
|
||||||
|
uint8_t data[ROBOT_IR_SENSOR_COUNT] = {};
|
||||||
|
std::fill_n(data, ROBOT_IR_SENSOR_COUNT, 0);
|
||||||
|
Spi::getInstance().write_array(Spi::IR_1_LED, ROBOT_IR_SENSOR_COUNT, data);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> IRSensors::read() {
|
||||||
|
if (cache.is_expired()) {
|
||||||
|
uint8_t data[ROBOT_IR_SENSOR_COUNT * 2] = {0};
|
||||||
|
Spi::getInstance().read_array(Spi::IR_1_H, 2 * ROBOT_IR_SENSOR_COUNT, data);
|
||||||
|
for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) {
|
||||||
|
cached_values.at(i) = mathUtils::from_bytes<uint16_t>(data + i * 2, 2);
|
||||||
|
}
|
||||||
|
cache.update();
|
||||||
|
}
|
||||||
|
return cached_values;
|
||||||
|
}
|
||||||
31
server_v2/src/Motors.cpp
Normal file
31
server_v2/src/Motors.cpp
Normal file
|
|
@ -0,0 +1,31 @@
|
||||||
|
#include "include/Motors.hpp"
|
||||||
|
#include "include/spi.hpp"
|
||||||
|
#include "include/Robot.hpp"
|
||||||
|
#include <spdlog/spdlog.h>
|
||||||
|
#include <limits>
|
||||||
|
|
||||||
|
void Motors::set_power(uint8_t port, double percent) {
|
||||||
|
if (fabs(percent) > 100.0) {
|
||||||
|
spdlog::error("Invalid motor percent specified. Should be -100 <= percent <= 100", percent);
|
||||||
|
}
|
||||||
|
|
||||||
|
Mode mode = Mode::COAST;
|
||||||
|
if (percent < 0.0) {
|
||||||
|
mode = Mode::BACKWARD;
|
||||||
|
} else if (percent > 0.0) {
|
||||||
|
mode = Mode::FORWARD;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto pwm = (uint16_t) (fabs(percent) * (std::numeric_limits<uint16_t>::max() / 100.0));
|
||||||
|
set_pwm(port, pwm, mode);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Motors::set_pwm(uint8_t port, uint16_t pwm, Mode mode) {
|
||||||
|
if (port >= ROBOT_MOTOR_COUNT) {
|
||||||
|
spdlog::error("Invalid motor port {} specified", port);
|
||||||
|
}
|
||||||
|
|
||||||
|
Spi::getInstance().write(Spi::MOTOR_1_PWM_H + port * 2, 2, pwm);
|
||||||
|
Spi::getInstance().write(Spi::PWM_1_CTRL + port, 1, mode);
|
||||||
|
}
|
||||||
|
|
||||||
21
server_v2/src/Odometry.cpp
Normal file
21
server_v2/src/Odometry.cpp
Normal file
|
|
@ -0,0 +1,21 @@
|
||||||
|
#include "include/Odometry.hpp"
|
||||||
|
|
||||||
|
Odometry::Odometry() : x_position(0), y_position(0), angular_orientation(0) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
Odometry::Odometry(double x_position, double y_position, double angular_orientation) : x_position(x_position), y_position(y_position), angular_orientation(angular_orientation) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
double Odometry::get_x_position() const {
|
||||||
|
return x_position;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Odometry::get_y_position() const {
|
||||||
|
return y_position;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Odometry::get_angular_orientation() const {
|
||||||
|
return angular_orientation;
|
||||||
|
}
|
||||||
83
server_v2/src/OdometryController.cpp
Normal file
83
server_v2/src/OdometryController.cpp
Normal file
|
|
@ -0,0 +1,83 @@
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
#include "include/OdometryController.hpp"
|
||||||
|
#include "include/Robot.hpp"
|
||||||
|
#include "include/Encoders.hpp"
|
||||||
|
#include <spdlog/spdlog.h>
|
||||||
|
|
||||||
|
#define MS_IN_S 1000
|
||||||
|
#define US_IN_S (1000 * MS_IN_S)
|
||||||
|
|
||||||
|
void OdometryController::enable() {
|
||||||
|
last_run = clock::now();
|
||||||
|
enabled = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void OdometryController::disable() {
|
||||||
|
enabled = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void OdometryController::reset() {
|
||||||
|
current_odometry = Odometry();
|
||||||
|
last_position_left = 0;
|
||||||
|
last_position_right = 0;
|
||||||
|
last_run = clock::now();
|
||||||
|
}
|
||||||
|
|
||||||
|
Odometry OdometryController::get() {
|
||||||
|
return current_odometry;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
OdometryController::OdometryController() {
|
||||||
|
odometry_thread = std::thread(&OdometryController::odometry_loop, this);
|
||||||
|
odometry_thread.detach();
|
||||||
|
}
|
||||||
|
|
||||||
|
[[noreturn]] void OdometryController::odometry_loop() {
|
||||||
|
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ);
|
||||||
|
while (true) {
|
||||||
|
std::this_thread::sleep_for(sleep_duration);
|
||||||
|
|
||||||
|
if (enabled) {
|
||||||
|
last_run = clock::now();
|
||||||
|
auto encoder_positions = Encoders::getInstance().get_positions();
|
||||||
|
|
||||||
|
auto current_position_left = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
|
||||||
|
auto current_position_right = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
|
||||||
|
auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER;
|
||||||
|
auto distance_right = (current_position_right - last_position_right) / ROBOT_TICKS_PER_METER;
|
||||||
|
last_position_left = current_position_left;
|
||||||
|
last_position_right = current_position_right;
|
||||||
|
|
||||||
|
auto distance = (distance_left + distance_right) * 2.0;
|
||||||
|
auto theta = (distance_left - distance_right) / ROBOT_ARBOR_LENGTH_M;
|
||||||
|
|
||||||
|
auto new_x_position = current_odometry.get_x_position();
|
||||||
|
auto new_y_position = current_odometry.get_y_position();
|
||||||
|
auto new_angular_orientation = current_odometry.get_angular_orientation();
|
||||||
|
if (distance != 0) {
|
||||||
|
auto current_orientation = current_odometry.get_angular_orientation();
|
||||||
|
|
||||||
|
auto distance_x = cos(theta) * distance;
|
||||||
|
auto distance_y = -sin(theta) * distance;
|
||||||
|
|
||||||
|
new_x_position = current_odometry.get_x_position() +
|
||||||
|
(cos(current_orientation) * distance_x -
|
||||||
|
sin(current_orientation) * distance_y);
|
||||||
|
new_y_position = current_odometry.get_y_position() +
|
||||||
|
(sin(current_orientation) * distance_x +
|
||||||
|
cos(current_orientation) * distance_y);
|
||||||
|
}
|
||||||
|
if (theta != 0) {
|
||||||
|
new_angular_orientation = current_odometry.get_angular_orientation() + theta;
|
||||||
|
}
|
||||||
|
current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation);
|
||||||
|
|
||||||
|
// spdlog::info("{} {} {} {} {} {}",
|
||||||
|
// current_position_left, current_position_right,
|
||||||
|
// distance_left, distance_right,
|
||||||
|
// distance, theta);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
50
server_v2/src/PID.cpp
Normal file
50
server_v2/src/PID.cpp
Normal file
|
|
@ -0,0 +1,50 @@
|
||||||
|
#include "include/PID.hpp"
|
||||||
|
#include <chrono>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
#define MS_IN_S 1000.0
|
||||||
|
#define US_IN_S (1000.0 * MS_IN_S)
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
|
||||||
|
PID::PID() : P(0), I(0), D(0), setpoint_ramp(0), limit(0), error_prev(0.0), setpoint_prev(0.0), integral_prev(0.0) {};
|
||||||
|
|
||||||
|
PID::PID(double P, double I, double D, double ramp, double limit)
|
||||||
|
: P(P), I(I), D(D), setpoint_ramp(ramp), limit(limit), error_prev(0.0), setpoint_prev(0.0), integral_prev(0.0) {
|
||||||
|
timestamp_prev = clock::now();
|
||||||
|
}
|
||||||
|
|
||||||
|
double PID::operator()(double setpoint, double process_variable) {
|
||||||
|
// calculate the time from the last call
|
||||||
|
auto timestamp_now = clock::now();
|
||||||
|
double delta_seconds = chrono::duration_cast<chrono::duration<double, std::micro>>(timestamp_now - timestamp_prev).count() / US_IN_S;
|
||||||
|
|
||||||
|
if (setpoint_ramp > 0) {
|
||||||
|
double setpoint_rate = (setpoint - setpoint_prev) / delta_seconds;
|
||||||
|
if (setpoint_rate > setpoint_ramp) {
|
||||||
|
setpoint = setpoint_prev + setpoint_ramp * delta_seconds;
|
||||||
|
} else if (setpoint_rate < -setpoint_ramp) {
|
||||||
|
setpoint = setpoint_prev - setpoint_ramp * delta_seconds;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
double error = setpoint - process_variable;
|
||||||
|
|
||||||
|
double proportional = P * error;
|
||||||
|
// Tustin transform of the integral part
|
||||||
|
double integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev);
|
||||||
|
double derivative = D * ((error - error_prev) / delta_seconds);
|
||||||
|
|
||||||
|
double output = proportional + integral + derivative;
|
||||||
|
|
||||||
|
// anti-windup - limit the output variable
|
||||||
|
output = std::min(std::max(-limit, output), limit);
|
||||||
|
// spdlog::info("E{} P{} I{} D{} O{} EP{} DS{}", error, proportional, integral, derivative, output, error_prev, delta_seconds);
|
||||||
|
|
||||||
|
integral_prev = integral;
|
||||||
|
setpoint_prev = setpoint;
|
||||||
|
error_prev = error;
|
||||||
|
timestamp_prev = timestamp_now;
|
||||||
|
return output;
|
||||||
|
}
|
||||||
|
|
@ -1,12 +1,9 @@
|
||||||
#include <spdlog/spdlog.h>
|
#include <spdlog/spdlog.h>
|
||||||
#include <chrono>
|
|
||||||
|
|
||||||
#include "include/reset.hpp"
|
#include "include/reset.hpp"
|
||||||
#include "include/spi.hpp"
|
#include "include/Encoders.hpp"
|
||||||
|
#include "include/OdometryController.hpp"
|
||||||
#define SOCKET_PATH "/tmp/compLib"
|
#include "include/ClosedLoopMotorController.hpp"
|
||||||
#define BUFFER_SIZE 64
|
|
||||||
#define SAMPLES 20000.0
|
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
|
|
@ -14,15 +11,14 @@ int main() {
|
||||||
Reset::reset_robot();
|
Reset::reset_robot();
|
||||||
spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
|
spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
|
||||||
|
|
||||||
auto start = chrono::steady_clock::now();
|
ClosedLoopMotorController::getInstance().set_speed(0, -125);
|
||||||
for (int i = 0; i < SAMPLES; i++) {
|
ClosedLoopMotorController::getInstance().set_speed(3, 125);
|
||||||
Spi::getInstance().read(Spi::Register::IDENTIFICATION_MODEL_ID, 1);
|
OdometryController::getInstance().enable();
|
||||||
|
|
||||||
|
for (int i = 0; i < 10000; ++i) {
|
||||||
|
auto odom = OdometryController::getInstance().get();
|
||||||
|
spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
|
||||||
|
usleep(1000);
|
||||||
}
|
}
|
||||||
auto end = chrono::steady_clock::now();
|
|
||||||
|
|
||||||
spdlog::info("Elapsed time in milliseconds: {}ms", chrono::duration_cast<chrono::milliseconds>(end - start).count());
|
|
||||||
spdlog::info("Loop rate: {}hz", SAMPLES / chrono::duration_cast<chrono::milliseconds>(end - start).count() * 1000.0);
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
Reference in a new issue