Start on v2

server_prototype/src/main.cpp

@@ -137,15 +137,15 @@ int main() {
 
     // spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0));
 
-    // usleep(1000.0 * (1000.0 / 10.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();
+    auto speeds = Motor::get_instance().get_speed();
-    // spdlog::info("Speed: {} {} Target: {}", speeds.at(0), speeds.at(3), setpoint);
+    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;

server_prototype/src/motor.cpp

@@ -140,7 +140,7 @@ void Motor::speed_control_loop() {
         }
 
         if (!should_control_speed) {
-            spdlog::info("continue");
+            // spdlog::info("continue");
             continue;
         }
 

server_v2/.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_v2/.idea/.gitignore

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

server_v2/.idea/.name

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

server_v2/.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_v2/.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_v2/.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_v2/.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_v2/.idea/server.iml

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

server_v2/.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_v2/CMakeLists.txt

@@ -0,0 +1,45 @@
+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
+        )
+
+set(HDR_FILES
+        include/spi.hpp
+        include/reset.hpp
+        include/mathUtils.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_v2/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_v2/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_v2/include/reset.hpp

@@ -0,0 +1,32 @@
+#ifndef COMPLIB_SERVER_RESET_HPP
+#define COMPLIB_SERVER_RESET_HPP
+
+#include <pigpio.h>
+#include <unistd.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_v2/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_v2/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_v2/src/main.cpp

@@ -0,0 +1,28 @@
+#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
+
+using namespace std;
+
+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);
+  }
+  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;
+}
+

server_v2/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]);
+    }
+}