From 4392dd348eb0eb3407929d0198ead6b2f8be83a3 Mon Sep 17 00:00:00 2001 From: Aidan Ferguson Date: Wed, 17 Feb 2016 16:39:08 -0500 Subject: [PATCH] Well, ... it's a rough draft, but it SHOULD match the behavior requested. --- src/Robot.cpp | 43 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 39 insertions(+), 4 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index 70fb409..ed81880 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -111,8 +111,8 @@ private: { if (driver_stick.GetRawButton(THUMB)) { - left_drive.Set(driver_stick.GetThrottle() * driver_stick.GetTwist); - right_drive.Set(-1 * driver_stick.GetThrottle() * driver_stick.GetTwist); + left_drive.Set(driver_stick.GetThrottle() * driver_stick.GetTwist()); + right_drive.Set(-1 * driver_stick.GetThrottle() * driver_stick.GetTwist()); } else { @@ -184,8 +184,43 @@ private: old_theta = theta; if (theta < 0.125) { - left_drive.Set(1); - right_drive.Set() + left_drive.Set(1 * driver_stick.GetThrottle()); + right_drive.Set((0.25 + 0.25 * 8 * theta) * driver_stick.GetThrottle()); + } + else if (theta < 0.25) + { + left_drive.Set(1 * driver_stick.GetThrottle()); + right_drive.Set((0.5 + 0.5 * 8 * (theta - 0.125)) * driver_stick.GetThrottle()); + } + else if (theta < .375) + { + left_drive.Set((1 - 0.5 * 8 * (theta - 0.25)) * driver_stick.GetThrottle()); + right_drive.Set(1 * driver_stick.GetThrottle()); + } + else if (theta < 0.5) + { + left_drive.Set((0.5 - 0.25 * 8 * (theta - 0.375)) * driver_stick.GetThrottle()); + right_drive.Set(1 * driver_stick.GetThrottle()); + } + else if (theta < 0.625) + { + left_drive.Set((0.25 - 0.25 * 8 * (theta - 0.5)) * driver_stick.GetThrottle()); + right_drive.Set((1 - 8 * (theta - 0.5)) * driver_stick.GetThrottle()); + } + else if (theta < 0.75) + { + left_drive.Set(-8 * (theta - 0.625) * driver_stick.GetThrottle()); + right_drive.Set(-8 * (theta - 0.625) * driver_stick.GetThrottle()); + } + else if (theta < 0.875) + { + left_drive.Set((8 * (theta - 0.75) - 1) * driver_stick.GetThrottle()); + right_drive.Set((8 * (theta - 0.75) - 1) * driver_stick.GetThrottle()); + } + else + { + left_drive.Set(8 * (theta - 0.75) * driver_stick.GetThrottle()); + right_drive.Set(0.25 * 8 * (theta - 0.75) * driver_stick.GetThrottle()); } } }