compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Rework library without unix sockets for now

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-05-21 23:29:55 +02:00
parent e9ae1a320a
commit 0bef6035ae
30 changed files with 987 additions and 136 deletions
Download patch file Download diff file Expand all files Collapse all files

187
server/.idea/workspace.xml generated Normal file
View 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>

BIN
server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdC/CMakeCCompilerId.o Normal file
View file

Binary file not shown.

BIN
server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdCXX/CMakeCXXCompilerId.o Normal file
View file

Binary file not shown.

BIN
server/cmake-build-debug/CMakeFiles/compLib_server.dir/CompLib.pb.cc.o Normal file
View file

Binary file not shown.

BIN
server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/Spi.cpp.o Normal file
View file

Binary file not shown.

BIN
server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/main.cpp.o Normal file
View file

Binary file not shown.

2
server_v2/.gitignore vendored
View file

@ -103,4 +103,6 @@ Temporary Items
# End of https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode
cmake-build*
build

18
server_v2/.idea/deployment.xml generated Normal file
View 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
View 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>

19
server_v2/CMakeLists.txt
View file

@ -25,13 +25,28 @@ set(PROTO_FILES
set(SRC_FILES
src/main.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
include/spi.hpp
include/reset.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)

23
server_v2/include/Cache.hpp Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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

40
server_v2/include/mathUtils.hpp
View file

@ -4,24 +4,36 @@
#include <cstdint>
namespace mathUtils {
inline int int_from_bytes(uint8_t *data, int length) {
int ret = 0;
inline int int_from_bytes(uint8_t *data, int length) {
int ret = 0;
int i = 0;
for (int j = length -1; j >= 0; j--) {
ret = ret | (data[i] << (j * 8));
i++;
}
return ret;
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++;
}
template<class T>
inline T from_bytes(const uint8_t *data, int length) {
T ret = 0;
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

213
server_v2/include/spi.hpp
View file

@ -14,126 +14,129 @@
class Spi {
public:
static Spi& getInstance()
{
static Spi instance;
return instance;
}
static Spi &getInstance() {
static Spi instance;
return instance;
}
Spi(Spi const&) = delete;
void operator=(Spi const&) = delete;
Spi(Spi const &) = delete;
int read(uint8_t reg, uint8_t length);
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,
void operator=(Spi const &) = delete;
// Motor encoder positions
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,
int read(uint8_t reg, uint8_t length);
// PWM Control Modes
PWM_1_CTRL = 26,
PWM_2_CTRL = 27,
PWM_3_CTRL = 28,
PWM_4_CTRL = 29,
void read_array(uint8_t reg, uint8_t length, uint8_t *data);
// Motor pwm speed
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,
void write(uint8_t reg, uint8_t length, int value);
// 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,
void write_array(uint8_t reg, uint8_t length, const uint8_t *data);
// 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,
enum Register : uint8_t {
IDENTIFICATION_MODEL_ID = 1,
IDENTIFICATION_MODEL_REV_MAJOR = 2,
IDENTIFICATION_MODEL_REV_MINOR = 3,
IDENTIFICATION_MODEL_REV_PATCH = 4,
// Display registers
DISPLAY_LINE_1_C0 = 69,
DISPLAY_LINE_2_C0 = 85,
DISPLAY_LINE_3_C0 = 101,
DISPLAY_LINE_4_C0 = 117,
// Motor encoder positions
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,
// Motor encoder velocities
MOTOR_1_VEL_H = 118,
MOTOR_1_VEL_L = 119,
MOTOR_2_VEL_H = 120,
MOTOR_2_VEL_L = 121,
MOTOR_3_VEL_H = 122,
MOTOR_3_VEL_L = 123,
MOTOR_4_VEL_H = 124,
MOTOR_4_VEL_L = 125
};
// PWM Control Modes
PWM_1_CTRL = 26,
PWM_2_CTRL = 27,
PWM_3_CTRL = 28,
PWM_4_CTRL = 29,
// Motor pwm speed
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
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:
Spi();
Spi();
int spi_file_descriptor{};
uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};
int spi_file_descriptor{};
uint8_t tx_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 clear_buffers();
void transfer();
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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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;
}

26
server_v2/src/main.cpp
View file

@ -1,12 +1,9 @@
#include <spdlog/spdlog.h>
#include <chrono>
#include "include/reset.hpp"
#include "include/spi.hpp"
#define SOCKET_PATH "/tmp/compLib"
#define BUFFER_SIZE 64
#define SAMPLES 20000.0
#include "include/Encoders.hpp"
#include "include/OdometryController.hpp"
#include "include/ClosedLoopMotorController.hpp"
using namespace std;
@ -14,15 +11,14 @@ int main() {
Reset::reset_robot();
spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
auto start = chrono::steady_clock::now();
for (int i = 0; i < SAMPLES; i++) {
Spi::getInstance().read(Spi::Register::IDENTIFICATION_MODEL_ID, 1);
ClosedLoopMotorController::getInstance().set_speed(0, -125);
ClosedLoopMotorController::getInstance().set_speed(3, 125);
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;
}