save!

server/include/encoder.hpp

@@ -3,12 +3,29 @@
 
 #include <vector>
 #include <cstdint>
+#include <chrono>
 
 #define ENCODER_COUNT 4
+#define ENCODER_CACHE_DURATION_MS 4 // 200 HZ
 
 class Encoder {
 public:
-    static std::vector<int32_t> read_all();
+    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/src/encoder.cpp

@@ -9,7 +9,26 @@ std::vector<int32_t> Encoder::read_all() {
     Spi::getInstance().read_array(Spi::Register::MOTOR_1_POS_B3, ENCODER_COUNT * 4, result_bytes);
 
     for (int i = 0; i < ENCODER_COUNT; i++) {
-        result.push_back(mathUtils::int_from_bytes(result_bytes + i * 4, 4));
+        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() {
+
 }

server/src/main.cpp

@@ -109,7 +109,7 @@ int main() {
   // double kp = 0.5;
   // double ki = 5.0;
   // double kd = 0.025;
-  double setpoint = -50;
+  double setpoint = 75;
   // double errror_sum = 0;
   // double last_error = 0;
   // int port = 0;
@@ -119,7 +119,7 @@ int main() {
   // auto start_time = last_time;
   // int i = 0;
   while (1 == 1) {
-    setpoint += 0.5;
+    // 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;
@@ -137,15 +137,15 @@ int main() {
 
     // 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);
+    // 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 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;

server/src/motor.cpp

@@ -112,7 +112,7 @@ std::vector<double> Motor::get_speed() {
 void Motor::reset_speed() {
     last_time_encoders_read = std::chrono::high_resolution_clock::now();
 
-    auto current_ticks = Encoder::read_all();
+    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);