From 4f45243e4bbc9dc0f5f6f5ce8c766a195e721774 Mon Sep 17 00:00:00 2001
From: huttongrabiel <huttonthomas@icloud.com>
Date: Sat, 6 Jan 2024 12:54:22 -0800
Subject: [PATCH] pretty much working + squash me

---
 src/autonomous/AutonomousTask.cpp      |  6 ++++--
 src/network/MissionControlProtocol.cpp | 14 ++++++++------
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/autonomous/AutonomousTask.cpp b/src/autonomous/AutonomousTask.cpp
index 994e4fe8..590c0761 100644
--- a/src/autonomous/AutonomousTask.cpp
+++ b/src/autonomous/AutonomousTask.cpp
@@ -1,7 +1,6 @@
 #include "AutonomousTask.h"
 
 #include "../Constants.h"
-#include "../Globals.h"
 #include "../commands/DriveToWaypointCommand.h"
 #include "../world_interface/world_interface.h"
 
@@ -31,6 +30,8 @@ void AutonomousTask::navigate() {
 	commands::DriveToWaypointCommand cmd(_waypoint_coords, THETA_KP, DRIVE_VEL, SLOW_DRIVE_VEL,
 										 DONE_THRESHOLD, CLOSE_TO_TARGET_DUR_VAL);
 
+    DiffDriveKinematics diffDriveKinematics(Constants::EFF_WHEEL_BASE);
+
 	auto sleepUntil = std::chrono::steady_clock().now();
 	while (!cmd.isDone()) {
 		auto latestGPS = robot::readGPS();
@@ -41,7 +42,8 @@ void AutonomousTask::navigate() {
 			navtypes::pose_t latestPos(gpsData.x(), gpsData.y(), latestHeading.getData());
 			cmd.setState(latestPos, robot::types::dataclock::now());
 			commands::command_t output = cmd.getOutput();
-			robot::setCmdVel(output.thetaVel, output.xVel);
+            auto scaledVels = diffDriveKinematics.ensureWithinWheelSpeedLimit(DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, output.xVel, output.thetaVel, Constants::MAX_WHEEL_VEL);
+			robot::setCmdVel(scaledVels(2), scaledVels(0));
 		}
 
 		std::unique_lock autonomousTaskLock(_autonomous_task_mutex);
diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp
index c735787c..25a1774e 100644
--- a/src/network/MissionControlProtocol.cpp
+++ b/src/network/MissionControlProtocol.cpp
@@ -227,15 +227,16 @@ void MissionControlProtocol::sendRoverPos() {
 static bool validateWaypointNavRequest(const json& j) {
 	return util::validateKey(j, "latitude", val_t::string) &&
 		   util::validateKey(j, "longitude", val_t::string);
-	util::validateKey(j, "isApproximate", val_t::boolean) &&
-		util::validateKey(j, "isGate", val_t::boolean);
+    // FIXME: Add these back in
+//	util::validateKey(j, "isApproximate", val_t::boolean) &&
+//		util::validateKey(j, "isGate", val_t::boolean);
 }
 
 void MissionControlProtocol::handleWaypointNavRequest(const json& j) {
 	std::string latitude = j["latitude"];
 	std::string longitude = j["longitude"];
-	bool isApproximate = j["isApproximate"];
-	bool isGate = j["isGate"];
+	//bool isApproximate = j["isApproximate"];
+	//bool isGate = j["isGate"];
 
 	if (Globals::AUTONOMOUS) { // && !isApproximate && !isGate) {
 		navtypes::point_t waypointCoords(std::stod(latitude), std::stod(longitude), 1);
@@ -419,8 +420,9 @@ MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
 		validateCameraStreamCloseRequest);
 	this->addMessageHandler(
 		WAYPOINT_NAV_REQ_TYPE,
-		std::bind(&MissionControlProtocol::handleWaypointNavRequest, this, _1),
-		validateWaypointNavRequest);
+		std::bind(&MissionControlProtocol::handleWaypointNavRequest, this, _1));
+        // TODO: Add this back in
+		//validateWaypointNavRequest);
 	this->addConnectionHandler(std::bind(&MissionControlProtocol::handleConnection, this));
 	this->addDisconnectionHandler(
 		std::bind(&MissionControlProtocol::stopAndShutdownPowerRepeat, this, false));