From 65f7795023aad68a3231e5763ea8d84d3b2b58af Mon Sep 17 00:00:00 2001
From: Adam Goldsmith <adam@adamgoldsmith.name>
Date: Sat, 12 Mar 2016 10:21:18 -0500
Subject: [PATCH] Simplify driving code

---
 Robot2016/src/Robot.cpp | 60 ++++++++++++++++++-----------------------
 1 file changed, 26 insertions(+), 34 deletions(-)

diff --git a/Robot2016/src/Robot.cpp b/Robot2016/src/Robot.cpp
index 6da8c07..8668d09 100644
--- a/Robot2016/src/Robot.cpp
+++ b/Robot2016/src/Robot.cpp
@@ -183,19 +183,35 @@ private:
 
 	void UpdateDrive()
 	{
-		float x = -driver_stick.GetX();
-		float y = -driver_stick.GetY();
-		if (x > 0)
+		// Enable arcade mode if button 7 is pressed, disable if
+		// button 8 is pressed
+		arcade = (driver_stick.GetRawButton(7) || arcade) && !driver_stick.GetRawButton(8);
+		if (arcade)
 		{
-			float right = y * SaneThrottle(driver_stick.GetThrottle());
-			float left = (1-x)*y * SaneThrottle(driver_stick.GetThrottle());
+			drive.ArcadeDrive(driver_stick);
+		}
+		else if (driver_stick.GetRawButton(THUMB)) //Turning
+		{
+			float left = driver_stick.GetTwist();
+			float right = -driver_stick.GetTwist();
 			drive.TankDrive(left, right);
 		}
-		else
+		else //Normal Drive
 		{
-			float left = y * SaneThrottle(driver_stick.GetThrottle());
-			float right = (1+x)*y * SaneThrottle(driver_stick.GetThrottle());
-			drive.TankDrive(left, right);
+			float x = -driver_stick.GetX();
+			float y = -driver_stick.GetY();
+			if (x > 0)
+			{
+				float right = y * SaneThrottle(driver_stick.GetThrottle());
+				float left = (1-x)*y * SaneThrottle(driver_stick.GetThrottle());
+				drive.TankDrive(left, right);
+			}
+			else
+			{
+				float left = y * SaneThrottle(driver_stick.GetThrottle());
+				float right = (1+x)*y * SaneThrottle(driver_stick.GetThrottle());
+				drive.TankDrive(left, right);
+			}
 		}
 	}
 
@@ -304,34 +320,10 @@ public:
 	void TeleopPeriodic()
 	{
 		LogCSVData();
+		UpdateDrive();
 		std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
 		std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
 
-		if(driver_stick.GetRawButton(7))
-		{
-			arcade = true;
-		}
-		if(driver_stick.GetRawButton(8))
-		{
-			arcade = false;
-		}
-		if (arcade)
-		{
-			drive.ArcadeDrive(driver_stick);
-		}
-		else
-		{
-			if (driver_stick.GetRawButton(THUMB))
-			{
-				float left = driver_stick.GetTwist();
-				float right = -driver_stick.GetTwist();
-				drive.TankDrive(left, right);
-			}
-			else
-			{
-				UpdateDrive();
-			}
-		}
 		//bool rampDoing = false;
 		//  This is shit code for testing.  Replace it with real code.
 		if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))