Start on v2

server_prototype/.gitignore

@@ -0,0 +1,106 @@
+
+# Created by https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode
+# Edit at https://www.toptal.com/developers/gitignore?templates=c++,macos,linux,visualstudiocode
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+
+### VisualStudioCode Patch ###
+# Ignore all local history of files
+.history
+.ionide
+
+# Support for Project snippet scope
+
+# End of https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode
+
+build

server_prototype/.idea/.gitignore

@@ -0,0 +1,6 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml

server_prototype/.idea/.name

@@ -0,0 +1 @@
+compLib_server

server_prototype/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
+</project>

server_prototype/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/server.iml" filepath="$PROJECT_DIR$/.idea/server.iml" />
+    </modules>
+  </component>
+</project>

server_prototype/.idea/saveactions_settings.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="SaveActionSettings">
+    <option name="actions">
+      <set>
+        <option value="activate" />
+        <option value="activateOnShortcut" />
+        <option value="activateOnBatch" />
+        <option value="organizeImports" />
+        <option value="reformat" />
+        <option value="rearrange" />
+      </set>
+    </option>
+  </component>
+</project>

server_prototype/.idea/serialmonitor_settings.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="SerialMonitorSettings" BaudRate="9600" LineEndingsIndex="0" ShowStatusWidget="false" />
+</project>

server_prototype/.idea/server.iml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module classpath="CMake" type="CPP_MODULE" version="4" />

server_prototype/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

server_prototype/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.16)
+
+set(PROJECT_NAME compLib_server)
+project(${PROJECT_NAME})
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH})
+
+if (NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif ()
+
+set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi")
+set(CMAKE_CXX_FLAGS_DEBUG "-g")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3")
+
+find_package(Protobuf REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(pigpio REQUIRED)
+
+set(PROTO_FILES
+        protos/CompLib.proto
+        )
+
+set(SRC_FILES
+        src/main.cpp
+        src/spi.cpp
+        src/encoder.cpp
+        src/motor.cpp
+        src/pid.cpp
+        )
+
+set(HDR_FILES
+        include/messageBuilder.hpp
+        include/errorMessages.hpp
+        include/spi.hpp
+        include/reset.hpp
+        include/mathUtils.hpp
+        include/encoder.hpp
+        include/robot.hpp
+        include/motor.hpp
+        include/pid.hpp
+        )
+
+include_directories(third_party/asio)
+
+PROTOBUF_GENERATE_CPP(PROTO_SRC PROTO_HDR ${PROTO_FILES})
+add_executable(${PROJECT_NAME} ${SRC_FILES} ${HDR_FILES} ${PROTO_SRC} ${PROTO_HDR} ${PROTO_FILES})
+target_include_directories(${PROJECT_NAME}
+        PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+        PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+        PUBLIC ${PROTOBUF_INCLUDE_DIRS}
+        )
+target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES} pigpio spdlog::spdlog)

server_prototype/cmake/modules/Findpigpio.cmake

@@ -0,0 +1,31 @@
+################################################################################
+### Find the pigpio shared libraries.
+################################################################################
+
+# Find the path to the pigpio includes.
+find_path(pigpio_INCLUDE_DIR 
+	NAMES pigpio.h pigpiod_if.h pigpiod_if2.h
+	HINTS /usr/local/include)
+	
+# Find the pigpio libraries.
+find_library(pigpio_LIBRARY 
+	NAMES libpigpio.so
+	HINTS /usr/local/lib)
+find_library(pigpiod_if_LIBRARY 
+	NAMES libpigpiod_if.so
+	HINTS /usr/local/lib)
+find_library(pigpiod_if2_LIBRARY 
+	NAMES libpigpiod_if2.so
+	HINTS /usr/local/lib)
+    
+# Set the pigpio variables to plural form to make them accessible for 
+# the paramount cmake modules.
+set(pigpio_INCLUDE_DIRS ${pigpio_INCLUDE_DIR})
+set(pigpio_INCLUDES     ${pigpio_INCLUDE_DIR})
+
+# Handle REQUIRED, QUIET, and version arguments 
+# and set the <packagename>_FOUND variable.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(pigpio 
+    DEFAULT_MSG 
+    pigpio_INCLUDE_DIR pigpio_LIBRARY pigpiod_if_LIBRARY pigpiod_if2_LIBRARY)

server_prototype/include/encoder.hpp

@@ -0,0 +1,31 @@
+#ifndef COMPLIB_SERVER_ENCODER_HPP
+#define COMPLIB_SERVER_ENCODER_HPP
+
+#include <vector>
+#include <cstdint>
+#include <chrono>
+
+#define ENCODER_COUNT 4
+#define ENCODER_CACHE_DURATION_MS 4 // 200 HZ
+
+class Encoder {
+public:
+    std::vector<int32_t> read_all();
+    std::vector<int32_t> read_all_cached();
+
+    static Encoder& get_instance()
+    {
+        static Encoder instance;
+        return instance;
+    }
+
+    Encoder(Encoder const&) = delete;
+    void operator=(Encoder const&) = delete;
+
+private:
+    Encoder();
+    int32_t encoder_cache[ENCODER_COUNT] = {0};
+    std::chrono::time_point<std::chrono::system_clock> last_read = std::chrono::system_clock::now();
+};
+
+#endif // COMPLIB_SERVER_ENCODER_HPP

server_prototype/include/errorMessages.hpp

@@ -0,0 +1,10 @@
+//
+// Created by KonstantinViesure on 06.03.22.
+//
+
+#ifndef COMPLIB_SERVER_ERRORMESSAGES_HPP
+#define COMPLIB_SERVER_ERRORMESSAGES_HPP
+
+#define ERROR_MESSAGE_UNKNOWN "Header.message_type unknown"
+
+#endif //COMPLIB_SERVER_ERRORMESSAGES_HPP

server_prototype/include/mathUtils.hpp

@@ -0,0 +1,27 @@
+#ifndef COMPLIB_SERVER_MATHUTILS_HPP
+#define COMPLIB_SERVER_MATHUTILS_HPP
+
+#include <cstdint>
+
+namespace mathUtils {
+    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;
+    }
+
+    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

server_prototype/include/messageBuilder.hpp

@@ -0,0 +1,28 @@
+#ifndef COMPLIB_SERVER_MESSAGECREATION_HPP
+#define COMPLIB_SERVER_MESSAGECREATION_HPP
+
+#include <CompLib.pb.h>
+
+namespace MessageBuilder {
+  CompLib::Header *header(const std::string &message_type) {
+    auto header = new CompLib::Header();
+    header->set_message_type(message_type);
+    return header;
+  }
+
+  CompLib::Status *status(bool successful, const std::string &error_message) {
+    auto status = new CompLib::Status();
+    status->set_successful(successful);
+    status->set_error_message(error_message);
+    return status;
+  }
+
+  CompLib::GenericResponse *genericResponse(bool successful, const std::string &error_message) {
+    auto genericResponse = new CompLib::GenericResponse();
+    genericResponse->set_allocated_header(header(CompLib::GenericResponse::descriptor()->full_name()));
+    genericResponse->set_allocated_status(status(successful, error_message));
+    return genericResponse;
+  }
+}
+
+#endif //COMPLIB_SERVER_MESSAGECREATION_HPP

server_prototype/include/motor.hpp

@@ -0,0 +1,61 @@
+#ifndef COMPLIB_SERVER_MOTOR_HPP
+#define COMPLIB_SERVER_MOTOR_HPP
+
+#include <cstdint>
+#include <vector>
+#include <chrono>
+#include <thread>
+
+#include "include/robot.hpp"
+
+class Motor {
+public:
+    enum Mode : uint8_t {
+        COAST = 0,
+        FORWARD = 1,
+        BACKWARD = 2,
+        BREAK = 3,
+        SERVO = 4,
+        NONE = 5
+    };
+
+    enum Control : uint8_t {
+        POWER = 0,
+        SPEED = 1
+    };
+
+    static Motor& get_instance()
+    {
+        static Motor instance;
+        return instance;
+    }
+
+    Motor(Motor const&) = delete;
+    void operator=(Motor const&) = delete;
+
+    void set_speed(uint8_t port, double rpm);
+    void set_power(uint8_t port, double percent);
+    static void set_pwm(uint8_t port, uint16_t pwm, Mode mode);
+
+    std::vector<double> get_speed();
+
+private:
+    Motor();
+
+    // Speed calculation and speed filter
+    int32_t last_encoder_values[MOTOR_COUNT] = {0};
+    double filtered_speeds[MOTOR_COUNT] = {0};
+    std::chrono::system_clock::time_point last_time_encoders_read;
+
+    // Speed control
+    double speed_targets[MOTOR_COUNT] = {0};
+    double motor_control_modes[MOTOR_COUNT] = {0};
+    std::thread speed_control_thread;
+
+    void reset_speed();
+    void speed_control_loop();
+
+    void _set_power(uint8_t port, double percent);
+};
+
+#endif // COMPLIB_SERVER_MOTOR_HPP

server_prototype/include/pid.hpp

@@ -0,0 +1,30 @@
+#ifndef COMPLIB_SERVER_PID_HPP
+#define COMPLIB_SERVER_PID_HPP
+
+#include <chrono>
+
+#define PID_RESET_DELAY_S 0.1L
+
+class PID
+{
+public:
+    PID(float P, float I, float D, float ramp, float limit);
+    ~PID() = default;
+
+    float operator() (float setpoint, float process_variable);
+    void reset();
+
+    float P = 1;
+    float I = 0;
+    float D = 0;
+    float setpoint_ramp = 0;
+    float limit = 0;
+
+protected:
+    float error_prev = 0;
+    float setpoint_prev = 0;
+    float integral_prev = 0;
+    std::chrono::system_clock::time_point timestamp_prev = std::chrono::high_resolution_clock::now();
+};
+
+#endif // COMPLIB_SERVER_PID_HPP

server_prototype/include/reset.hpp

@@ -0,0 +1,31 @@
+#ifndef COMPLIB_SERVER_RESET_HPP
+#define COMPLIB_SERVER_RESET_HPP
+
+#include <pigpio.h>
+
+#define RESET_PIN 23
+#define BOOT_PIN 17
+
+#define RESET_SLEEP_TIME_US 1000 * 100
+#define RESET_STARTUP_SLEEP_TIME_US 1000 * 500
+
+namespace Reset {
+    void reset_robot() {
+        gpioInitialise();
+
+        gpioSetMode(BOOT_PIN, PI_OUTPUT);
+        gpioSetMode(RESET_PIN, PI_OUTPUT);
+
+        gpioWrite(BOOT_PIN, 0);
+        gpioWrite(RESET_PIN, 0);
+
+        usleep(RESET_SLEEP_TIME_US);
+
+        gpioWrite(RESET_PIN, 1);
+
+        usleep(RESET_STARTUP_SLEEP_TIME_US);
+    }
+}
+
+
+#endif //COMPLIB_SERVER_RESET_HPP

server_prototype/include/robot.hpp

@@ -0,0 +1,27 @@
+#ifndef COMPLIB_SERVER_ROBOT_HPP
+#define COMPLIB_SERVER_ROBOT_HPP
+
+#define PI 3.14159265359L
+
+#define WHEEL_CIRCUMFERENCE_MM (71.0L * PI)
+#define TICKS_PER_TURN (27.7L * 100.0L)
+#define ARBOR_LENGTH_MM 139.0L
+#define ARBOR_LENGTH_M (ARBOR_LENGTH_MM / 1000.0L)
+#define TICKS_PER_METER (1000.0L / WHEEL_CIRCUMFERENCE_MM * TICKS_PER_TURN)
+
+#define MOTOR_COUNT 4
+#define MAX_MOTOR_SPEED 65535
+
+#define MOTOR_MAX_DELTA_TIME_S 0.1L
+#define MOTOR_FILTER_ALPHA 0.05L
+#define MOTOR_FILTER_RESET_DELAY_US (1000 * 10)
+#define MOTOR_SPEED_CONTROL_RESET_DELAY_S 0.01L
+#define MOTOR_SPEED_CONTROL_LOOP_HZ 150
+#define MOTOR_SPEED_CONTROL_KP 0.5L
+#define MOTOR_SPEED_CONTROL_KI 5.0L
+#define MOTOR_SPEED_CONTROL_KD 0.025L
+
+#define LEFT_PORT 3
+#define RIGHT_PORT 0
+
+#endif // COMPLIB_SERVER_ROBOT_HPP

server_prototype/include/spi.hpp

@@ -0,0 +1,140 @@
+#ifndef COMPLIB_SERVER_SPI_HPP
+#define COMPLIB_SERVER_SPI_HPP
+
+#include <mutex>
+
+//SPI_MODE_0 (0,0) 	CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge, output data (change) on falling edge
+//SPI_MODE_1 (0,1) 	CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge, output data (change) on rising edge
+//SPI_MODE_2 (1,0) 	CPOL = 1, CPHA = 0, Clock idle high, data is clocked in on falling edge, output data (change) on rising edge
+//SPI_MODE_3 (1,1) 	CPOL = 1, CPHA = 1, Clock idle high, data is clocked in on rising, edge output data (change) on falling edge
+
+#define SPI_BUFFER_SIZE 20
+#define SPI_SPEED 2000000 // 2 MHz
+#define SPI_BITS_PER_WORD 8
+
+class Spi {
+public:
+    static Spi& getInstance()
+    {
+        static Spi instance;
+        return instance;
+    }
+
+    Spi(Spi const&) = delete;
+    void operator=(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,
+
+        // 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,
+
+        // 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 = 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
+    };
+
+private:
+    Spi();
+
+    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;
+
+    void transfer();
+    void clear_buffers();
+
+    uint8_t calculate_checksum(uint8_t* data, uint8_t length);
+};
+
+
+#endif //COMPLIB_SERVER_SPI_HPP

server_prototype/protos/CompLib.proto

@@ -0,0 +1,36 @@
+syntax = "proto3";
+
+package CompLib;
+
+message Header {
+  string message_type = 1;
+}
+
+message Status {
+  bool successful = 1;
+  string error_message = 2;
+}
+
+message GenericRequest {
+  Header header = 1;
+}
+
+message GenericResponse {
+  Header header = 1;
+  Status status = 2;
+}
+
+message ReadSensorsRequest {
+  Header header = 1;
+}
+
+message ReadSensorsResponse {
+  Header header = 1;
+  Status status = 2;
+  uint32 ir_1 = 3;
+  uint32 ir_2 = 4;
+  uint32 ir_3 = 5;
+  uint32 ir_4 = 6;
+  uint32 ir_5 = 7;
+}
+

server_prototype/src/encoder.cpp

@@ -0,0 +1,34 @@
+#include "include/encoder.hpp"
+#include "include/mathUtils.hpp"
+#include "include/spi.hpp"
+
+std::vector<int32_t> Encoder::read_all() {
+    std::vector<int32_t> result;
+    uint8_t result_bytes[ENCODER_COUNT * 4] = {0};
+
+    Spi::getInstance().read_array(Spi::Register::MOTOR_1_POS_B3, ENCODER_COUNT * 4, result_bytes);
+
+    for (int i = 0; i < ENCODER_COUNT; i++) {
+        int32_t encoder_value = mathUtils::int_from_bytes(result_bytes + i * 4, 4);
+        result.push_back(encoder_value);
+        encoder_cache[i] = encoder_value;
+    }
+
+    last_read = std::chrono::system_clock::now();
+    return result;
+}
+
+std::vector<int32_t> Encoder::read_all_cached() {
+    auto last_read_delay_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+            std::chrono::system_clock::now() - last_read
+        ).count();
+
+    if (last_read_delay_ms > ENCODER_CACHE_DURATION_MS) {
+        return read_all();
+    }
+    return std::vector<int32_t>(std::begin(encoder_cache), std::end(encoder_cache));
+}
+
+Encoder::Encoder() {
+
+}

server_prototype/src/main.cpp

@@ -0,0 +1,167 @@
+#include <iostream>
+#include <CompLib.pb.h>
+#include <sys/un.h>
+#include <sys/socket.h>
+#include <zconf.h>
+#include "include/mathUtils.hpp"
+#include "include/messageBuilder.hpp"
+#include "include/errorMessages.hpp"
+#include <spdlog/spdlog.h>
+#include <chrono>
+#include <algorithm>
+
+#include "include/spi.hpp"
+#include "include/reset.hpp"
+#include "include/encoder.hpp"
+#include "include/motor.hpp"
+#include "include/robot.hpp"
+
+#define SOCKET_PATH "/tmp/compLib"
+#define BUFFER_SIZE 64
+
+google::protobuf::Message *processMessage(const std::string &serializedMessage) {
+  CompLib::GenericRequest message;
+  message.ParseFromString(serializedMessage);
+  auto messageTypeName = message.header().message_type();
+
+  if (messageTypeName == CompLib::ReadSensorsRequest::GetDescriptor()->full_name()) {
+    CompLib::ReadSensorsRequest readSensorsRequest;
+    readSensorsRequest.ParseFromString(serializedMessage);
+  } else if (messageTypeName == CompLib::ReadSensorsResponse::GetDescriptor()->full_name()) {
+    CompLib::ReadSensorsResponse readSensorsResponse;
+    readSensorsResponse.ParseFromString(serializedMessage);
+    std::cout << readSensorsResponse.ir_1() << std::endl;
+    std::cout << readSensorsResponse.ir_2() << std::endl;
+    std::cout << readSensorsResponse.ir_3() << std::endl;
+    std::cout << readSensorsResponse.ir_4() << std::endl;
+    std::cout << readSensorsResponse.ir_5() << std::endl;
+  } else {
+    std::cout << messageTypeName << " not found!" << std::endl;
+  }
+
+  google::protobuf::Message *returnMessage = MessageBuilder::genericResponse(false, ERROR_MESSAGE_UNKNOWN);
+  return returnMessage;
+}
+
+[[noreturn]] void socketServer() {
+  struct sockaddr_un socketAddress;
+  int socketFileDescriptor = socket(AF_UNIX, SOCK_STREAM, 0);
+  remove(SOCKET_PATH);
+
+  memset(&socketAddress, 0, sizeof(struct sockaddr_un));
+  socketAddress.sun_family = AF_UNIX;
+  strncpy(socketAddress.sun_path, SOCKET_PATH, sizeof(socketAddress.sun_path) - 1);
+
+  if (bind(socketFileDescriptor, (struct sockaddr *) &socketAddress, sizeof(struct sockaddr_un)) == -1) {
+    exit(-1);
+  }
+
+  if (listen(socketFileDescriptor, 1) == -1) {
+    exit(-2);
+  }
+
+  char readBuffer[BUFFER_SIZE];
+  char writeBuffer[BUFFER_SIZE];
+  for (;;) {
+    int clientFileDescriptor = accept(socketFileDescriptor, NULL, NULL);
+    auto numRead = read(clientFileDescriptor, readBuffer, 1);
+    std::cout << numRead << std::endl;
+
+    uint8_t messageSize = readBuffer[0];
+    std::cout << std::to_string(messageSize) << std::endl;
+
+    numRead = read(clientFileDescriptor, readBuffer, readBuffer[0]);
+    std::cout << numRead << std::endl;
+
+    std::string stringMessage;
+    for (int i{}; i < messageSize; i++) {
+      stringMessage += readBuffer[i];
+    }
+
+    auto response = processMessage(stringMessage);
+    uint8_t responseSize = response->ByteSizeLong();
+    writeBuffer[0] = responseSize;
+
+    std::cout << std::to_string(responseSize) << std::endl;
+    write(clientFileDescriptor, writeBuffer, 1);
+
+    response->SerializeToArray(writeBuffer, BUFFER_SIZE);
+    write(clientFileDescriptor, writeBuffer, responseSize);
+
+    close(clientFileDescriptor);
+  }
+}
+
+int main() {
+  // socketServer();
+  Reset::reset_robot();
+  spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
+  // Spi &spi = Spi::getInstance();
+  // int version = spi.read(1, 1);
+
+  // double count = 20000;
+  // double start_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+  // for (int i{0}; i < count; i++){
+  //     spi.read(1, 1);
+  // }
+  // double end_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+  // double kp = 0.5;
+  // double ki = 5.0;
+  // double kd = 0.025;
+  double setpoint = 75;
+  // double errror_sum = 0;
+  // double last_error = 0;
+  // int port = 0;
+  // long double accumulator = 0;
+  // long double alpha = 0.2;
+  // auto last_time = std::chrono::high_resolution_clock::now();
+  // auto start_time = last_time;
+  // int i = 0;
+  while (1 == 1) {
+    // setpoint += 0.5;
+    // auto current_time = std::chrono::high_resolution_clock::now();
+    // long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
+    // long double delta_seconds_total = std::chrono::duration_cast<std::chrono::microseconds>(current_time - start_time).count() / 1000000.0;
+
+    // last_time = current_time;
+
+    // auto speeds = Motor::get_instance().get_speed();
+    // double e = setpoint - speeds.at(0);
+    // errror_sum += e * delta_seconds;
+    // double error_diff = (e - last_error) / delta_seconds;
+    // last_error = e;
+    // double u = kp * e + errror_sum * ki + error_diff * kd;
+
+    // Motor::power(0, std::min(std::max(-100.0, u), 100.0));
+
+    // spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0));
+
+    usleep(1000.0 * (1000.0 / 10.0));
+    // Motor::get_instance().set_speed(0, -setpoint);
+    // Motor::get_instance().set_speed(3, setpoint);
+    // i += 1;
+    // if (i % 1000 == 0) {
+    //   setpoint *= -1;
+    // }
+    auto speeds = Motor::get_instance().get_speed();
+    spdlog::info("Speed: {} {} Target: {}", speeds.at(0), speeds.at(3), setpoint);
+
+    // auto current_time = std::chrono::high_resolution_clock::now();
+    // long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
+    // last_time = current_time;
+
+    // uint8_t result_bytes[2] = {0};
+    // Spi::getInstance().read_array(Spi::Register::MOTOR_1_VEL_H, 2, result_bytes);
+    // int16_t tps = mathUtils::int_from_bytes(result_bytes, 2);
+    // std::vector<int32_t> current_ticks = Encoder::read_all();
+
+    // long double delta_ticks = current_ticks.at(0) - last_ticks;
+
+    // spdlog::info("{} {}", tps, delta_ticks / delta_seconds);
+
+    // last_ticks = current_ticks.at(0);
+  }
+  return 0;
+}
+

server_prototype/src/motor.cpp

@@ -0,0 +1,155 @@
+#include <stdexcept>
+#include <unistd.h>
+#include <chrono>
+#include <spdlog/spdlog.h>
+
+#include "include/motor.hpp"
+#include "include/spi.hpp"
+#include "include/robot.hpp"
+#include "include/encoder.hpp"
+#include "include/mathUtils.hpp"
+#include "include/pid.hpp"
+
+void check_port(uint8_t port) {
+    if (port >= MOTOR_COUNT) {
+        throw std::invalid_argument("Invalid motor port specified");
+    }
+}
+
+Motor::Motor() {
+    reset_speed();
+
+    speed_control_thread = std::thread(&Motor::speed_control_loop, this);
+    speed_control_thread.detach();
+}
+
+void Motor::set_speed(uint8_t port, double rpm) {
+    check_port(port);
+    
+    speed_targets[port] = rpm;
+    motor_control_modes[port] = Motor::Control::SPEED;
+}
+
+void Motor::set_power(uint8_t port, double percent) {
+    motor_control_modes[port] = Motor::Control::POWER;
+    _set_power(port, percent);
+}
+
+void Motor::_set_power(uint8_t port, double percent) {
+    check_port(port);
+    if (abs(percent) > 100) {
+        throw std::invalid_argument("Invalid motor percent specified. Should be -100 <= percent <= 100");
+    }
+
+    Mode mode = Motor::Mode::COAST;
+    if (percent < 0) {
+        mode = Motor::Mode::BACKWARD;
+    } else if (percent > 0) {
+        mode = Motor::Mode::FORWARD;
+    }
+
+    uint16_t pwm = abs(percent) * (MAX_MOTOR_SPEED / 100.0);
+    Motor::set_pwm(port, pwm, mode);
+}
+
+void Motor::set_pwm(uint8_t port, uint16_t pwm, Mode mode) {
+    check_port(port);
+    if (mode > Motor::Mode::BREAK) {
+        throw std::invalid_argument("Invalid motor mode specified");
+    }
+
+    if (port == 0) {
+        Spi::getInstance().write(Spi::Register::MOTOR_1_PWM_H, 2, pwm);
+        Spi::getInstance().write(Spi::Register::PWM_1_CTRL, 1, mode);
+    } else if (port == 1) {
+        Spi::getInstance().write(Spi::Register::MOTOR_2_PWM_H, 2, pwm);
+        Spi::getInstance().write(Spi::Register::PWM_2_CTRL, 1, mode);
+    } else if (port == 2) {
+        Spi::getInstance().write(Spi::Register::MOTOR_3_PWM_H, 2, pwm);
+        Spi::getInstance().write(Spi::Register::PWM_3_CTRL, 1, mode);
+    } else if (port == 3) {
+        Spi::getInstance().write(Spi::Register::MOTOR_4_PWM_H, 2, pwm);
+        Spi::getInstance().write(Spi::Register::PWM_4_CTRL, 1, mode);
+    }
+}
+
+std::vector<double> Motor::get_speed() {
+    // auto current_time = std::chrono::high_resolution_clock::now();
+    // long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time_encoders_read).count() / 1000000.0;
+    
+    // last_time_encoders_read = current_time;
+
+    // if (delta_seconds > MOTOR_MAX_DELTA_TIME_S) {
+    //     reset_speed();
+    //     usleep(MOTOR_FILTER_RESET_DELAY_US);
+    //     return Motor::get_speed();
+    // }
+
+    // std::vector<int32_t> current_ticks = Encoder::read_all();
+    // std::vector<double> velocities_rpm;
+    // for (int i = 0; i < MOTOR_COUNT; i++) {
+    //     long double delta_ticks = current_ticks.at(i) - last_encoder_values[i];
+
+    //     long double velocity_rpm = (delta_ticks / delta_seconds) * 60.0L / TICKS_PER_TURN;
+    //     filtered_speeds[i] = (long double) ((MOTOR_FILTER_ALPHA * velocity_rpm) + (1.0 - MOTOR_FILTER_ALPHA) * filtered_speeds[i]);
+    //     velocities_rpm.push_back(filtered_speeds[i]);
+
+    //     last_encoder_values[i] = current_ticks.at(i);
+    // }
+
+    std::vector<double> velocities_rpm;
+    uint8_t result_bytes[MOTOR_COUNT * 2] = {0};
+
+    Spi::getInstance().read_array(Spi::Register::MOTOR_1_VEL_H, MOTOR_COUNT * 2, result_bytes);
+    for (int i = 0; i < MOTOR_COUNT; i++) {
+        int16_t ticks_per_second = mathUtils::int_from_bytes(result_bytes + i * 2, 2);
+        velocities_rpm.push_back(ticks_per_second * 60.0L / TICKS_PER_TURN);
+    }
+
+    return velocities_rpm;
+}
+
+void Motor::reset_speed() {
+    last_time_encoders_read = std::chrono::high_resolution_clock::now();
+
+    auto current_ticks = Encoder::get_instance().read_all_cached();
+    for (int i = 0; i < MOTOR_COUNT; i++) {
+        filtered_speeds[i] = 0;
+        last_encoder_values[i] = current_ticks.at(i);
+    }
+}
+
+void Motor::speed_control_loop() {
+    std::vector<PID> pids;
+
+    for (int i = 0; i < MOTOR_COUNT; i++) {
+        pids.push_back(
+            PID{MOTOR_SPEED_CONTROL_KP, MOTOR_SPEED_CONTROL_KI, MOTOR_SPEED_CONTROL_KD, 100.0, 100.0}
+        );
+    }
+
+    while (1 == 1) {
+        float sleep_time = std::round(1000.0 / MOTOR_SPEED_CONTROL_LOOP_HZ);
+        std::this_thread::sleep_for(std::chrono::microseconds((int) sleep_time * 1000));
+
+        bool should_control_speed = false;
+        for (int i = 0; i < MOTOR_COUNT; i++) {
+            if (motor_control_modes[i] == Motor::Control::SPEED) {
+                should_control_speed = true;
+            }
+        }
+
+        if (!should_control_speed) {
+            // spdlog::info("continue");
+            continue;
+        }
+
+        auto speeds = Motor::get_instance().get_speed();
+        for (int i = 0; i < MOTOR_COUNT; i++) {
+            if (motor_control_modes[i] == Motor::Control::SPEED) {
+                float power = pids.at(i)(speed_targets[i], speeds.at(i));
+                _set_power(i, power);
+            }
+        }
+    }
+}

server_prototype/src/pid.cpp

@@ -0,0 +1,64 @@
+#include "include/pid.hpp"
+#include <chrono>
+#include <algorithm>
+#include <spdlog/spdlog.h>
+
+PID::PID(float P, float I, float D, float ramp, float limit)
+    : P(P)
+    , I(I)
+    , D(D)
+    , setpoint_ramp(ramp)
+    , limit(limit)
+    , error_prev(0.0f)
+    , setpoint_prev(0.0f)
+    , integral_prev(0.0f)
+{
+    timestamp_prev = std::chrono::high_resolution_clock::now();
+}
+
+float PID::operator() (float setpoint, float process_variable){
+    // calculate the time from the last call
+    auto timestamp_now = std::chrono::high_resolution_clock::now();
+    long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(timestamp_now - timestamp_prev).count() / 1000000.0;
+
+    if (delta_seconds > PID_RESET_DELAY_S) {
+        reset();
+        delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(timestamp_now - timestamp_prev).count() / 1000000.0;
+    }
+
+    if(setpoint_ramp > 0){
+        float 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;
+        }
+    }
+
+    float error = setpoint - process_variable;
+
+    float proportional = P * error;
+    // Tustin transform of the integral part
+    float integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev);
+    float derivative = D * ((error - error_prev) / delta_seconds);
+
+    float output = proportional + integral + derivative;
+
+    // antiwindup - 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;
+}
+
+void PID::reset() {
+    spdlog::info("RESET");
+    integral_prev = 0;
+    setpoint_prev = 0;
+    error_prev = 0;
+    timestamp_prev = std::chrono::high_resolution_clock::now();
+}

server_prototype/src/spi.cpp

@@ -0,0 +1,113 @@
+#include <fcntl.h>				//Needed for SPI port
+#include <sys/ioctl.h>			//Needed for SPI port
+#include <linux/spi/spidev.h>	//Needed for SPI port
+#include <unistd.h>			    //Needed for SPI port
+#include <iostream>
+#include <string.h>
+#include <spdlog/spdlog.h>
+
+#include "include/spi.hpp"
+#include "include/mathUtils.hpp"
+
+void check_for_error(int error_code, std::string error_message) {
+    if (error_code < 0) {
+        spdlog::error(error_message);
+        exit(1);
+    }
+}
+
+Spi::Spi() {
+    int spi_mode = SPI_MODE_0;
+    int spi_bits_per_workd = SPI_BITS_PER_WORD;
+    int spi_speed = SPI_SPEED;
+
+    check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device");
+    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE");
+    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE");
+    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD");
+    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD");
+    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ");
+    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ");
+}
+
+void Spi::clear_buffers() {
+    memset(tx_buffer, 0, SPI_BUFFER_SIZE);
+    memset(rx_buffer, 0, SPI_BUFFER_SIZE);
+}
+
+uint8_t Spi::calculate_checksum(uint8_t* data, uint8_t length) {
+    int sum = 0;
+    for (int i = 0; i < length; i++) {
+        sum += data[i];
+    }
+    return sum % 256;
+}
+
+int Spi::read(uint8_t reg, uint8_t length) {
+    std::lock_guard<std::recursive_mutex> lock(spi_mutex);
+
+    uint8_t read_buffer[SPI_BUFFER_SIZE] = {0};
+    read_array(reg, length, read_buffer);
+
+    return mathUtils::int_from_bytes(read_buffer, length);
+}
+
+void Spi::read_array(uint8_t reg, uint8_t length, uint8_t* data) {
+    std::lock_guard<std::recursive_mutex> lock(spi_mutex);
+
+    clear_buffers();
+    tx_buffer[0] = 0;
+    tx_buffer[1] = reg;
+    tx_buffer[2] = length;
+
+    transfer();
+
+    uint8_t checksum = calculate_checksum(rx_buffer, length + 3);
+    if (checksum != rx_buffer[length + 3]) {
+        spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length +3], checksum);
+    }
+
+    memcpy(data, rx_buffer + 2, length);
+}
+
+void Spi::write(uint8_t reg, uint8_t length, int value) {
+    std::lock_guard<std::recursive_mutex> lock(spi_mutex);
+
+    uint8_t write_buffer[SPI_BUFFER_SIZE] = {0};
+    mathUtils::bytes_from_int(value, length, write_buffer);
+    write_array(reg, length, write_buffer);
+}
+
+void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t* data) {
+    std::lock_guard<std::recursive_mutex> lock(spi_mutex);
+
+    clear_buffers();
+    tx_buffer[0] = 1;
+    tx_buffer[1] = reg;
+    tx_buffer[2] = length;
+    memcpy(tx_buffer + 3, data, length);
+
+    transfer();
+}
+
+void Spi::transfer() {
+    std::lock_guard<std::recursive_mutex> lock(spi_mutex);
+    
+    struct spi_ioc_transfer spi;
+    memset(&spi, 0, sizeof(spi));
+    
+    spi.tx_buf = (unsigned long) tx_buffer;
+	spi.rx_buf = (unsigned long) rx_buffer;
+	spi.len = SPI_BUFFER_SIZE;
+	spi.delay_usecs = 0;
+	spi.speed_hz = SPI_SPEED;
+    spi.bits_per_word = SPI_BITS_PER_WORD;
+    spi.cs_change = 0;
+    
+    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
+    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
+
+    if (tx_buffer[1] != rx_buffer[1]) {
+        spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]);
+    }
+}