From 1c51dc6e41bfe6d54e5e0e7531c2370938fd0f6f Mon Sep 17 00:00:00 2001
From: Adam Goldsmith <adam@adamgoldsmith.name>
Date: Sat, 5 Mar 2016 13:44:17 -0500
Subject: [PATCH] Basic auto mode

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

diff --git a/Robot2016/src/Robot.cpp b/Robot2016/src/Robot.cpp
index 6ea8ff4..d66a28d 100644
--- a/Robot2016/src/Robot.cpp
+++ b/Robot2016/src/Robot.cpp
@@ -44,6 +44,7 @@ private:
 	RobotDrive drive;
 	Shooter shooter;
 	Joystick driver_stick, operator_stick;
+	Timer auto_clock;
 
 	// instance variables
 	bool pickupRunning;  // don't want to spam the Talon with set messages.  Toggle the pickup when a button is pressed or released.
@@ -55,6 +56,15 @@ private:
 	bool arcade;
 	float shooter_power;
 
+	enum auto_states
+	{
+		START,
+		DRIVING_FORWARD,
+		STOP
+	};
+
+	auto_states auto_status;
+
 	LiveWindow *lw = LiveWindow::GetInstance();
 	SendableChooser *chooser;
 	const std::string autoNameDefault = "Default";
@@ -218,16 +228,45 @@ public:
 			//Custom Auto goes here
 		} else {
 			//Default Auto goes here
+			auto_status = START;
 		}
 	}
 
 	void AutonomousPeriodic()
 	{
+		const float drivePower = 0.7;
 		LogCSVData();
 		if(autoSelected == autoNameCustom){
 			//Custom Auto goes here
 		} else {
 			//Default Auto goes here
+			switch (auto_status)
+			{
+				case START:
+				{
+					auto_clock.Start();
+					auto_status = DRIVING_FORWARD;
+					break;
+				}
+				case DRIVING_FORWARD:
+				{
+					if (auto_clock.Get() > 5.0 )
+					{
+						drive.TankDrive(0.0, 0.0);
+						auto_status = STOP;
+					}
+					else
+					{
+						drive.TankDrive(drivePower, drivePower);
+					}
+					break;
+				}
+				case STOP:
+				{
+					std::cout << "All done!\n" ;
+					break;
+				}
+			}
 		}
 	}