From a0ec5407ed4a93c3ccb15d571a90d9ce66520b99 Mon Sep 17 00:00:00 2001
From: Serrial Error <serrialerror@outlook.com>
Date: Mon, 12 Jan 2026 19:25:24 -0800
Subject: [PATCH 1/2] added initial logger

---
 include/drivetrain.hpp |  2 +-
 include/logger.hpp     |  3 +++
 src/drivetrain.cpp     | 42 +++++++++++++++++++++++++++++++-----------
 src/logger.cpp         | 10 ++++++++++
 4 files changed, 45 insertions(+), 12 deletions(-)
 create mode 100644 include/logger.hpp
 create mode 100644 src/logger.cpp

diff --git a/include/drivetrain.hpp b/include/drivetrain.hpp
index 1f0722d..8efbeae 100644
--- a/include/drivetrain.hpp
+++ b/include/drivetrain.hpp
@@ -345,7 +345,7 @@ class drivetrain {
 	 *
 	 * @param[in] distance Distance to travel in meters (positive = forward, negative = backward)
 	 */
-	void linear_mp(const float distance);
+	void linear_mp(const float distance, bool log = false);
 	
 	/**
 	 * @brief Executes an angular motion profile to rotate a specified angle
diff --git a/include/logger.hpp b/include/logger.hpp
new file mode 100644
index 0000000..4f69d00
--- /dev/null
+++ b/include/logger.hpp
@@ -0,0 +1,3 @@
+#include "api.h"
+
+void print_vector(const std::vector<float>& vector, const char* name);
diff --git a/src/drivetrain.cpp b/src/drivetrain.cpp
index 7373af9..702e4c3 100644
--- a/src/drivetrain.cpp
+++ b/src/drivetrain.cpp
@@ -9,6 +9,7 @@
 #include "drivetrain.hpp"
 #include "helper-functions.hpp"
 #include "structs.hpp"
+#include "logger.hpp"
 
 drivetrain::drivetrain(const wheels<std::reference_wrapper<pros::Motor>>& motors_,
                        const float wheelbase_length_,
@@ -201,41 +202,60 @@ void drivetrain::move_differential_robot_vels_ramsete(std::vector<differentialVe
 	}
 }
 
-void drivetrain::linear_mp(const float distance) {
+void drivetrain::linear_mp(const float distance, bool log) {
     float time = 0.f;
     bool start = true;
+	std::optional<std::vector<float>> linear_velocities;
+    if (log) {
+        linear_velocities.emplace();
+        linear_velocities->reserve(500);
+    }
+
     while(!LinearMP.profileFinished(time) || start) {
         float linear_velocity = LinearMP.velocity(time, distance);
+		if (linear_velocities) {
+			linear_velocities->push_back(linear_velocity);
+		}
 	    std::vector<differentialVels> desired_differential_vel = {{linear_velocity, 0.f}};
 	    move_differential_robot_vels(desired_differential_vel);
         pros::delay(static_cast<int>(timestep * 1000.f));
         time += timestep;
         start = false;
     }
-    motor_brakes();
-}
 
-void print_vector(const std::vector<float>& vector, const char* name) {
-	printf("%s = [", name);
-    for (size_t i = 0; i < vector.size(); ++i) {
-        printf("%.4f", vector[i]);
-        if (i + 1 < vector.size()) printf(",");
-    }
-    printf("]\n");	
+	if (linear_velocities) {
+		print_vector(*linear_velocities, "L");
+	}
+
+    motor_brakes();
 }
 
 void drivetrain::angular_mp(const float angle) {
     float time = 0.f;
     bool start = true;
+	std::optional<std::vector<float>> angular_velocities;
+    if (log) {
+        angular_velocities.emplace();
+        angular_velocities->reserve(500);
+    }
+
     while(!AngularMP.profileFinished(time) || start) {
         float angular_velocity = AngularMP.velocity(time, angle);
+		if (angular_velocities) {
+			angular_velocities->push_back(linear_velocity);
+		}
 	    std::vector<differentialVels> desired_differential_vel = {{0.f, angular_velocity}};
 	    move_differential_robot_vels(desired_differential_vel);
         pros::delay(timestep * 1000.f);
         time += timestep;
         start = false;
     }
-    motor_brakes();
+	
+	if (angular_velocities) {
+		print_vector(*angular_velocities, "L");
+	}
+    
+	motor_brakes();
 }
 
 void drivetrain::calculate_and_print_motor_constants() {
diff --git a/src/logger.cpp b/src/logger.cpp
new file mode 100644
index 0000000..d5e135c
--- /dev/null
+++ b/src/logger.cpp
@@ -0,0 +1,10 @@
+#include "logger.hpp"
+
+void print_vector(const std::vector<float>& vector, const char* name) {
+	printf("%s = [", name);
+    for (size_t i = 0; i < vector.size(); ++i) {
+        printf("%.4f", vector[i]);
+        if (i + 1 < vector.size()) printf(",");
+    }
+    printf("]\n");	
+}

From d5cde9e29ada55eddf5c1eb4ad4a1ddeb5e7ea2c Mon Sep 17 00:00:00 2001
From: Serrial Error <serrialerror@outlook.com>
Date: Mon, 12 Jan 2026 20:11:27 -0800
Subject: [PATCH 2/2] fixed compilation errors

---
 include/drivetrain.hpp | 2 +-
 src/drivetrain.cpp     | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/include/drivetrain.hpp b/include/drivetrain.hpp
index 8efbeae..66dbff5 100644
--- a/include/drivetrain.hpp
+++ b/include/drivetrain.hpp
@@ -357,7 +357,7 @@ class drivetrain {
 	 *
 	 * @param[in] angle Angle to rotate in radians (positive = counterclockwise, negative = clockwise)
 	 */
-	void angular_mp(const float angle);
+	void angular_mp(const float angle, bool log = false);
 	
 	/**
 	 * @brief Moves to a target pose using motion profiles
diff --git a/src/drivetrain.cpp b/src/drivetrain.cpp
index 702e4c3..b8c04ba 100644
--- a/src/drivetrain.cpp
+++ b/src/drivetrain.cpp
@@ -230,7 +230,7 @@ void drivetrain::linear_mp(const float distance, bool log) {
     motor_brakes();
 }
 
-void drivetrain::angular_mp(const float angle) {
+void drivetrain::angular_mp(const float angle, bool log) {
     float time = 0.f;
     bool start = true;
 	std::optional<std::vector<float>> angular_velocities;
@@ -242,7 +242,7 @@ void drivetrain::angular_mp(const float angle) {
     while(!AngularMP.profileFinished(time) || start) {
         float angular_velocity = AngularMP.velocity(time, angle);
 		if (angular_velocities) {
-			angular_velocities->push_back(linear_velocity);
+			angular_velocities->push_back(angular_velocity);
 		}
 	    std::vector<differentialVels> desired_differential_vel = {{0.f, angular_velocity}};
 	    move_differential_robot_vels(desired_differential_vel);