From d92afcde6ef9f81d9439fe98d7c312917b6f9fd1 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Fri, 13 Feb 2015 22:30:15 -0500
Subject: [PATCH] Worked on auto. Fixed the bug of the century: robot could not
 collect and close collector at the same time

---
 Commands/Autonomous/AutoDrive.cpp     |  1 +
 Commands/Autonomous/Autonomous.cpp    | 26 ++++++++++++++------------
 Commands/Autonomous/Turn.cpp          |  2 ++
 Commands/Collector/CloseCollector.cpp |  3 +--
 Commands/Collector/CollectTote.cpp    |  7 ++++---
 Commands/Collector/RollIn.cpp         |  1 -
 DentRobot.cpp                         |  6 +++---
 7 files changed, 25 insertions(+), 21 deletions(-)

diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index f279f03..0636003 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -25,6 +25,7 @@ bool AutoDrive::IsFinished(){
   return IsTimedOut();
 }
 void AutoDrive::End(){
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
 }
 void AutoDrive::Interrupted(){
   End();
diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index 1201f93..1ee23c4 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -7,28 +7,30 @@
 #include "../Collector/CloseCollector.h"
 #include "../Collector/OpenCollector.h"
 #include "../Collector/RollIn.h"
+#include "../Collector/CollectTote.h"
 Autonomous::Autonomous(int seq){
-  SmartDashboard::GetNumber("Auto Wait Time");
+  //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 RollIn());
-      AddSequential(new Turn());
+      AddSequential(new CollectTote());
+      //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 RollIn());
+      //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 RollIn());
+      AddSequential(new Raise());
       break;
     case 2:
       // Drive forward a bit, turn around, collect tote then bin
@@ -40,7 +42,7 @@ Autonomous::Autonomous(int seq){
       break;
     case 3:
       // Wait a desigated value, drive to Auto Zone (TM)
-      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      //Wait(SmartDashboard::GetNumber("Auto Wait Time"));
       AddSequential(new AutoDrive(2.0));
       break;
     default:
diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp
index 174f6cc..b42bd40 100644
--- a/Commands/Autonomous/Turn.cpp
+++ b/Commands/Autonomous/Turn.cpp
@@ -15,6 +15,8 @@ bool Turn::IsFinished(){
   return IsTimedOut();
 }
 void Turn::End(){
+  // Stop driving
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
 }
 void Turn::Interrupted(){
   End();
diff --git a/Commands/Collector/CloseCollector.cpp b/Commands/Collector/CloseCollector.cpp
index 2418732..b3a473d 100644
--- a/Commands/Collector/CloseCollector.cpp
+++ b/Commands/Collector/CloseCollector.cpp
@@ -1,10 +1,9 @@
 #include "CloseCollector.h"
 CloseCollector::CloseCollector() : Command("CloseCollector"){
-  Requires(DentRobot::collector);
 }
 void CloseCollector::Initialize(){
   printf("Initialized collector: 0.5\n");
-  SetTimeout(2.5);
+  SetTimeout(.5);
 }
 void CloseCollector::Execute(){
   //printf("Closing collector: -0.5f\n");
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
index 88b078b..141738c 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/CollectTote.cpp
@@ -4,8 +4,9 @@
 #include "RollIn.h"
 #include "CloseCollector.h"
 CollectTote::CollectTote(){
-  AddParallel(new RollIn());
-  AddParallel(new AutoDrive(0.5, -0.75));
-  AddSequential(new CloseCollector());
+  AddParallel(new CloseCollector());
+  AddSequential(new RollIn());
+  // TODO: Fix null error
+  //AddParallel(new AutoDrive(0.5, -0.75));
 }
 // vim: ts=2:sw=2:et
diff --git a/Commands/Collector/RollIn.cpp b/Commands/Collector/RollIn.cpp
index 68c49b8..e22e12f 100644
--- a/Commands/Collector/RollIn.cpp
+++ b/Commands/Collector/RollIn.cpp
@@ -1,6 +1,5 @@
 #include "RollIn.h"
 RollIn::RollIn() : Command("RollIn"){
-  Requires(DentRobot::collector);
 }
 void RollIn::Initialize(){
   printf("Initialized RollIn\n");
diff --git a/DentRobot.cpp b/DentRobot.cpp
index cd65e54..875799e 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -16,12 +16,12 @@ DentRobot::DentRobot(){
   aut=new Autonomous(0);
   CameraServer::GetInstance()->SetQuality(25);
   CameraServer::GetInstance()->StartAutomaticCapture("cam0");
-  SmartDashboard::PutNumber("Auto Wait Time", 1.0);
-  SmartDashboard::PutNumber("Auto Sequence", 0);
+  //SmartDashboard::PutNumber("Auto Wait Time", 1.0);
+  //SmartDashboard::PutNumber("Auto Sequence", 0);
   printf("Initialized");
 }
 void DentRobot::RobotInit(){
-  SmartDashboard::PutNumber("CodeVersion",0.001);
+  //SmartDashboard::PutNumber("CodeVersion",0.001);
 }
 void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();