Rework library without unix sockets for now

server_v2/include/Cache.hpp

@@ -0,0 +1,23 @@
+#ifndef COMPLIB_SERVER_CACHE_HPP
+#define COMPLIB_SERVER_CACHE_HPP
+
+#include <chrono>
+
+class Cache {
+public:
+  explicit Cache(int rate_hz);
+
+  bool is_expired();
+
+  void update();
+
+private:
+
+  typedef std::chrono::steady_clock clock;
+
+  std::chrono::microseconds cache_duration{};
+  std::chrono::time_point<std::chrono::steady_clock> last_update{};
+};
+
+
+#endif //COMPLIB_SERVER_CACHE_HPP

server_v2/include/ClosedLoopMotorController.hpp

@@ -0,0 +1,46 @@
+#ifndef COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
+#define COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
+
+
+#include <cstdint>
+#include <array>
+#include <thread>
+
+#include "include/PID.hpp"
+#include "include/Robot.hpp"
+
+class ClosedLoopMotorController {
+public:
+  static ClosedLoopMotorController &getInstance() {
+    static ClosedLoopMotorController instance;
+    return instance;
+  }
+
+  ClosedLoopMotorController(ClosedLoopMotorController const &) = delete;
+
+  void operator=(ClosedLoopMotorController const &) = delete;
+
+  void set_power(uint8_t port, double power);
+
+  void set_speed(uint8_t port, double speed_rpm);
+
+private:
+  enum ControlMode : uint8_t {
+    NONE = 0,
+    POWER = 1,
+    SPEED = 2
+  };
+
+  ClosedLoopMotorController();
+
+  [[noreturn]] void speed_control_loop();
+
+  std::array<PID, ROBOT_MOTOR_COUNT> pids;
+  std::array<ControlMode, ROBOT_MOTOR_COUNT> control_modes{ControlMode::NONE};
+  std::array<double, ROBOT_MOTOR_COUNT> speed_targets{0};
+
+  std::thread speed_control_thread;
+};
+
+
+#endif //COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP

server_v2/include/Encoders.hpp

@@ -0,0 +1,35 @@
+#ifndef COMPLIB_SERVER_ENCODERS_HPP
+#define COMPLIB_SERVER_ENCODERS_HPP
+
+#include <array>
+#include <cstdint>
+#include "include/Cache.hpp"
+#include "include/Robot.hpp"
+
+class Encoders {
+public:
+  static Encoders &getInstance() {
+    static Encoders instance;
+    return instance;
+  }
+
+  Encoders(Encoders const &) = delete;
+
+  void operator=(Encoders const &) = delete;
+
+  std::array<int32_t, ROBOT_MOTOR_COUNT> get_positions();
+
+  std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rpm();
+
+private:
+  Encoders() = default;
+
+  Cache position_cache{ROBOT_ENCODER_RATE_HZ};
+  Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
+
+  std::array<int32_t, ROBOT_MOTOR_COUNT> cached_positions = {0};
+  std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rpm = {0};
+};
+
+
+#endif //COMPLIB_SERVER_ENCODERS_HPP

server_v2/include/IRSensors.hpp

@@ -0,0 +1,37 @@
+#ifndef COMPLIB_SERVER_IRSENSORS_HPP
+#define COMPLIB_SERVER_IRSENSORS_HPP
+
+#include <array>
+#include <cstdint>
+
+#include "include/Cache.hpp"
+#include "include/Robot.hpp"
+
+class IRSensors {
+public:
+  static IRSensors &getInstance() {
+    static IRSensors instance;
+    return instance;
+  }
+
+  IRSensors(IRSensors const &) = delete;
+
+  void operator=(IRSensors const &) = delete;
+
+  static void enable();
+
+  static void disable();
+
+  std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> read();
+
+private:
+
+  IRSensors() = default;
+
+  Cache cache{ROBOT_IR_RATE_HZ};
+
+  std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> cached_values = {0};
+};
+
+
+#endif //COMPLIB_SERVER_IRSENSORS_HPP

server_v2/include/Motors.hpp

@@ -0,0 +1,26 @@
+#ifndef COMPLIB_SERVER_MOTORS_HPP
+#define COMPLIB_SERVER_MOTORS_HPP
+
+#include <cstdint>
+
+class Motors {
+public:
+  enum Mode : uint8_t {
+    COAST = 0,
+    FORWARD = 1,
+    BACKWARD = 2,
+    BREAK = 3,
+    SERVO = 4,
+    NONE = 5
+  };
+
+  static void set_power(uint8_t port, double percent);
+
+  static void set_pwm(uint8_t port, uint16_t pwm, Mode mode);
+
+private:
+
+  Motors() = default;
+};
+
+#endif //COMPLIB_SERVER_MOTORS_HPP

server_v2/include/Odometry.hpp

@@ -0,0 +1,24 @@
+#ifndef COMPLIB_SERVER_ODOMETRY_HPP
+#define COMPLIB_SERVER_ODOMETRY_HPP
+
+class Odometry {
+
+public:
+  Odometry();
+
+  Odometry(double x_position, double y_position, double angular_orientation);
+
+  double get_x_position() const;
+
+  double get_y_position() const;
+
+  double get_angular_orientation() const;
+
+
+private:
+  double x_position;
+  double y_position;
+  double angular_orientation;
+};
+
+#endif //COMPLIB_SERVER_ODOMETRY_HPP

server_v2/include/OdometryController.hpp

@@ -0,0 +1,46 @@
+#ifndef COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
+#define COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
+
+
+#include <thread>
+#include <chrono>
+
+#include "include/Odometry.hpp"
+
+class OdometryController {
+public:
+  static OdometryController &getInstance() {
+    static OdometryController instance;
+    return instance;
+  }
+
+  OdometryController(OdometryController const &) = delete;
+
+  void operator=(OdometryController const &) = delete;
+
+  void enable();
+
+  void disable();
+
+  void reset();
+
+  Odometry get();
+
+private:
+  typedef std::chrono::steady_clock clock;
+
+  OdometryController();
+
+  std::thread odometry_thread;
+
+  [[noreturn]] void odometry_loop();
+
+  Odometry current_odometry{};
+  bool enabled = false;
+  double last_position_left{0};
+  double last_position_right{0};
+  std::chrono::time_point<clock> last_run;
+};
+
+
+#endif //COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP

server_v2/include/PID.hpp

@@ -0,0 +1,32 @@
+#ifndef COMPLIB_SERVER_PID_HPP
+#define COMPLIB_SERVER_PID_HPP
+
+#include <chrono>
+
+class PID {
+public:
+  PID();
+
+  PID(double P, double I, double D, double ramp, double limit);
+
+  ~PID() = default;
+
+  double operator()(double setpoint, double process_variable);
+
+  double P = 1;
+  double I = 0;
+  double D = 0;
+  double setpoint_ramp = 0;
+  double limit = 0;
+
+private:
+  typedef std::chrono::steady_clock clock;
+#
+
+  double error_prev = 0;
+  double setpoint_prev = 0;
+  double integral_prev = 0;
+  clock::time_point timestamp_prev = clock::now();
+};
+
+#endif // COMPLIB_SERVER_PID_HPP

server_v2/include/Robot.hpp

@@ -0,0 +1,31 @@
+#ifndef COMPLIB_SERVER_ROBOT_HPP
+#define COMPLIB_SERVER_ROBOT_HPP
+
+#include <cmath>
+
+#define ROBOT_IR_SENSOR_COUNT 5
+#define ROBOT_IR_RATE_HZ 250
+
+#define ROBOT_MOTOR_COUNT 4
+#define ROBOT_MOTOR_SPEED_CONTROL_KP 0.5
+#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0
+#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.025
+#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 100.0
+#define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
+
+#define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250
+#define ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT 3
+#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT 0
+#define ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT 1.0
+#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0
+
+#define ROBOT_ENCODER_RATE_HZ 250
+
+#define ROBOT_WHEEL_CIRCUMFERENCE_MM (71.0 * M_PI)
+#define ROBOT_TICKS_PER_TURN (27.7 * 100.0)
+#define ROBOT_ARBOR_LENGTH_MM 139.0
+#define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0)
+#define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN)
+
+
+#endif //COMPLIB_SERVER_ROBOT_HPP

server_v2/include/mathUtils.hpp

@@ -4,24 +4,36 @@
 #include <cstdint>
 
 namespace mathUtils {
-    inline int int_from_bytes(uint8_t *data, int length) {
-        int ret = 0;
+  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;
+    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++;
-        }
+  template<class T>
+  inline T from_bytes(const uint8_t *data, int length) {
+    T 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/spi.hpp

@@ -14,126 +14,129 @@
 
 class Spi {
 public:
-    static Spi& getInstance()
-    {
-        static Spi instance;
-        return instance;
-    }
+  static Spi &getInstance() {
+    static Spi instance;
+    return instance;
+  }
 
-    Spi(Spi const&) = delete;
-    void operator=(Spi const&) = delete;
+  Spi(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,
+  void operator=(Spi const &) = delete;
 
-        // 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,
+  int read(uint8_t reg, uint8_t length);
 
-        // PWM Control Modes
-        PWM_1_CTRL = 26,
-        PWM_2_CTRL = 27,
-        PWM_3_CTRL = 28,
-        PWM_4_CTRL = 29,
+  void read_array(uint8_t reg, uint8_t length, uint8_t *data);
 
-        // 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,
+  void write(uint8_t reg, uint8_t length, int value);
 
-        // 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,
+  void write_array(uint8_t reg, uint8_t length, const uint8_t *data);
 
-        // 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,
+  enum Register : uint8_t {
+    IDENTIFICATION_MODEL_ID = 1,
+    IDENTIFICATION_MODEL_REV_MAJOR = 2,
+    IDENTIFICATION_MODEL_REV_MINOR = 3,
+    IDENTIFICATION_MODEL_REV_PATCH = 4,
 
-        // Display registers
-        DISPLAY_LINE_1_C0 = 69,
-        DISPLAY_LINE_2_C0 = 85,
-        DISPLAY_LINE_3_C0 = 101,
-        DISPLAY_LINE_4_C0 = 117,
+    // 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,
 
-        // 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
-    };
+    // 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 = 133,
+    MOTOR_1_VEL_L = 134,
+    MOTOR_2_VEL_H = 135,
+    MOTOR_2_VEL_L = 136,
+    MOTOR_3_VEL_H = 137,
+    MOTOR_3_VEL_L = 138,
+    MOTOR_4_VEL_H = 139,
+    MOTOR_4_VEL_L = 140
+  };
 
 private:
-    Spi();
+  Spi();
 
-    int spi_file_descriptor{};
-    uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
-    uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};
+  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;
+  std::recursive_mutex spi_mutex;
 
-    void transfer();
-    void clear_buffers();
+  void transfer();
 
-    uint8_t calculate_checksum(uint8_t* data, uint8_t length);
+  void clear_buffers();
+
+  uint8_t calculate_checksum(uint8_t *data, uint8_t length);
 };