From 45bda434a973731678f1fd282d97507825f41140 Mon Sep 17 00:00:00 2001
From: Adam Goldsmith <adam@adamgoldsmith.name>
Date: Sat, 5 Mar 2016 14:28:32 -0500
Subject: [PATCH] Add real TestPeriodic

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

diff --git a/Robot2016/src/Robot.cpp b/Robot2016/src/Robot.cpp
index 6303a61..bcd86dd 100644
--- a/Robot2016/src/Robot.cpp
+++ b/Robot2016/src/Robot.cpp
@@ -435,7 +435,147 @@ public:
 
 	void TestPeriodic()
 	{
-		lw->Run();
+		const int driveTime = 5; //Drive for 5 seconds
+		const float drivePower = 0.5, armPower=.35, rampPower=.4; //Arbitrary powers to test at
+		static Timer t;
+		enum test_stages {
+			INIT,
+			ARMS_UP,
+			ARMS_DOWN,
+			RAMP_UP,
+			RAMP_DOWN,
+			DRIVE_FORWARD,
+			DRIVE_BACKWARD,
+			TURN_CW,
+			TURN_CCW,
+			END};
+
+		static enum test_stages test_stage, old_test_stage;
+
+		if (old_test_stage != test_stage)
+		{
+			//Reset timer between stages
+			t.Stop();
+			t.Reset();
+
+			//Wait for a button press between stages
+			if (operator_stick.GetRawButton(TRIGGER))
+				old_test_stage = test_stage;
+		}
+		else
+		{
+			if(!t.Get()) t.Start();
+
+			switch (test_stage)
+			{
+			case INIT:
+			{
+				break;
+			}
+
+			case ARMS_UP:
+			{
+				if (arms.GetForwardLimitOK())
+					arms.Set(1);
+				else
+				{
+					arms.Set(0);
+					test_stage = ARMS_DOWN;
+				}
+				break;
+			}
+
+			case ARMS_DOWN:
+			{
+				if (arms.GetReverseLimitOK())
+					arms.Set(-1);
+				else
+				{
+					arms.Set(0);
+					test_stage = RAMP_UP;
+				}
+				break;
+			}
+
+			case RAMP_UP:
+			{
+				if (arms.GetForwardLimitOK())
+					ramp.Set(1);
+				else
+				{
+					ramp.Set(0);
+					test_stage = RAMP_DOWN;
+				}
+				break;
+			}
+
+			case RAMP_DOWN:
+			{
+				if (arms.GetReverseLimitOK())
+					ramp.Set(-1);
+				else
+				{
+					ramp.Set(0);
+					test_stage = DRIVE_FORWARD;
+				}
+				break;
+			}
+
+			case DRIVE_FORWARD:
+			{
+				if (t.Get() < driveTime)
+					drive.TankDrive(drivePower, drivePower);
+				else
+				{
+					drive.TankDrive(0.0, 0.0);
+					test_stage = DRIVE_BACKWARD;
+				}
+				break;
+			}
+
+			case DRIVE_BACKWARD:
+			{
+				if (t.Get() < driveTime)
+					drive.TankDrive(-drivePower, -drivePower);
+				else
+				{
+					drive.TankDrive(0.0, 0.0);
+					test_stage = TURN_CW;
+				}
+				break;
+			}
+
+			case TURN_CW:
+			{
+				if (t.Get() < driveTime)
+					drive.TankDrive(drivePower, -drivePower);
+				else
+				{
+					drive.TankDrive(0.0, 0.0);
+					test_stage = TURN_CCW;
+				}
+				break;
+			}
+
+			case TURN_CCW:
+			{
+				if (t.Get() < driveTime)
+					drive.TankDrive(-drivePower, drivePower);
+				else
+				{
+					drive.TankDrive(0.0, 0.0);
+					test_stage = END;
+				}
+				break;
+			}
+
+			case END:
+			{
+				break;
+			}
+			}
+		}
+
 		LogCSVData();
 	}
 };