Worked on autonomous

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(){
 }

Commands/Autonomous/AutoDrive.h

@@ -8,7 +8,7 @@
 
 class AutoDrive: public Command{
   public:
-    AutoDrive();
+    AutoDrive(double);
     void Initialize();
     void Execute();
     bool IsFinished();

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

Commands/Autonomous/Autonomous.h

@@ -8,7 +8,7 @@
 
 class Autonomous: public CommandGroup{
   public:
-    Autonomous();
+    Autonomous(int);
 };
 #endif
 // vim: ts=2:sw=2:et

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(){