From e9ae1a320a654f4beb8981528029f230bf3d35b6 Mon Sep 17 00:00:00 2001 From: root Date: Sat, 21 May 2022 13:10:28 +0100 Subject: [PATCH] Start on v2 --- {server => server_prototype}/.gitignore | 0 {server => server_prototype}/.idea/.gitignore | 0 {server => server_prototype}/.idea/.name | 0 {server => server_prototype}/.idea/misc.xml | 0 .../.idea/modules.xml | 0 .../.idea/saveactions_settings.xml | 0 .../.idea/serialmonitor_settings.xml | 0 {server => server_prototype}/.idea/server.iml | 0 {server => server_prototype}/.idea/vcs.xml | 0 {server => server_prototype}/CMakeLists.txt | 0 .../cmake/modules/Findpigpio.cmake | 0 .../include/encoder.hpp | 0 .../include/errorMessages.hpp | 0 .../include/mathUtils.hpp | 0 .../include/messageBuilder.hpp | 0 .../include/motor.hpp | 0 {server => server_prototype}/include/pid.hpp | 0 .../include/reset.hpp | 0 .../include/robot.hpp | 0 {server => server_prototype}/include/spi.hpp | 0 .../protos/CompLib.proto | 0 {server => server_prototype}/src/encoder.cpp | 0 {server => server_prototype}/src/main.cpp | 6 +- {server => server_prototype}/src/motor.cpp | 2 +- {server => server_prototype}/src/pid.cpp | 0 {server => server_prototype}/src/spi.cpp | 0 server_v2/.gitignore | 106 +++++++++++++ server_v2/.idea/.gitignore | 6 + server_v2/.idea/.name | 1 + server_v2/.idea/misc.xml | 4 + server_v2/.idea/modules.xml | 8 + server_v2/.idea/saveactions_settings.xml | 15 ++ server_v2/.idea/serialmonitor_settings.xml | 4 + server_v2/.idea/server.iml | 2 + server_v2/.idea/vcs.xml | 6 + server_v2/CMakeLists.txt | 45 ++++++ server_v2/cmake/modules/Findpigpio.cmake | 31 ++++ server_v2/include/mathUtils.hpp | 27 ++++ server_v2/include/reset.hpp | 32 ++++ server_v2/include/spi.hpp | 140 ++++++++++++++++++ server_v2/protos/CompLib.proto | 36 +++++ server_v2/src/main.cpp | 28 ++++ server_v2/src/spi.cpp | 113 ++++++++++++++ 43 files changed, 608 insertions(+), 4 deletions(-) rename {server => server_prototype}/.gitignore (100%) rename {server => server_prototype}/.idea/.gitignore (100%) rename {server => server_prototype}/.idea/.name (100%) rename {server => server_prototype}/.idea/misc.xml (100%) rename {server => server_prototype}/.idea/modules.xml (100%) rename {server => server_prototype}/.idea/saveactions_settings.xml (100%) rename {server => server_prototype}/.idea/serialmonitor_settings.xml (100%) rename {server => server_prototype}/.idea/server.iml (100%) rename {server => server_prototype}/.idea/vcs.xml (100%) rename {server => server_prototype}/CMakeLists.txt (100%) rename {server => server_prototype}/cmake/modules/Findpigpio.cmake (100%) rename {server => server_prototype}/include/encoder.hpp (100%) rename {server => server_prototype}/include/errorMessages.hpp (100%) rename {server => server_prototype}/include/mathUtils.hpp (100%) rename {server => server_prototype}/include/messageBuilder.hpp (100%) rename {server => server_prototype}/include/motor.hpp (100%) rename {server => server_prototype}/include/pid.hpp (100%) rename {server => server_prototype}/include/reset.hpp (100%) rename {server => server_prototype}/include/robot.hpp (100%) rename {server => server_prototype}/include/spi.hpp (100%) rename {server => server_prototype}/protos/CompLib.proto (100%) rename {server => server_prototype}/src/encoder.cpp (100%) rename {server => server_prototype}/src/main.cpp (96%) rename {server => server_prototype}/src/motor.cpp (99%) rename {server => server_prototype}/src/pid.cpp (100%) rename {server => server_prototype}/src/spi.cpp (100%) create mode 100644 server_v2/.gitignore create mode 100644 server_v2/.idea/.gitignore create mode 100644 server_v2/.idea/.name create mode 100644 server_v2/.idea/misc.xml create mode 100644 server_v2/.idea/modules.xml create mode 100644 server_v2/.idea/saveactions_settings.xml create mode 100644 server_v2/.idea/serialmonitor_settings.xml create mode 100644 server_v2/.idea/server.iml create mode 100644 server_v2/.idea/vcs.xml create mode 100644 server_v2/CMakeLists.txt create mode 100644 server_v2/cmake/modules/Findpigpio.cmake create mode 100644 server_v2/include/mathUtils.hpp create mode 100644 server_v2/include/reset.hpp create mode 100644 server_v2/include/spi.hpp create mode 100644 server_v2/protos/CompLib.proto create mode 100644 server_v2/src/main.cpp create mode 100644 server_v2/src/spi.cpp diff --git a/server/.gitignore b/server_prototype/.gitignore similarity index 100% rename from server/.gitignore rename to server_prototype/.gitignore diff --git a/server/.idea/.gitignore b/server_prototype/.idea/.gitignore similarity index 100% rename from server/.idea/.gitignore rename to server_prototype/.idea/.gitignore diff --git a/server/.idea/.name b/server_prototype/.idea/.name similarity index 100% rename from server/.idea/.name rename to server_prototype/.idea/.name diff --git a/server/.idea/misc.xml b/server_prototype/.idea/misc.xml similarity index 100% rename from server/.idea/misc.xml rename to server_prototype/.idea/misc.xml diff --git a/server/.idea/modules.xml b/server_prototype/.idea/modules.xml similarity index 100% rename from server/.idea/modules.xml rename to server_prototype/.idea/modules.xml diff --git a/server/.idea/saveactions_settings.xml b/server_prototype/.idea/saveactions_settings.xml similarity index 100% rename from server/.idea/saveactions_settings.xml rename to server_prototype/.idea/saveactions_settings.xml diff --git a/server/.idea/serialmonitor_settings.xml b/server_prototype/.idea/serialmonitor_settings.xml similarity index 100% rename from server/.idea/serialmonitor_settings.xml rename to server_prototype/.idea/serialmonitor_settings.xml diff --git a/server/.idea/server.iml b/server_prototype/.idea/server.iml similarity index 100% rename from server/.idea/server.iml rename to server_prototype/.idea/server.iml diff --git a/server/.idea/vcs.xml b/server_prototype/.idea/vcs.xml similarity index 100% rename from server/.idea/vcs.xml rename to server_prototype/.idea/vcs.xml diff --git a/server/CMakeLists.txt b/server_prototype/CMakeLists.txt similarity index 100% rename from server/CMakeLists.txt rename to server_prototype/CMakeLists.txt diff --git a/server/cmake/modules/Findpigpio.cmake b/server_prototype/cmake/modules/Findpigpio.cmake similarity index 100% rename from server/cmake/modules/Findpigpio.cmake rename to server_prototype/cmake/modules/Findpigpio.cmake diff --git a/server/include/encoder.hpp b/server_prototype/include/encoder.hpp similarity index 100% rename from server/include/encoder.hpp rename to server_prototype/include/encoder.hpp diff --git a/server/include/errorMessages.hpp b/server_prototype/include/errorMessages.hpp similarity index 100% rename from server/include/errorMessages.hpp rename to server_prototype/include/errorMessages.hpp diff --git a/server/include/mathUtils.hpp b/server_prototype/include/mathUtils.hpp similarity index 100% rename from server/include/mathUtils.hpp rename to server_prototype/include/mathUtils.hpp diff --git a/server/include/messageBuilder.hpp b/server_prototype/include/messageBuilder.hpp similarity index 100% rename from server/include/messageBuilder.hpp rename to server_prototype/include/messageBuilder.hpp diff --git a/server/include/motor.hpp b/server_prototype/include/motor.hpp similarity index 100% rename from server/include/motor.hpp rename to server_prototype/include/motor.hpp diff --git a/server/include/pid.hpp b/server_prototype/include/pid.hpp similarity index 100% rename from server/include/pid.hpp rename to server_prototype/include/pid.hpp diff --git a/server/include/reset.hpp b/server_prototype/include/reset.hpp similarity index 100% rename from server/include/reset.hpp rename to server_prototype/include/reset.hpp diff --git a/server/include/robot.hpp b/server_prototype/include/robot.hpp similarity index 100% rename from server/include/robot.hpp rename to server_prototype/include/robot.hpp diff --git a/server/include/spi.hpp b/server_prototype/include/spi.hpp similarity index 100% rename from server/include/spi.hpp rename to server_prototype/include/spi.hpp diff --git a/server/protos/CompLib.proto b/server_prototype/protos/CompLib.proto similarity index 100% rename from server/protos/CompLib.proto rename to server_prototype/protos/CompLib.proto diff --git a/server/src/encoder.cpp b/server_prototype/src/encoder.cpp similarity index 100% rename from server/src/encoder.cpp rename to server_prototype/src/encoder.cpp diff --git a/server/src/main.cpp b/server_prototype/src/main.cpp similarity index 96% rename from server/src/main.cpp rename to server_prototype/src/main.cpp index 2f9429d..f1c4d58 100644 --- a/server/src/main.cpp +++ b/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(); - // spdlog::info("Speed: {} {} Target: {}", speeds.at(0), speeds.at(3), setpoint); + 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(current_time - last_time).count() / 1000000.0; diff --git a/server/src/motor.cpp b/server_prototype/src/motor.cpp similarity index 99% rename from server/src/motor.cpp rename to server_prototype/src/motor.cpp index 28c534f..0176b5b 100644 --- a/server/src/motor.cpp +++ b/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; } diff --git a/server/src/pid.cpp b/server_prototype/src/pid.cpp similarity index 100% rename from server/src/pid.cpp rename to server_prototype/src/pid.cpp diff --git a/server/src/spi.cpp b/server_prototype/src/spi.cpp similarity index 100% rename from server/src/spi.cpp rename to server_prototype/src/spi.cpp diff --git a/server_v2/.gitignore b/server_v2/.gitignore new file mode 100644 index 0000000..5b4bcb6 --- /dev/null +++ b/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 diff --git a/server_v2/.idea/.gitignore b/server_v2/.idea/.gitignore new file mode 100644 index 0000000..8bf4d45 --- /dev/null +++ b/server_v2/.idea/.gitignore @@ -0,0 +1,6 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/server_v2/.idea/.name b/server_v2/.idea/.name new file mode 100644 index 0000000..4e811b7 --- /dev/null +++ b/server_v2/.idea/.name @@ -0,0 +1 @@ +compLib_server \ No newline at end of file diff --git a/server_v2/.idea/misc.xml b/server_v2/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/server_v2/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/server_v2/.idea/modules.xml b/server_v2/.idea/modules.xml new file mode 100644 index 0000000..fbd90b4 --- /dev/null +++ b/server_v2/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/server_v2/.idea/saveactions_settings.xml b/server_v2/.idea/saveactions_settings.xml new file mode 100644 index 0000000..b7b3f34 --- /dev/null +++ b/server_v2/.idea/saveactions_settings.xml @@ -0,0 +1,15 @@ + + + + + + \ No newline at end of file diff --git a/server_v2/.idea/serialmonitor_settings.xml b/server_v2/.idea/serialmonitor_settings.xml new file mode 100644 index 0000000..eff936b --- /dev/null +++ b/server_v2/.idea/serialmonitor_settings.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/server_v2/.idea/server.iml b/server_v2/.idea/server.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/server_v2/.idea/server.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/server_v2/.idea/vcs.xml b/server_v2/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/server_v2/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt new file mode 100644 index 0000000..41eedd2 --- /dev/null +++ b/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) diff --git a/server_v2/cmake/modules/Findpigpio.cmake b/server_v2/cmake/modules/Findpigpio.cmake new file mode 100644 index 0000000..1308822 --- /dev/null +++ b/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 _FOUND variable. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(pigpio + DEFAULT_MSG + pigpio_INCLUDE_DIR pigpio_LIBRARY pigpiod_if_LIBRARY pigpiod_if2_LIBRARY) \ No newline at end of file diff --git a/server_v2/include/mathUtils.hpp b/server_v2/include/mathUtils.hpp new file mode 100644 index 0000000..b4eee56 --- /dev/null +++ b/server_v2/include/mathUtils.hpp @@ -0,0 +1,27 @@ +#ifndef COMPLIB_SERVER_MATHUTILS_HPP +#define COMPLIB_SERVER_MATHUTILS_HPP + +#include + +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 \ No newline at end of file diff --git a/server_v2/include/reset.hpp b/server_v2/include/reset.hpp new file mode 100644 index 0000000..07f1580 --- /dev/null +++ b/server_v2/include/reset.hpp @@ -0,0 +1,32 @@ +#ifndef COMPLIB_SERVER_RESET_HPP +#define COMPLIB_SERVER_RESET_HPP + +#include +#include + +#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 diff --git a/server_v2/include/spi.hpp b/server_v2/include/spi.hpp new file mode 100644 index 0000000..c48e5a4 --- /dev/null +++ b/server_v2/include/spi.hpp @@ -0,0 +1,140 @@ +#ifndef COMPLIB_SERVER_SPI_HPP +#define COMPLIB_SERVER_SPI_HPP + +#include + +//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 diff --git a/server_v2/protos/CompLib.proto b/server_v2/protos/CompLib.proto new file mode 100644 index 0000000..77a202d --- /dev/null +++ b/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; +} + diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp new file mode 100644 index 0000000..a80d960 --- /dev/null +++ b/server_v2/src/main.cpp @@ -0,0 +1,28 @@ +#include +#include + +#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(end - start).count()); + spdlog::info("Loop rate: {}hz", SAMPLES / chrono::duration_cast(end - start).count() * 1000.0); + + return 0; +} + diff --git a/server_v2/src/spi.cpp b/server_v2/src/spi.cpp new file mode 100644 index 0000000..9314905 --- /dev/null +++ b/server_v2/src/spi.cpp @@ -0,0 +1,113 @@ +#include //Needed for SPI port +#include //Needed for SPI port +#include //Needed for SPI port +#include //Needed for SPI port +#include +#include +#include + +#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 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 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 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 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 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]); + } +}