From 5710edfadd5f7b2adafeda5fc62889ec65be1a56 Mon Sep 17 00:00:00 2001
From: Aidan Ferguson <aidanlferguson@gmail.com>
Date: Wed, 17 Feb 2016 15:22:32 -0500
Subject: [PATCH] It now only twists when the thumb button is pressed and I
 added the beginnings of an UpdateDrive method.

---
 src/Robot.cpp | 41 +++++++++++++++++++++++++++++++++++++++--
 1 file changed, 39 insertions(+), 2 deletions(-)

diff --git a/src/Robot.cpp b/src/Robot.cpp
index 1873305..70fb409 100644
--- a/src/Robot.cpp
+++ b/src/Robot.cpp
@@ -1,5 +1,6 @@
 #include "WPILib.h"
 #include "Shooter.h"
+#include <cmath>
 
 #ifndef BUTTON_LAYOUT
 #define BUTTON_LAYOUT
@@ -41,6 +42,8 @@ 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;
 
 	LiveWindow *lw = LiveWindow::GetInstance();
 	SendableChooser *chooser;
@@ -106,9 +109,16 @@ private:
 
 	void TeleopPeriodic()
 	{
-		drive.ArcadeDrive(&driver_stick, true);
+		if (driver_stick.GetRawButton(THUMB))
+		{
+			left_drive.Set(driver_stick.GetThrottle() * driver_stick.GetTwist);
+			right_drive.Set(-1 * driver_stick.GetThrottle() * driver_stick.GetTwist);
+		}
+		else
+		{
+			UpdateDrive();
+		}
 
-		//  This is shit code for testing.  Replace it with real code.
 		if(operator_stick.GetRawButton(RAMP_RAISE))
 		{
 			ramp.Set(1);
@@ -152,6 +162,33 @@ private:
 	{
 		lw->Run();
 	}
+
+	void UpdateDrive()
+	{
+		//regenerating theta
+		float x = driver_stick.GetX();
+		float y = driver_stick.GetY();
+		if (x < 0)
+		{
+			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)
+			{
+				left_drive.Set(1);
+				right_drive.Set()
+			}
+		}
+	}
 };
 
 START_ROBOT_CLASS(Robot)