From 1a488065ac1cd229e1b738261eacde4a23afac7e Mon Sep 17 00:00:00 2001
From: root <konstantin@localhost.systems>
Date: Mon, 21 Mar 2022 18:51:16 +0000
Subject: [PATCH] Add get speed function to motor class

---
 server/src/main.cpp  | 60 +++++++++++++++++++++++---------------------
 server/src/motor.cpp |  3 ---
 2 files changed, 31 insertions(+), 32 deletions(-)

diff --git a/server/src/main.cpp b/server/src/main.cpp
index 119aec9..6930681 100644
--- a/server/src/main.cpp
+++ b/server/src/main.cpp
@@ -7,6 +7,7 @@
 #include "include/errorMessages.hpp"
 #include <spdlog/spdlog.h>
 #include <chrono>
+#include <algorithm>
 
 #include "include/spi.hpp"
 #include "include/reset.hpp"
@@ -88,12 +89,12 @@ google::protobuf::Message *processMessage(const std::string &serializedMessage)
 
     close(clientFileDescriptor);
   }
-
 }
 
 int main() {
   // socketServer();
   Reset::reset_robot();
+  spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
   // Spi &spi = Spi::getInstance();
   // int version = spi.read(1, 1);
 
@@ -104,43 +105,44 @@ int main() {
   // }
   // double end_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
 
-  double power = 0;
+  double kp = 0.5;
+  double ki = 5.0;
+  double kd = 0.025;
+  double setpoint = 0;
+  double errror_sum = 0;
+  double last_error = 0;
   // int port = 0;
   // long double accumulator = 0;
   // long double alpha = 0.2;
-  while (power < 100) {
-    power += 0.1;
-    Motor::power(0, power);
-    Motor::power(3, power);
+  auto last_time = std::chrono::high_resolution_clock::now();
+  auto start_time = last_time;
+  int i = 0;
+  while (1 == 1) {
+    setpoint += 0.05;
+    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;
+
+    last_time = current_time;
 
     auto speeds = Motor::get_instance().get_speed();
-    // spdlog::info("{:05.0f} {:05.0f}", speeds.at(0), speeds.at(3));
+    double e = setpoint - speeds.at(0);
+    errror_sum += e * delta_seconds;
+    double error_diff = (e - last_error) / delta_seconds;
+    last_error = e;
+    double u = kp * e + errror_sum * ki + error_diff * kd;
+
+    Motor::power(0, std::min(std::max(-100.0, u), 100.0));
+
+    spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0));
 
-    // auto start_values = Encoder::read_all();
-    // spdlog::info("{}", start_values.at(3));
-    // auto start_time = std::chrono::high_resolution_clock::now();
     usleep(1000.0 * (1000.0 / 150.0));
-    // auto end_values = Encoder::read_all();
-    
-    // auto elapsed_time = std::chrono::high_resolution_clock::now() - start_time;
-    // long double elapsed_ticks = end_values.at(port) - start_values.at(port);
 
-    // long double elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(
-    //     elapsed_time).count();
-    // long double elapsed_s = std::chrono::duration_cast<std::chrono::microseconds>(
-    //     elapsed_time).count() / 1000000.0;
-
-    // long double vel_ticks_s = elapsed_ticks / elapsed_s;
-    // long double vel_rpm = vel_ticks_s * 60.0L / TICKS_PER_TURN;
-
-    // accumulator = (alpha * vel_rpm) + (1.0 - alpha) * accumulator;
-
-    // spdlog::info("{:05.0f} {:05.0f} {:05.0f} {:05.0f} {:05.0f}", elapsed_us, elapsed_ticks, vel_ticks_s,  vel_rpm, accumulator);
+    i += 1;
+    // if (i % 1000 == 0) {
+    //   setpoint *= -1;
+    // }
   }
-
-
-  // spdlog::info(count / (end_time - start_time) * 1000.0);
-  // spi.write(Spi::Register::DISPLAY_LINE_1_C0, 1, 66);
   return 0;
 }
 
diff --git a/server/src/motor.cpp b/server/src/motor.cpp
index d12ea1d..5ea9ec6 100644
--- a/server/src/motor.cpp
+++ b/server/src/motor.cpp
@@ -76,7 +76,6 @@ std::vector<double> Motor::get_speed() {
         velocities_rpm.push_back(filtered_speeds[i]);
 
         last_encoder_values[i] = current_ticks.at(i);
-        spdlog::info("{} {}", i, current_ticks.at(i));
     }
 
     return velocities_rpm;
@@ -85,8 +84,6 @@ std::vector<double> Motor::get_speed() {
 void Motor::reset_speed() {
     last_time_read = std::chrono::high_resolution_clock::now();
 
-    spdlog::info("Reeeeeset");
-
     auto current_ticks = Encoder::read_all();
     for (int i = 0; i < MOTOR_COUNT; i++) {
         filtered_speeds[i] = 0;