Start on v2

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