Add encoder with filter

server/include/motor.hpp

@@ -0,0 +1,44 @@
+#ifndef COMPLIB_SERVER_MOTOR_HPP
+#define COMPLIB_SERVER_MOTOR_HPP
+
+#include <cstdint>
+#include <vector>
+#include <chrono>
+
+#include "include/robot.hpp"
+
+class Motor {
+public:
+    enum Mode : uint8_t {
+        COAST = 0,
+        FORWARD = 1,
+        BACKWARD = 2,
+        BREAK = 3,
+        SERVO = 4,
+        NONE = 5
+    };
+
+    static Motor& get_instance()
+    {
+        static Motor instance;
+        return instance;
+    }
+
+    Motor(Motor const&) = delete;
+    void operator=(Motor const&) = delete;
+
+    static void power(uint8_t port, double percent);
+    static void pwm(uint8_t port, uint16_t pwm, Mode mode);
+
+    std::vector<double> get_speed();
+private:
+    Motor();
+
+    int32_t last_encoder_values[MOTOR_COUNT] = {0};
+    double filtered_speeds[MOTOR_COUNT] = {0};
+    std::chrono::system_clock::time_point last_time_read;
+    
+    void reset_speed();
+};
+
+#endif // COMPLIB_SERVER_MOTOR_HPP