Basic auto mode

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;
+				}
+			}
 		}
 	}