From 24844d9086b90ca34106699f95671aa9e94b66ae Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Fri, 13 Feb 2015 17:34:38 -0500
Subject: [PATCH] Worked on autonomous

---
 Commands/Autonomous/AutoDrive.cpp  |  4 +--
 Commands/Autonomous/AutoDrive.h    |  2 +-
 Commands/Autonomous/Autonomous.cpp | 50 ++++++++++++++++++++++++------
 Commands/Autonomous/Autonomous.h   |  2 +-
 DentRobot.cpp                      |  4 ++-
 5 files changed, 47 insertions(+), 15 deletions(-)

diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index 4d46525..df147d7 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -1,9 +1,9 @@
 #include "AutoDrive.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-AutoDrive::AutoDrive() : Command("AutoDrive"){
+AutoDrive::AutoDrive(double wait) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(0.5);
+  SetTimeout(wait);
 }
 void AutoDrive::Initialize(){
 }
diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h
index 26f2aa3..d84f33f 100644
--- a/Commands/Autonomous/AutoDrive.h
+++ b/Commands/Autonomous/AutoDrive.h
@@ -8,7 +8,7 @@
 
 class AutoDrive: public Command{
   public:
-    AutoDrive();
+    AutoDrive(double);
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index 0d0df58..55b62d2 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -7,15 +7,45 @@
 #include "../Collector/CloseCollector.h"
 #include "../Collector/OpenCollector.h"
 #include "../Collector/CollectTote.h"
-Autonomous::Autonomous(){
-  AddSequential(new Raise());
-  AddSequential(new Lower());
-  AddParallel(new OpenCollector());
-  AddParallel(new CloseCollector());
-  AddSequential(new Turn());
-  AddParallel(new AutoDrive());
-  AddParallel(new CloseCollector());
-  AddParallel(new CollectTote());
-  AddSequential(new Turn());
+Autonomous::Autonomous(int seq){
+  SmartDashboard::GetNumber("Auto Wait Time");
+  switch(seq){
+    case 0:
+      // Just for testing
+      AddSequential(new Raise());
+      AddSequential(new Lower());
+      AddSequential(new OpenCollector());
+      AddSequential(new CloseCollector());
+      AddSequential(new Turn());
+      AddParallel(new AutoDrive(0.5));
+      AddParallel(new CloseCollector());
+      AddSequential(new CollectTote());
+      AddSequential(new Turn());
+      break;
+    case 1:
+      // Drive forward a bit, turn around, collect tote then bin
+      AddParallel(new Raise());
+      AddSequential(new AutoDrive(0.5));
+      AddSequential(new Turn());
+      AddSequential(new Turn());
+      AddSequential(new CollectTote());
+      break;
+    case 2:
+      // Drive forward a bit, turn around, collect tote then bin
+      AddParallel(new Raise());
+      AddParallel(new AutoDrive(0.5));
+      AddSequential(new Turn());
+      AddSequential(new Turn());
+      AddSequential(new CollectTote());
+      break;
+    case 3:
+      // Wait a desigated value, drive to Auto Zone (TM)
+      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      AddSequential(new AutoDrive(2.0));
+      break;
+    default:
+      printf("Invalid seq: %d\n", seq);
+      break;
+  }
 }
 // vim: ts=2:sw=2:et
diff --git a/Commands/Autonomous/Autonomous.h b/Commands/Autonomous/Autonomous.h
index 43ca69c..f5786e2 100644
--- a/Commands/Autonomous/Autonomous.h
+++ b/Commands/Autonomous/Autonomous.h
@@ -8,7 +8,7 @@
 
 class Autonomous: public CommandGroup{
   public:
-    Autonomous();
+    Autonomous(int);
 };
 #endif
 // vim: ts=2:sw=2:et
diff --git a/DentRobot.cpp b/DentRobot.cpp
index e801009..cd65e54 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -13,9 +13,11 @@ DentRobot::DentRobot(){
   drivetrain=new Drivetrain();
   elevator=new Elevator();
   binElevator=new BinElevator();
-  aut=new Autonomous();
+  aut=new Autonomous(0);
   CameraServer::GetInstance()->SetQuality(25);
   CameraServer::GetInstance()->StartAutomaticCapture("cam0");
+  SmartDashboard::PutNumber("Auto Wait Time", 1.0);
+  SmartDashboard::PutNumber("Auto Sequence", 0);
   printf("Initialized");
 }
 void DentRobot::RobotInit(){