Rework library without unix sockets for now

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