From 5b47a61a78c15234ba3580e60ad761c366c737a8 Mon Sep 17 00:00:00 2001
From: Aidan Ferguson <aidanlferguson@gmail.com>
Date: Fri, 19 Feb 2016 15:47:52 -0500
Subject: [PATCH] Updated for simpler picewise function.

---
 src/Robot.cpp | 60 +++++++--------------------------------------------
 1 file changed, 8 insertions(+), 52 deletions(-)

diff --git a/src/Robot.cpp b/src/Robot.cpp
index ed81880..9828e3e 100644
--- a/src/Robot.cpp
+++ b/src/Robot.cpp
@@ -1,6 +1,5 @@
 #include "WPILib.h"
 #include "Shooter.h"
-#include <cmath>
 
 #ifndef BUTTON_LAYOUT
 #define BUTTON_LAYOUT
@@ -42,8 +41,7 @@ private:
 	// instance variables
 	bool pickupRunning;  // don't want to spam the Talon with set messages.  Toggle the pickup when a button is pressed or released.
 	bool inverting;
-	float theta;
-	float old_theta;
+	float oldXY;
 
 	LiveWindow *lw = LiveWindow::GetInstance();
 	SendableChooser *chooser;
@@ -165,62 +163,20 @@ private:
 
 	void UpdateDrive()
 	{
-		//regenerating theta
 		float x = driver_stick.GetX();
 		float y = driver_stick.GetY();
-		if (x < 0)
+		if ((x + y) != oldXY)
 		{
-			theta = atan(y / x) * -1;
-		}
-		else
-		{
-			theta = atan(y / x);
-		}
-		theta = theta / 6.283185307;	// TODO: This math is optimized for humans. Fix it after initial debugging.
-		theta = theta + 1;	// I know this is a lot of steps, but I want it to be really readable, because there are at least two stupid mistakes I've yet to find.
-		if (theta != old_theta)
-		{
-			//actually update motors
-			old_theta = theta;
-			if (theta < 0.125)
+			oldXY = x + y;
+			if (x > 0)
 			{
-				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());
+				left_drive.Set(y * driver_stick.GetThrottle());
+				right_drive.Set((1-x)*y * 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());
+				right_drive.Set(y * driver_stick.GetThrottle());
+				left_drive.Set((1+x)*y * driver_stick.GetThrottle());
 			}
 		}
 	}