From c5d2ee82177069148ce03bacb16341f9440cc3f1 Mon Sep 17 00:00:00 2001
From: Louis Asanaka <25756888+LouisAsanaka@users.noreply.github.com>
Date: Sat, 7 Mar 2020 22:43:28 +0800
Subject: [PATCH] Finally fixed path following

The brute force time check will hog the CPU for the duration of the path, which is not ideal.
---
 include/constants.hpp |  2 +-
 src/simController.cpp | 19 +++++++++++++++----
 2 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/include/constants.hpp b/include/constants.hpp
index 7b15a53..34c5ad9 100644
--- a/include/constants.hpp
+++ b/include/constants.hpp
@@ -8,7 +8,7 @@
 #define POINT_CIRCLE_RADIUS 12
 
 // Max frame rate
-#define FRAMERATE 120.0f
+#define FRAMERATE 60.0f
 #define IDEAL_DT (1 / FRAMERATE)
 
 // Meters to inches
diff --git a/src/simController.cpp b/src/simController.cpp
index 396eff2..f0635b8 100644
--- a/src/simController.cpp
+++ b/src/simController.cpp
@@ -299,7 +299,7 @@ void SimController::generateProfile() {
     while (isBuffering.load(std::memory_order_acquire)) {
         std::this_thread::sleep_for(std::chrono::milliseconds(1));
     }
-    pathGen.send(points.getPoints(), 10, 20);
+    pathGen.send(points.getPoints(), 8, 8);
 }
 
 void SimController::fillPath(const char* bytes, size_t n) {
@@ -383,22 +383,33 @@ void SimController::executeProfile() {
         trajectory[0].angle
     );
     robotMutex.unlock();
+    
     for (int i = 0; i < pathLength - 1 && isPathing.load(std::memory_order_acquire); ++i) {
         float dt = trajectory[i + 1].dt;
-
         float linearSpeed = trajectory[i].velocity;
         float angularSpeed = (trajectory[i + 1].angle - trajectory[i].angle) / dt;
 
         //std::cout << "Robot Angle: " << robot.getBody()->GetAngle() * 180 / b2_pi << std::endl;
         //std::cout << "Target Angle: " << trajectory[i + 1].angle * 180 / b2_pi << std::endl;
-        std::cout << dt << std::endl;
+        //std::cout << linearSpeed << " | " << trajectory[i].velocity << std::endl;
 
         robotMutex.lock();
         robot.setChassisSpeeds(linearSpeed, angularSpeed);
         //robot.update(dt);
         robotMutex.unlock();
 
-        std::this_thread::sleep_until(std::chrono::steady_clock::now() + std::chrono::duration<float>(dt));
+        // sleep_until isn't precise enough, so gotta CPU hog cycles now
+        // TODO: Find a better way of doing this
+        auto start = std::chrono::steady_clock::now();
+        while (true) {
+            auto now = std::chrono::steady_clock::now();
+            auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(now - start);
+            if (elapsed.count() * 1e-6 > dt) {
+                break;
+            }
+            //std::this_thread::sleep_for(std::chrono::duration<float>(dt / 10));
+        }
+        //std::this_thread::sleep_until(std::chrono::steady_clock::now() + std::chrono::duration<float>(dt));
     }
 }