Start on v2
This commit is contained in:
root
parent
4d5c26d10c
commit
e9ae1a320a
43 changed files with 608 additions and 4 deletions
106
server/.gitignore
vendored
106
server/.gitignore
vendored
|
|
@ -1,106 +0,0 @@
|
|||
|
||||
# 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
|
||||
6
server/.idea/.gitignore
generated
vendored
6
server/.idea/.gitignore
generated
vendored
|
|
@ -1,6 +0,0 @@
|
|||
# Default ignored files
|
||||
/shelf/
|
||||
/workspace.xml
|
||||
# Datasource local storage ignored files
|
||||
/dataSources/
|
||||
/dataSources.local.xml
|
||||
1
server/.idea/.name
generated
1
server/.idea/.name
generated
|
|
@ -1 +0,0 @@
|
|||
compLib_server
|
||||
4
server/.idea/misc.xml
generated
4
server/.idea/misc.xml
generated
|
|
@ -1,4 +0,0 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
|
||||
</project>
|
||||
8
server/.idea/modules.xml
generated
8
server/.idea/modules.xml
generated
|
|
@ -1,8 +0,0 @@
|
|||
<?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>
|
||||
15
server/.idea/saveactions_settings.xml
generated
15
server/.idea/saveactions_settings.xml
generated
|
|
@ -1,15 +0,0 @@
|
|||
<?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>
|
||||
4
server/.idea/serialmonitor_settings.xml
generated
4
server/.idea/serialmonitor_settings.xml
generated
|
|
@ -1,4 +0,0 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="SerialMonitorSettings" BaudRate="9600" LineEndingsIndex="0" ShowStatusWidget="false" />
|
||||
</project>
|
||||
2
server/.idea/server.iml
generated
2
server/.idea/server.iml
generated
|
|
@ -1,2 +0,0 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<module classpath="CMake" type="CPP_MODULE" version="4" />
|
||||
6
server/.idea/vcs.xml
generated
6
server/.idea/vcs.xml
generated
|
|
@ -1,6 +0,0 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="VcsDirectoryMappings">
|
||||
<mapping directory="$PROJECT_DIR$/.." vcs="Git" />
|
||||
</component>
|
||||
</project>
|
||||
|
|
@ -1,54 +0,0 @@
|
|||
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)
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
################################################################################
|
||||
### 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)
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
#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
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
//
|
||||
// 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
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
#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
|
||||
|
|
@ -1,28 +0,0 @@
|
|||
#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
|
||||
|
|
@ -1,61 +0,0 @@
|
|||
#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
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
#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
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
#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
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
#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
|
||||
|
|
@ -1,140 +0,0 @@
|
|||
#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
|
||||
|
|
@ -1,36 +0,0 @@
|
|||
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;
|
||||
}
|
||||
|
||||
|
|
@ -1,34 +0,0 @@
|
|||
#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() {
|
||||
|
||||
}
|
||||
|
|
@ -1,167 +0,0 @@
|
|||
#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;
|
||||
}
|
||||
|
||||
|
|
@ -1,155 +0,0 @@
|
|||
#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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,64 +0,0 @@
|
|||
#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();
|
||||
}
|
||||
|
|
@ -1,113 +0,0 @@
|
|||
#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]);
|
||||
}
|
||||
}
|
||||
Reference in a new issue