From 97c5182e889c01f8c95824bc4679844a17d6f7d1 Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Thu, 12 Feb 2015 17:56:09 +0000
Subject: [PATCH 01/18] Updated makefile to create bin directory if it doesn't
 exist

---
 Makefile | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/Makefile b/Makefile
index 0eee4bc..9bf4a49 100644
--- a/Makefile
+++ b/Makefile
@@ -9,6 +9,7 @@ EXEC=bin/FRCUserProgram
 CLEANSER=rm -r
 
 all : $(OBJECTS)
+	if [ ! -d bin ];then mkdir bin; fi
 	$(CC) -L$(WPILIB)/lib $(LDFLAGS) -o $(EXEC) $(OBJECTS) -lwpi
 
 %.o : %.cpp
@@ -17,10 +18,10 @@ all : $(OBJECTS)
 clean:
 	$(CLEANSER) $(OBJECTS) bin/FRCUserProgram
 
-deploy:
+deploy: all
 	@cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) 'cat > /home/lvuser/FRCUserProgram2&&rm /home/lvuser/FRCUserProgram;mv /home/lvuser/FRCUserProgram2 /home/lvuser/FRCUserProgram&&. /etc/profile.d/natinst-path.sh;chmod a+x /home/lvuser/FRCUserProgram'
 
-debug:
+debug: all
 	@cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) 'cat > /home/lvuser/FRCUserProgram2&&rm /home/lvuser/FRCUserProgram;mv /home/lvuser/FRCUserProgram2 /home/lvuser/FRCUserProgram&&. /etc/profile.d/natinst-path.sh;chmod a+x /home/lvuser/FRCUserProgram;/home/lvuser/run.sh'
 
 putkey:

From 09fe0ca9c657e145d7657dea342121adba18d954 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Thu, 12 Feb 2015 21:18:48 -0500
Subject: [PATCH 02/18] Removed HH* files, changed auto, implemented encoder
 (untested)

---
 Commands/Autonomous/Autonomous.cpp |  5 ++--
 Commands/Elevator/Raise.cpp        |  4 +++
 DentRobot.cpp                      | 12 ++++++--
 HHBase.cpp                         | 28 ------------------
 HHBase.h                           | 26 -----------------
 HHRobot.cpp                        | 46 ------------------------------
 HHRobot.h                          | 19 ------------
 OI.cpp                             |  4 +--
 OI.h                               |  2 ++
 Subsystems/Elevator.cpp            | 12 ++++++++
 Subsystems/Elevator.h              |  3 ++
 11 files changed, 34 insertions(+), 127 deletions(-)
 delete mode 100644 HHBase.cpp
 delete mode 100644 HHBase.h
 delete mode 100644 HHRobot.cpp
 delete mode 100644 HHRobot.h

diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index bc64654..0d0df58 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -8,11 +8,10 @@
 #include "../Collector/OpenCollector.h"
 #include "../Collector/CollectTote.h"
 Autonomous::Autonomous(){
-  //AddSequential(new Raise());
-  //AddSequential(new Lower());
-  //AddSequential(new AutoDrive());
   AddSequential(new Raise());
   AddSequential(new Lower());
+  AddParallel(new OpenCollector());
+  AddParallel(new CloseCollector());
   AddSequential(new Turn());
   AddParallel(new AutoDrive());
   AddParallel(new CloseCollector());
diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp
index 6f900b4..cd8885f 100644
--- a/Commands/Elevator/Raise.cpp
+++ b/Commands/Elevator/Raise.cpp
@@ -18,6 +18,10 @@ bool Raise::IsFinished(){
   }
 }
 void Raise::End(){
+  // If the elevator is at the top
+  if(DentRobot::elevator->GetElevatorTop()){
+    DentRobot::elevator->SetUseEncoder(true);
+  }
   DentRobot::elevator->Run(0.0f);
 }
 void Raise::Interrupted(){
diff --git a/DentRobot.cpp b/DentRobot.cpp
index f5eafec..e801009 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -1,4 +1,5 @@
 #include "DentRobot.h"
+#include "OI.h"
 #include "Commands/Autonomous/Autonomous.h"
 OI* DentRobot::oi=NULL;
 Collector* DentRobot::collector=NULL;
@@ -32,12 +33,17 @@ void DentRobot::AutonomousPeriodic(){
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::TeleopInit(){
-  //if (aut != NULL){
-  //  aut->Cancel();
-  //}
+  if (aut != NULL){
+    aut->Cancel();
+  }
 }
 void DentRobot::TeleopPeriodic(){
   Scheduler::GetInstance()->Run();
+  // TODO: Calibrate 1.0 to the height we want the elevator to automatically raise
+  if(elevator->GetUseEncoder()&&elevator->GetHeight()<=-1.0){
+    // Raise the elevator if it dips below elevatorTop
+    oi->raise->Start();
+  }
 }
 void DentRobot::TestPeriodic(){
 }
diff --git a/HHBase.cpp b/HHBase.cpp
deleted file mode 100644
index 0023bc3..0000000
--- a/HHBase.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-#include "HHBase.h"
-#include <iostream>
-#include <fstream>
-#include <stdexcept>
-#include <map>
-HHBase::HHBase():
-  hhbot(new HHRobot()){
-    printf("Done\n");
-  }
-void HHBase::RobotInit(){
-}
-void HHBase::DisabledInit(){}
-void HHBase::AutonomousInit(){
-}
-void HHBase::TeleopInit(){
-}
-void HHBase::DisabledContinuous(){}
-void HHBase::AutonomousContinuous(){}
-void HHBase::TeleopContinuous(){}
-void HHBase::DisabledPeriodic(){}
-void HHBase::AutonomousPeriodic(){
-}
-void HHBase::TeleopPeriodic(){
-  hhbot->Handler();
-}
-void HHBase::Test(){}
-START_ROBOT_CLASS(HHBase);
-// vim: ts=2:sw=2:et
diff --git a/HHBase.h b/HHBase.h
deleted file mode 100644
index c4e66e2..0000000
--- a/HHBase.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef __HH_BASE_H__
-#define __HH_BASE_H__
-#include <WPILib.h>
-#include <string>
-#include "HHRobot.h"
-//Because this is the first header to be included, classes need to be declared here
-class HHRobot;
-class HHBase : public IterativeRobot{
-  private:
-	HHRobot *hhbot;
-  public:
-    HHBase();
-    void RobotInit();
-    void DisabledInit();
-    void AutonomousInit();
-    void TeleopInit();
-    void DisabledContinuous();
-    void AutonomousContinuous();
-    void TeleopContinuous();
-    void DisabledPeriodic();
-    void AutonomousPeriodic();
-    void TeleopPeriodic();
-    void Test();
-};
-#endif
-// vim: ts=2:sw=2:et
diff --git a/HHRobot.cpp b/HHRobot.cpp
deleted file mode 100644
index 1020bc1..0000000
--- a/HHRobot.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-#include "HHRobot.h"
-#include "HHBase.h"
-HHRobot::HHRobot():
-  hhdrive(new RobotDrive(2,0,3,1)),
-  gyro(new Gyro(1)),
-  collector(new DentCollector(4, 5, 6, 7)),
-  driveStick(new Extreme3dPro(0)){
-    hhdrive->SetExpiration(0.1);
-    hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
-    hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
-  }
-void HHRobot::Init(){
-  printf("Initing\n");
-  printf("Code Version: %f\n",0000.1);
-  gyro->Reset();
-}
-//Main function used to handle periodic tasks on the robot
-void HHRobot::Handler(){
-  const float Kp = 0.3;
-  if(driveStick->GetJoystickButton(1)==1){
-    hhdrive->MecanumDrive_Cartesian(driveStick->GetJoystickAxis("z"), 0, driveStick->GetJoystickAxis("x"));
-  }else if(driveStick->GetJoystickButton(2)==1){
-    hhdrive->MecanumDrive_Cartesian(driveStick->GetJoystickAxis("z"), driveStick->GetJoystickAxis("y"), 0);
-  }else if(driveStick->GetJoystickButton(3)==1){
-    hhdrive->Drive(driveStick->GetJoystickAxis("y"), driveStick->GetJoystickAxis("y")*Kp*-gyro->GetAngle());
-  }else{
-    hhdrive->MecanumDrive_Cartesian(driveStick->GetJoystickAxis("z"), driveStick->GetJoystickAxis("y"), driveStick->GetJoystickAxis("x"));
-  }
-  if(driveStick->GetJoystickButton(11)==1){
-    collector->Collect(driveStick->GetThrottle());
-  }else if(driveStick->GetJoystickButton(12)==1){
-    collector->Collect(-driveStick->GetThrottle());
-  }else if(driveStick->GetJoystickButton(9)==1){
-    collector->Raise(driveStick->GetThrottle());
-  }else if(driveStick->GetJoystickButton(10)==1){
-    collector->Raise(-driveStick->GetThrottle());
-  }else{
-    collector->Rest();
-  }
-  SmartDashboard::PutNumber("hambone1", driveStick->GetThrottle());
-  SmartDashboard::PutNumber("hambone2", driveStick->GetJoystickAxis("joystick"));
-  SmartDashboard::PutNumber("hambone3", driveStick->GetRawJoystickAxis(3));
-  printf("hambone2: %f", driveStick->GetThrottle());
-  Wait(0.005);
-}
-// vim: ts=2:sw=2:et
diff --git a/HHRobot.h b/HHRobot.h
deleted file mode 100644
index f6dc004..0000000
--- a/HHRobot.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef __ROBOT_H__
-#define __ROBOT_H__
-#include <WPILib.h>
-#include "HHBase.h"
-#include "classes/Collector.h"
-#include "hhlib/input/controller/Joystick.h"
-class HHRobot{
-  private:
-    RobotDrive *hhdrive;
-    Gyro *gyro;
-    DentCollector *collector;
-    Extreme3dPro *driveStick;
-  public:
-    HHRobot();
-    void Init();
-    void Handler();
-};
-#endif
-// vim: ts=2:sw=2:et
diff --git a/OI.cpp b/OI.cpp
index 54c4212..e01e119 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -20,8 +20,8 @@ OI::OI() {
   left1->WhileHeld(new CollectTote());
   left2->WhileHeld(new ReleaseTote());
   // Elevator
-  Raise* raise=new Raise();
-  Lower* lower=new Lower();
+  raise=new Raise();
+  lower=new Lower();
   JoystickButton *right3=new JoystickButton(rightStick, 3);
   JoystickButton *right4=new JoystickButton(rightStick, 4);
   JoystickButton *right5=new JoystickButton(rightStick, 5);
diff --git a/OI.h b/OI.h
index 7f0929f..357dbda 100644
--- a/OI.h
+++ b/OI.h
@@ -2,6 +2,7 @@
 #define OI_H
 
 #include "WPILib.h"
+#include "Commands/Command.h"
 
 class OI
 {
@@ -11,6 +12,7 @@ class OI
     OI();
     Joystick* GetRightStick();
     Joystick* GetLeftStick();
+    Command *raise, *lower;
 };
 #endif
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp
index 9264b4a..ffe24c3 100644
--- a/Subsystems/Elevator.cpp
+++ b/Subsystems/Elevator.cpp
@@ -5,10 +5,16 @@ Elevator::Elevator(){
   elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
   elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
   elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
+  // Checks if the elevator is drifting
+  useEncoder=false;
 }
 void Elevator::InitDefaultCommand(){
 }
 void Elevator::Run(double power){
+  // If we're not telling it to stop
+  if(power != 0.0){
+    SetUseEncoder(false);
+  }
   motor->Set(power);
 }
 void Elevator::ResetEncoder(){
@@ -23,4 +29,10 @@ bool Elevator::GetElevatorBottom(){
 bool Elevator::GetElevatorTop(){
   return elevatorTop->Get();
 }
+void Elevator::SetUseEncoder(bool param){
+  useEncoder=param;
+}
+bool Elevator::GetUseEncoder(){
+  return useEncoder;
+}
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Elevator.h b/Subsystems/Elevator.h
index bdc0c8d..5454063 100644
--- a/Subsystems/Elevator.h
+++ b/Subsystems/Elevator.h
@@ -9,6 +9,7 @@ class Elevator{
     Encoder *elevatorEncoder;
     static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
     DigitalInput *elevatorBottom, *elevatorTop;
+    bool useEncoder;
   public:
     Elevator();
     void InitDefaultCommand();
@@ -17,6 +18,8 @@ class Elevator{
     double GetHeight();
     bool GetElevatorTop();
     bool GetElevatorBottom();
+    void SetUseEncoder(bool);
+    bool GetUseEncoder();
 };
 #endif
 // vim: ts=2:sw=2:et

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 03/18] 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(){

From dc593a328236fb16c732dc5241f64e1f391a06a9 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Fri, 13 Feb 2015 19:40:23 -0500
Subject: [PATCH 04/18] Renamed collector commands (untested)

---
 Commands/Autonomous/Autonomous.cpp               |  8 ++++----
 .../Collector/{CollectTote.cpp => RollIn.cpp}    | 16 ++++++++--------
 Commands/Collector/{CollectTote.h => RollIn.h}   |  8 ++++----
 .../Collector/{ReleaseTote.cpp => RollOut.cpp}   | 14 +++++++-------
 Commands/Collector/{ReleaseTote.h => RollOut.h}  |  8 ++++----
 OI.cpp                                           |  8 ++++----
 6 files changed, 31 insertions(+), 31 deletions(-)
 rename Commands/Collector/{CollectTote.cpp => RollIn.cpp} (57%)
 rename Commands/Collector/{CollectTote.h => RollIn.h} (72%)
 rename Commands/Collector/{ReleaseTote.cpp => RollOut.cpp} (64%)
 rename Commands/Collector/{ReleaseTote.h => RollOut.h} (72%)

diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index 55b62d2..1201f93 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -6,7 +6,7 @@
 #include "Turn.h"
 #include "../Collector/CloseCollector.h"
 #include "../Collector/OpenCollector.h"
-#include "../Collector/CollectTote.h"
+#include "../Collector/RollIn.h"
 Autonomous::Autonomous(int seq){
   SmartDashboard::GetNumber("Auto Wait Time");
   switch(seq){
@@ -19,7 +19,7 @@ Autonomous::Autonomous(int seq){
       AddSequential(new Turn());
       AddParallel(new AutoDrive(0.5));
       AddParallel(new CloseCollector());
-      AddSequential(new CollectTote());
+      AddSequential(new RollIn());
       AddSequential(new Turn());
       break;
     case 1:
@@ -28,7 +28,7 @@ Autonomous::Autonomous(int seq){
       AddSequential(new AutoDrive(0.5));
       AddSequential(new Turn());
       AddSequential(new Turn());
-      AddSequential(new CollectTote());
+      AddSequential(new RollIn());
       break;
     case 2:
       // Drive forward a bit, turn around, collect tote then bin
@@ -36,7 +36,7 @@ Autonomous::Autonomous(int seq){
       AddParallel(new AutoDrive(0.5));
       AddSequential(new Turn());
       AddSequential(new Turn());
-      AddSequential(new CollectTote());
+      AddSequential(new RollIn());
       break;
     case 3:
       // Wait a desigated value, drive to Auto Zone (TM)
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/RollIn.cpp
similarity index 57%
rename from Commands/Collector/CollectTote.cpp
rename to Commands/Collector/RollIn.cpp
index acabd4e..68c49b8 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/RollIn.cpp
@@ -1,22 +1,22 @@
-#include "CollectTote.h"
-CollectTote::CollectTote() : Command("CollectTote"){
+#include "RollIn.h"
+RollIn::RollIn() : Command("RollIn"){
   Requires(DentRobot::collector);
 }
-void CollectTote::Initialize(){
-  printf("Initialized CollectTote\n");
+void RollIn::Initialize(){
+  printf("Initialized RollIn\n");
   SetTimeout(2.0);
 }
-void CollectTote::Execute(){
+void RollIn::Execute(){
   //TODO check this value to move the motors in the right direction
   DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
-bool CollectTote::IsFinished(){
+bool RollIn::IsFinished(){
   return DentRobot::collector->BoxCollected()||IsTimedOut();
 }
-void CollectTote::End(){
+void RollIn::End(){
   DentRobot::collector->MoveRollers(0.0);
 }
-void CollectTote::Interrupted(){
+void RollIn::Interrupted(){
   End();
 }
 // vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CollectTote.h b/Commands/Collector/RollIn.h
similarity index 72%
rename from Commands/Collector/CollectTote.h
rename to Commands/Collector/RollIn.h
index 03074fd..8a222aa 100644
--- a/Commands/Collector/CollectTote.h
+++ b/Commands/Collector/RollIn.h
@@ -1,14 +1,14 @@
-#ifndef COLLECTTOTE_H
-#define COLLECTTOTE_H
+#ifndef ROLLIN_H
+#define ROLLIN_H
 
 #include "Commands/Command.h"
 #include "../../CommandBase.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
-class CollectTote: public Command{
+class RollIn: public Command{
   public:
-    CollectTote();
+    RollIn();
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/Commands/Collector/ReleaseTote.cpp b/Commands/Collector/RollOut.cpp
similarity index 64%
rename from Commands/Collector/ReleaseTote.cpp
rename to Commands/Collector/RollOut.cpp
index 744efcb..7f7297d 100644
--- a/Commands/Collector/ReleaseTote.cpp
+++ b/Commands/Collector/RollOut.cpp
@@ -1,22 +1,22 @@
-#include "ReleaseTote.h"
-ReleaseTote::ReleaseTote() : Command("ReleaseTote"){
+#include "RollOut.h"
+RollOut::RollOut() : Command("RollOut"){
   Requires(DentRobot::collector);
 }
-void ReleaseTote::Initialize(){
+void RollOut::Initialize(){
   SetTimeout(2.0);
 }
-void ReleaseTote::Execute(){
+void RollOut::Execute(){
   //TODO check this value to move the motors in the right direction
   // Devide by 2 twice because this speed should be half the collector speed
   DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2/2);
 }
-bool ReleaseTote::IsFinished(){
+bool RollOut::IsFinished(){
   return DentRobot::collector->BoxCollected();
 }
-void ReleaseTote::End(){
+void RollOut::End(){
   DentRobot::collector->MoveRollers(0.0f);
 }
-void ReleaseTote::Interrupted(){
+void RollOut::Interrupted(){
   End();
 }
 // vim: ts=2:sw=2:et
diff --git a/Commands/Collector/ReleaseTote.h b/Commands/Collector/RollOut.h
similarity index 72%
rename from Commands/Collector/ReleaseTote.h
rename to Commands/Collector/RollOut.h
index 722680f..7143d07 100644
--- a/Commands/Collector/ReleaseTote.h
+++ b/Commands/Collector/RollOut.h
@@ -1,14 +1,14 @@
-#ifndef RELEASETOTE_H
-#define RELEASETOTE_H
+#ifndef ROLLOUT_H
+#define ROLLOUT_H
 
 #include "Commands/Command.h"
 #include "../../CommandBase.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
-class ReleaseTote: public Command{
+class RollOut: public Command{
   public:
-    ReleaseTote();
+    RollOut();
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/OI.cpp b/OI.cpp
index e01e119..84abd5c 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -3,8 +3,8 @@
 #include "Commands/Elevator/Raise.h"
 #include "Commands/Collector/OpenCollector.h"
 #include "Commands/Collector/CloseCollector.h"
-#include "Commands/Collector/CollectTote.h"
-#include "Commands/Collector/ReleaseTote.h"
+#include "Commands/Collector/RollIn.h"
+#include "Commands/Collector/RollOut.h"
 
 OI::OI() {
   // Joysticks
@@ -17,8 +17,8 @@ OI::OI() {
   JoystickButton *left2=new JoystickButton(leftStick, 2);
   right1->WhileHeld(new CloseCollector());
   right2->WhileHeld(new OpenCollector());
-  left1->WhileHeld(new CollectTote());
-  left2->WhileHeld(new ReleaseTote());
+  left1->WhileHeld(new RollIn());
+  left2->WhileHeld(new RollOut());
   // Elevator
   raise=new Raise();
   lower=new Lower();

From 79b635d5542d4094747082eb4ed5dd6eb57a2b93 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Fri, 13 Feb 2015 21:41:38 -0500
Subject: [PATCH 05/18] Added BinElevator commands

---
 Commands/Autonomous/AutoDrive.cpp  | 12 +++++++++++-
 Commands/Autonomous/AutoDrive.h    |  3 +++
 Commands/Collector/CollectTote.cpp | 11 +++++++++++
 Commands/Collector/CollectTote.h   | 14 ++++++++++++++
 Commands/Collector/ReleaseTote.cpp | 11 +++++++++++
 Commands/Collector/ReleaseTote.h   | 14 ++++++++++++++
 OI.cpp                             | 13 +++++++++++++
 OI.h                               |  2 +-
 RobotMap.h                         |  8 ++++----
 9 files changed, 82 insertions(+), 6 deletions(-)
 create mode 100644 Commands/Collector/CollectTote.cpp
 create mode 100644 Commands/Collector/CollectTote.h
 create mode 100644 Commands/Collector/ReleaseTote.cpp
 create mode 100644 Commands/Collector/ReleaseTote.h

diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index df147d7..f279f03 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -4,12 +4,22 @@
 AutoDrive::AutoDrive(double wait) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
   SetTimeout(wait);
+  power=NULL;
+}
+AutoDrive::AutoDrive(double wait, double p) : Command("AutoDrive"){
+  Requires(DentRobot::drivetrain);
+  SetTimeout(wait);
+  *power=p;
 }
 void AutoDrive::Initialize(){
 }
 void AutoDrive::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
+  if(power==NULL){
+    DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
+  }else{
+    DentRobot::drivetrain->DriveMecanum(0.0,*power,0.0,0.9,0.0);
+  }
 }
 bool AutoDrive::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h
index d84f33f..3c1e9ae 100644
--- a/Commands/Autonomous/AutoDrive.h
+++ b/Commands/Autonomous/AutoDrive.h
@@ -7,8 +7,11 @@
 #include "WPILib.h"
 
 class AutoDrive: public Command{
+  private:
+    double *power;
   public:
     AutoDrive(double);
+    AutoDrive(double, double);
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
new file mode 100644
index 0000000..88b078b
--- /dev/null
+++ b/Commands/Collector/CollectTote.cpp
@@ -0,0 +1,11 @@
+#include "CollectTote.h"
+#include "../../DentRobot.h"
+#include "../Autonomous/AutoDrive.h"
+#include "RollIn.h"
+#include "CloseCollector.h"
+CollectTote::CollectTote(){
+  AddParallel(new RollIn());
+  AddParallel(new AutoDrive(0.5, -0.75));
+  AddSequential(new CloseCollector());
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CollectTote.h b/Commands/Collector/CollectTote.h
new file mode 100644
index 0000000..be6d94e
--- /dev/null
+++ b/Commands/Collector/CollectTote.h
@@ -0,0 +1,14 @@
+#ifndef COLLECTTOTE_H
+#define COLLECTTOTE_H
+
+#include "Commands/CommandGroup.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CollectTote: public CommandGroup{
+  public:
+    CollectTote();
+};
+#endif
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/ReleaseTote.cpp b/Commands/Collector/ReleaseTote.cpp
new file mode 100644
index 0000000..c136661
--- /dev/null
+++ b/Commands/Collector/ReleaseTote.cpp
@@ -0,0 +1,11 @@
+#include "ReleaseTote.h"
+#include "../../DentRobot.h"
+#include "RollOut.h"
+#include "../Autonomous/AutoDrive.h"
+#include "CloseCollector.h"
+ReleaseTote::ReleaseTote(){
+  AddParallel(new RollOut());
+  AddParallel(new AutoDrive(0.5, 0.75));
+  AddSequential(new CloseCollector());
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/ReleaseTote.h b/Commands/Collector/ReleaseTote.h
new file mode 100644
index 0000000..df04a04
--- /dev/null
+++ b/Commands/Collector/ReleaseTote.h
@@ -0,0 +1,14 @@
+#ifndef RELEASETOTE_H
+#define RELEASETOTE_H
+
+#include "Commands/CommandGroup.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class ReleaseTote: public CommandGroup{
+  public:
+    ReleaseTote();
+};
+#endif
+// vim: ts=2:sw=2:et
diff --git a/OI.cpp b/OI.cpp
index 84abd5c..9a4afa5 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -5,6 +5,8 @@
 #include "Commands/Collector/CloseCollector.h"
 #include "Commands/Collector/RollIn.h"
 #include "Commands/Collector/RollOut.h"
+#include "Commands/BinElevator/BinLower.h"
+#include "Commands/BinElevator/BinRaise.h"
 
 OI::OI() {
   // Joysticks
@@ -34,10 +36,21 @@ OI::OI() {
   right6->WhenPressed(raise);
   right5->CancelWhenPressed(lower);
   right6->CancelWhenPressed(lower);
+  // BinElevator
+  binRaise=new BinRaise();
+  binLower=new BinLower();
+  JoystickButton *right7=new JoystickButton(rightStick, 7);
+  JoystickButton *right8=new JoystickButton(rightStick, 8);
+  right7->WhenPressed(binLower);
+  right7->CancelWhenPressed(binRaise);
+  right8->WhenPressed(binRaise);
+  right8->CancelWhenPressed(binLower);
   // Cancel
   JoystickButton *right12=new JoystickButton(rightStick, 12);
   right12->CancelWhenPressed(raise);
   right12->CancelWhenPressed(lower);
+  right12->CancelWhenPressed(binRaise);
+  right12->CancelWhenPressed(binLower);
 }
 Joystick* OI::GetRightStick(){
   return rightStick;
diff --git a/OI.h b/OI.h
index 357dbda..e62b157 100644
--- a/OI.h
+++ b/OI.h
@@ -12,7 +12,7 @@ class OI
     OI();
     Joystick* GetRightStick();
     Joystick* GetLeftStick();
-    Command *raise, *lower;
+    Command *raise, *lower, *binLower, *binRaise;
 };
 #endif
 // vim: ts=2:sw=2:et
diff --git a/RobotMap.h b/RobotMap.h
index a3abe10..afeb205 100644
--- a/RobotMap.h
+++ b/RobotMap.h
@@ -9,16 +9,16 @@
 #define ELEVATOR_COLELCT_TOTE_DIO 1
 #define ELEVATOR_READY_TOTE_DIO 2
 #define ELEVATOR_TOP_DIO 5
-#define ELEVATOR_ENCODERA 0
-#define ELEVATOR_ENCODERB 1
+#define ELEVATOR_ENCODERA 4
+#define ELEVATOR_ENCODERB 9
 
 // BinElevator
 #define BINELEVATOR_CAN 11
 #define BINELEVATOR_BOTTOM_DIO 6
 #define BINELEVATOR_COLELCT_BIN_DIO 7
 #define BINELEVATOR_TOP_DIO 8
-#define BINELEVATOR_ENCODERA 2
-#define BINELEVATOR_ENCODERB 3
+#define BINELEVATOR_ENCODERA 10
+#define BINELEVATOR_ENCODERB 11
 
 // Drivetrain
 #define DRIVE_FRONT_LEFT_CAN 2

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 06/18] 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();

From e66a67143a69550f716425d585b796816251cc4f Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 14 Feb 2015 13:36:50 -0500
Subject: [PATCH 07/18] Changed power from pointer (untested)

---
 Commands/Autonomous/AutoDrive.cpp  | 9 +++++----
 Commands/Autonomous/AutoDrive.h    | 2 +-
 Commands/Collector/CollectTote.cpp | 2 +-
 3 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index 0636003..24fce24 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -4,21 +4,22 @@
 AutoDrive::AutoDrive(double wait) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
   SetTimeout(wait);
-  power=NULL;
+  power=5;
 }
 AutoDrive::AutoDrive(double wait, double p) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
   SetTimeout(wait);
-  *power=p;
+  power=p;
 }
 void AutoDrive::Initialize(){
 }
 void AutoDrive::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  if(power==NULL){
+  //if(power==NULL){
+  if(power==5){
     DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
   }else{
-    DentRobot::drivetrain->DriveMecanum(0.0,*power,0.0,0.9,0.0);
+    DentRobot::drivetrain->DriveMecanum(0.0,power,0.0,0.9,0.0);
   }
 }
 bool AutoDrive::IsFinished(){
diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h
index 3c1e9ae..f2d4785 100644
--- a/Commands/Autonomous/AutoDrive.h
+++ b/Commands/Autonomous/AutoDrive.h
@@ -8,7 +8,7 @@
 
 class AutoDrive: public Command{
   private:
-    double *power;
+    double power;
   public:
     AutoDrive(double);
     AutoDrive(double, double);
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
index 141738c..cf2397b 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/CollectTote.cpp
@@ -5,8 +5,8 @@
 #include "CloseCollector.h"
 CollectTote::CollectTote(){
   AddParallel(new CloseCollector());
-  AddSequential(new RollIn());
   // TODO: Fix null error
   //AddParallel(new AutoDrive(0.5, -0.75));
+  AddSequential(new RollIn());
 }
 // vim: ts=2:sw=2:et

From e07c7a2da76102f82f2ef991edb7ea154f564fdd Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 14 Feb 2015 13:45:54 -0500
Subject: [PATCH 08/18] Added binLower, binRaise on joystick

---
 OI.cpp | 18 ++++++------------
 1 file changed, 6 insertions(+), 12 deletions(-)

diff --git a/OI.cpp b/OI.cpp
index 9a4afa5..3107635 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -24,27 +24,21 @@ OI::OI() {
   // Elevator
   raise=new Raise();
   lower=new Lower();
-  JoystickButton *right3=new JoystickButton(rightStick, 3);
   JoystickButton *right4=new JoystickButton(rightStick, 4);
-  JoystickButton *right5=new JoystickButton(rightStick, 5);
   JoystickButton *right6=new JoystickButton(rightStick, 6);
-  right3->WhenPressed(lower);
   right4->WhenPressed(lower);
-  right3->CancelWhenPressed(raise);
   right4->CancelWhenPressed(raise);
-  right5->WhenPressed(raise);
   right6->WhenPressed(raise);
-  right5->CancelWhenPressed(lower);
   right6->CancelWhenPressed(lower);
   // BinElevator
+  JoystickButton *right3=new JoystickButton(rightStick, 3);
+  JoystickButton *right5=new JoystickButton(rightStick, 5);
   binRaise=new BinRaise();
   binLower=new BinLower();
-  JoystickButton *right7=new JoystickButton(rightStick, 7);
-  JoystickButton *right8=new JoystickButton(rightStick, 8);
-  right7->WhenPressed(binLower);
-  right7->CancelWhenPressed(binRaise);
-  right8->WhenPressed(binRaise);
-  right8->CancelWhenPressed(binLower);
+  right3->WhenPressed(binLower);
+  right3->CancelWhenPressed(binRaise);
+  right5->WhenPressed(binRaise);
+  right5->CancelWhenPressed(binLower);
   // Cancel
   JoystickButton *right12=new JoystickButton(rightStick, 12);
   right12->CancelWhenPressed(raise);

From c783651d562861d50168da1d689c0cbd83453eab Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 14 Feb 2015 14:31:38 -0500
Subject: [PATCH 09/18] Minor collector changes

---
 Commands/Collector/CloseCollector.cpp | 2 +-
 Commands/Collector/CollectTote.cpp    | 9 +++++++--
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/Commands/Collector/CloseCollector.cpp b/Commands/Collector/CloseCollector.cpp
index b3a473d..31e2431 100644
--- a/Commands/Collector/CloseCollector.cpp
+++ b/Commands/Collector/CloseCollector.cpp
@@ -3,7 +3,7 @@ CloseCollector::CloseCollector() : Command("CloseCollector"){
 }
 void CloseCollector::Initialize(){
   printf("Initialized collector: 0.5\n");
-  SetTimeout(.5);
+  SetTimeout(1);
 }
 void CloseCollector::Execute(){
   //printf("Closing collector: -0.5f\n");
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
index cf2397b..d4ab890 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/CollectTote.cpp
@@ -5,8 +5,13 @@
 #include "CloseCollector.h"
 CollectTote::CollectTote(){
   AddParallel(new CloseCollector());
-  // TODO: Fix null error
-  //AddParallel(new AutoDrive(0.5, -0.75));
+  AddParallel(new AutoDrive(0.5, -1.0));
+  AddSequential(new RollIn());
+  AddParallel(new CloseCollector());
+  AddParallel(new AutoDrive(0.5, 1.0));
+  AddSequential(new RollIn());
+  AddParallel(new CloseCollector());
+  AddParallel(new AutoDrive(0.5, -1.0));
   AddSequential(new RollIn());
 }
 // vim: ts=2:sw=2:et

From c8acfae930c4642fe83e820e954f386e135c3de3 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 14 Feb 2015 15:24:42 -0500
Subject: [PATCH 10/18] Removed window motors (untested)

---
 Commands/Autonomous/Autonomous.cpp    |  5 -----
 Commands/Collector/CloseCollector.cpp | 27 ---------------------------
 Commands/Collector/CloseCollector.h   | 20 --------------------
 Commands/Collector/CollectTote.cpp    |  4 ----
 Commands/Collector/OpenCollector.cpp  | 22 ----------------------
 Commands/Collector/OpenCollector.h    | 20 --------------------
 Commands/Collector/ReleaseTote.cpp    |  2 --
 OI.cpp                                |  6 ------
 Subsystems/Collector.cpp              |  6 ------
 Subsystems/Collector.h                |  3 +--
 TODO.txt                              |  0
 11 files changed, 1 insertion(+), 114 deletions(-)
 delete mode 100644 Commands/Collector/CloseCollector.cpp
 delete mode 100644 Commands/Collector/CloseCollector.h
 delete mode 100644 Commands/Collector/OpenCollector.cpp
 delete mode 100644 Commands/Collector/OpenCollector.h
 delete mode 100644 TODO.txt

diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index 1ee23c4..e13fac2 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -4,8 +4,6 @@
 #include "../Elevator/Lower.h"
 #include "AutoDrive.h"
 #include "Turn.h"
-#include "../Collector/CloseCollector.h"
-#include "../Collector/OpenCollector.h"
 #include "../Collector/RollIn.h"
 #include "../Collector/CollectTote.h"
 Autonomous::Autonomous(int seq){
@@ -16,11 +14,8 @@ Autonomous::Autonomous(int seq){
       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;
diff --git a/Commands/Collector/CloseCollector.cpp b/Commands/Collector/CloseCollector.cpp
deleted file mode 100644
index 31e2431..0000000
--- a/Commands/Collector/CloseCollector.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-#include "CloseCollector.h"
-CloseCollector::CloseCollector() : Command("CloseCollector"){
-}
-void CloseCollector::Initialize(){
-  printf("Initialized collector: 0.5\n");
-  SetTimeout(1);
-}
-void CloseCollector::Execute(){
-  //printf("Closing collector: -0.5f\n");
-  DentRobot::collector->MoveArms(-0.5);
-  //DentRobot::collector->MoveArms(-(-DentRobot::oi->GetRightStick()->GetRawAxis(3)+1)/2*.3/.5);
-}
-bool CloseCollector::IsFinished(){
-  if(DentRobot::collector->ArmSensor()||IsTimedOut()){
-    printf("Stopped Closing: %d, %d\n",DentRobot::collector->ArmSensor(), IsTimedOut());
-    return true;
-  }else{
-    return false;
-  }
-}
-void CloseCollector::End(){
-  DentRobot::collector->MoveArms(0.0f);
-}
-void CloseCollector::Interrupted(){
-  End();
-}
-// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CloseCollector.h b/Commands/Collector/CloseCollector.h
deleted file mode 100644
index cd26268..0000000
--- a/Commands/Collector/CloseCollector.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef CLOSECOLLECTOR_H
-#define CLOSECOLLECTOR_H
-
-#include "Commands/Command.h"
-#include "../../CommandBase.h"
-#include "../../DentRobot.h"
-#include "WPILib.h"
-
-class CloseCollector: public Command{
-  public:
-    CloseCollector();
-    void Initialize();
-    void Execute();
-    bool IsFinished();
-    void End();
-    void Interrupted();
-};
-
-#endif
-// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
index d4ab890..39e134a 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/CollectTote.cpp
@@ -2,15 +2,11 @@
 #include "../../DentRobot.h"
 #include "../Autonomous/AutoDrive.h"
 #include "RollIn.h"
-#include "CloseCollector.h"
 CollectTote::CollectTote(){
-  AddParallel(new CloseCollector());
   AddParallel(new AutoDrive(0.5, -1.0));
   AddSequential(new RollIn());
-  AddParallel(new CloseCollector());
   AddParallel(new AutoDrive(0.5, 1.0));
   AddSequential(new RollIn());
-  AddParallel(new CloseCollector());
   AddParallel(new AutoDrive(0.5, -1.0));
   AddSequential(new RollIn());
 }
diff --git a/Commands/Collector/OpenCollector.cpp b/Commands/Collector/OpenCollector.cpp
deleted file mode 100644
index 5cebc3e..0000000
--- a/Commands/Collector/OpenCollector.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "OpenCollector.h"
-OpenCollector::OpenCollector() : Command("OpenCollector"){
-  Requires(DentRobot::collector);
-}
-void OpenCollector::Initialize(){
-  SetTimeout(0.5);
-}
-void OpenCollector::Execute(){
-  DentRobot::collector->MoveArms(0.35);
-  //DentRobot::collector->MoveArms((-DentRobot::oi->GetRightStick()->GetRawAxis(3)+1)/2*.3/.5);
-}
-bool OpenCollector::IsFinished(){
-  //return DentRobot::collector->ArmSensor();
-  return IsTimedOut();
-}
-void OpenCollector::End(){
-  DentRobot::collector->MoveArms(0.0f);
-}
-void OpenCollector::Interrupted(){
-  End();
-}
-// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/OpenCollector.h b/Commands/Collector/OpenCollector.h
deleted file mode 100644
index 2686251..0000000
--- a/Commands/Collector/OpenCollector.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef OPENCOLLECTOR_H
-#define OPENCOLLECTOR_H
-
-#include "Commands/Command.h"
-#include "../../CommandBase.h"
-#include "../../DentRobot.h"
-#include "WPILib.h"
-
-class OpenCollector: public Command{
-  public:
-    OpenCollector();
-    void Initialize();
-    void Execute();
-    bool IsFinished();
-    void End();
-    void Interrupted();
-};
-
-#endif
-// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/ReleaseTote.cpp b/Commands/Collector/ReleaseTote.cpp
index c136661..c10ddd2 100644
--- a/Commands/Collector/ReleaseTote.cpp
+++ b/Commands/Collector/ReleaseTote.cpp
@@ -2,10 +2,8 @@
 #include "../../DentRobot.h"
 #include "RollOut.h"
 #include "../Autonomous/AutoDrive.h"
-#include "CloseCollector.h"
 ReleaseTote::ReleaseTote(){
   AddParallel(new RollOut());
   AddParallel(new AutoDrive(0.5, 0.75));
-  AddSequential(new CloseCollector());
 }
 // vim: ts=2:sw=2:et
diff --git a/OI.cpp b/OI.cpp
index 3107635..656da90 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -1,8 +1,6 @@
 #include "OI.h"
 #include "Commands/Elevator/Lower.h"
 #include "Commands/Elevator/Raise.h"
-#include "Commands/Collector/OpenCollector.h"
-#include "Commands/Collector/CloseCollector.h"
 #include "Commands/Collector/RollIn.h"
 #include "Commands/Collector/RollOut.h"
 #include "Commands/BinElevator/BinLower.h"
@@ -13,12 +11,8 @@ OI::OI() {
   leftStick=new Joystick(0);
   rightStick=new Joystick(1);
   // Collector
-  JoystickButton *right1=new JoystickButton(rightStick, 1);
-  JoystickButton *right2=new JoystickButton(rightStick, 2);
   JoystickButton *left1=new JoystickButton(leftStick, 1);
   JoystickButton *left2=new JoystickButton(leftStick, 2);
-  right1->WhileHeld(new CloseCollector());
-  right2->WhileHeld(new OpenCollector());
   left1->WhileHeld(new RollIn());
   left2->WhileHeld(new RollOut());
   // Elevator
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index fcc7959..fe8c589 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -2,18 +2,12 @@
 #include "../RobotMap.h"
 
 Collector::Collector() : Subsystem("Collector") {
-  windowMotorLeft=new CANTalon(COLLECTOR_WINDOW_LEFT_CAN);
-  windowMotorRight=new CANTalon(COLLECTOR_WINDOW_RIGHT_CAN);
   collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN);
   collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN);
   collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
 }
 void Collector::InitDefaultCommand() {
 }
-void Collector::MoveArms(double a){
-  windowMotorLeft->Set(a);
-  windowMotorRight->Set(-a);
-}
 void Collector::MoveRollers(double a){
   collectorMotorLeft->Set(a);
   collectorMotorBottom->Set(a);
diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h
index 92262ac..c74ed35 100644
--- a/Subsystems/Collector.h
+++ b/Subsystems/Collector.h
@@ -5,11 +5,10 @@
 class Collector: public Subsystem
 {
   private:
-    CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRight;
+    CANTalon *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRight;
   public:
     Collector();
     void InitDefaultCommand();
-    void MoveArms(double);
     void MoveRollers(double);
     bool ArmSensor();
     bool BoxCollected();
diff --git a/TODO.txt b/TODO.txt
deleted file mode 100644
index e69de29..0000000

From 5aab16c8b197b6be41531b82671438bba1266742 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 14 Feb 2015 16:16:03 -0500
Subject: [PATCH 11/18] Minor collector improvements

---
 Commands/Collector/RollIn.cpp  |  2 +-
 Commands/Collector/RollOut.cpp |  2 +-
 Commands/Drivetrain/Drive.cpp  |  2 +-
 Subsystems/Collector.cpp       | 12 ++----------
 Subsystems/Collector.h         |  2 --
 5 files changed, 5 insertions(+), 15 deletions(-)

diff --git a/Commands/Collector/RollIn.cpp b/Commands/Collector/RollIn.cpp
index e22e12f..3b19252 100644
--- a/Commands/Collector/RollIn.cpp
+++ b/Commands/Collector/RollIn.cpp
@@ -10,7 +10,7 @@ void RollIn::Execute(){
   DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool RollIn::IsFinished(){
-  return DentRobot::collector->BoxCollected()||IsTimedOut();
+  return IsTimedOut();
 }
 void RollIn::End(){
   DentRobot::collector->MoveRollers(0.0);
diff --git a/Commands/Collector/RollOut.cpp b/Commands/Collector/RollOut.cpp
index 7f7297d..82247ec 100644
--- a/Commands/Collector/RollOut.cpp
+++ b/Commands/Collector/RollOut.cpp
@@ -11,7 +11,7 @@ void RollOut::Execute(){
   DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2/2);
 }
 bool RollOut::IsFinished(){
-  return DentRobot::collector->BoxCollected();
+  return IsTimedOut();
 }
 void RollOut::End(){
   DentRobot::collector->MoveRollers(0.0f);
diff --git a/Commands/Drivetrain/Drive.cpp b/Commands/Drivetrain/Drive.cpp
index 3b91bae..a8c1241 100644
--- a/Commands/Drivetrain/Drive.cpp
+++ b/Commands/Drivetrain/Drive.cpp
@@ -21,7 +21,7 @@ void Drive::Execute(){
   DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
 }
 bool Drive::IsFinished(){
-  return false;
+  return IsTimedOut();
 }
 void Drive::End(){
 }
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index fe8c589..c3a1429 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -1,24 +1,16 @@
 #include "Collector.h"
 #include "../RobotMap.h"
 
-Collector::Collector() : Subsystem("Collector") {
+Collector::Collector() : Subsystem("Collector"){
   collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN);
   collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN);
   collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
 }
-void Collector::InitDefaultCommand() {
+void Collector::InitDefaultCommand(){
 }
 void Collector::MoveRollers(double a){
   collectorMotorLeft->Set(a);
   collectorMotorBottom->Set(a);
   collectorMotorRight->Set(-a);
 }
-bool Collector::ArmSensor(){
-  // TODO: include limit switch code
-  return false;
-}
-bool Collector::BoxCollected(){
-  return false;
-  //return boxSwitch->Get();
-}
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h
index c74ed35..dbddc51 100644
--- a/Subsystems/Collector.h
+++ b/Subsystems/Collector.h
@@ -10,8 +10,6 @@ class Collector: public Subsystem
     Collector();
     void InitDefaultCommand();
     void MoveRollers(double);
-    bool ArmSensor();
-    bool BoxCollected();
 };
 #endif
 // vim: ts=2:sw=2:et

From 7665f16a9ba5e40aa3daa7f9789edb1539f19d7f Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 14 Feb 2015 17:12:58 -0500
Subject: [PATCH 12/18] Added collectorMotorRamp

---
 RobotMap.h               | 10 ++++------
 Subsystems/Collector.cpp |  2 ++
 Subsystems/Collector.h   |  2 +-
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/RobotMap.h b/RobotMap.h
index afeb205..987561e 100644
--- a/RobotMap.h
+++ b/RobotMap.h
@@ -5,10 +5,9 @@
 
 // Elevator
 #define ELEVATOR_CAN 1
-#define ELEVATOR_BOTTOM_DIO 0
-#define ELEVATOR_COLELCT_TOTE_DIO 1
-#define ELEVATOR_READY_TOTE_DIO 2
-#define ELEVATOR_TOP_DIO 5
+#define ELEVATOR_BOTTOM_DIO 9
+#define ELEVATOR_MIDDLE_DIO 8
+#define ELEVATOR_TOP_DIO 7
 #define ELEVATOR_ENCODERA 4
 #define ELEVATOR_ENCODERB 9
 
@@ -27,8 +26,7 @@
 #define DRIVE_BACK_RIGHT_CAN 5
 
 // Collector
-#define COLLECTOR_WINDOW_LEFT_CAN 6
-#define COLLECTOR_WINDOW_RIGHT_CAN 7
+#define COLLECTOR_RAMP_CAN 7
 #define COLLECTOR_LEFT_CAN 8
 #define COLLECTOR_BOTTOM_CAN 10
 #define COLLECTOR_RIGHT_CAN 9
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index c3a1429..c7cd582 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -4,6 +4,7 @@
 Collector::Collector() : Subsystem("Collector"){
   collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN);
   collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN);
+  collectorMotorRamp=new CANTalon(COLLECTOR_RAMP_CAN);
   collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
 }
 void Collector::InitDefaultCommand(){
@@ -11,6 +12,7 @@ void Collector::InitDefaultCommand(){
 void Collector::MoveRollers(double a){
   collectorMotorLeft->Set(a);
   collectorMotorBottom->Set(a);
+  collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
 }
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h
index dbddc51..ddb31ed 100644
--- a/Subsystems/Collector.h
+++ b/Subsystems/Collector.h
@@ -5,7 +5,7 @@
 class Collector: public Subsystem
 {
   private:
-    CANTalon *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRight;
+    CANTalon *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRamp, *collectorMotorRight;
   public:
     Collector();
     void InitDefaultCommand();

From 9490b8fb9b8cffd5d3c260f43ffe876e65208cb7 Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Sun, 15 Feb 2015 12:53:19 +0000
Subject: [PATCH 13/18] Fixed sensor readings and added sonar

---
 Commands/BinElevator/BinLower.cpp |  2 +-
 Commands/BinElevator/BinRaise.cpp |  2 +-
 Commands/Collector/RollIn.cpp     |  2 +-
 Commands/Collector/RollOut.cpp    |  2 +-
 Commands/Elevator/Raise.cpp       | 11 +++++++++--
 RobotMap.h                        |  2 ++
 Subsystems/Collector.cpp          |  6 ++++++
 Subsystems/Collector.h            |  3 +++
 Subsystems/Elevator.cpp           |  5 +++++
 Subsystems/Elevator.h             |  4 +++-
 10 files changed, 32 insertions(+), 7 deletions(-)

diff --git a/Commands/BinElevator/BinLower.cpp b/Commands/BinElevator/BinLower.cpp
index 158f98a..b4001d8 100644
--- a/Commands/BinElevator/BinLower.cpp
+++ b/Commands/BinElevator/BinLower.cpp
@@ -10,7 +10,7 @@ void BinLower::Execute(){
   DentRobot::binElevator->Run(-1.0);
 }
 bool BinLower::IsFinished(){
-  if (!DentRobot::binElevator->GetElevatorBottom()||IsTimedOut()){
+  if (/*!DentRobot::binElevator->GetElevatorBottom()||*/IsTimedOut()){
     printf("Robot stoped BinLowering. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorBottom());
     return true;
   }else{
diff --git a/Commands/BinElevator/BinRaise.cpp b/Commands/BinElevator/BinRaise.cpp
index aa765a4..1fe0148 100644
--- a/Commands/BinElevator/BinRaise.cpp
+++ b/Commands/BinElevator/BinRaise.cpp
@@ -10,7 +10,7 @@ void BinRaise::Execute(){
   DentRobot::binElevator->Run(1.0);
 }
 bool BinRaise::IsFinished(){
-  if (!DentRobot::binElevator->GetElevatorTop()||IsTimedOut()){
+  if (/*!DentRobot::binElevator->GetElevatorTop()||*/IsTimedOut()){
     printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorTop());
     return true;
   }else{
diff --git a/Commands/Collector/RollIn.cpp b/Commands/Collector/RollIn.cpp
index 3b19252..5ff7287 100644
--- a/Commands/Collector/RollIn.cpp
+++ b/Commands/Collector/RollIn.cpp
@@ -7,7 +7,7 @@ void RollIn::Initialize(){
 }
 void RollIn::Execute(){
   //TODO check this value to move the motors in the right direction
-  DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+  DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool RollIn::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Collector/RollOut.cpp b/Commands/Collector/RollOut.cpp
index 82247ec..d4b9290 100644
--- a/Commands/Collector/RollOut.cpp
+++ b/Commands/Collector/RollOut.cpp
@@ -8,7 +8,7 @@ void RollOut::Initialize(){
 void RollOut::Execute(){
   //TODO check this value to move the motors in the right direction
   // Devide by 2 twice because this speed should be half the collector speed
-  DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2/2);
+  DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool RollOut::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp
index cd8885f..c606bfc 100644
--- a/Commands/Elevator/Raise.cpp
+++ b/Commands/Elevator/Raise.cpp
@@ -10,8 +10,15 @@ void Raise::Execute(){
   DentRobot::elevator->Run(1.0);
 }
 bool Raise::IsFinished(){
-  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
-    printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
+  if(!DentRobot::elevator->GetElevatorMiddle()){
+    DentRobot::elevator->stoppedAtSensor=true;
+  }
+  if ((DentRobot::elevator->stoppedAtSensor)){
+    printf("Stopped at the middle sensor\n");
+    DentRobot::elevator->stoppedAtSensor=false;
+    return true;
+  }else if (!DentRobot::elevator->GetElevatorTop()) {
+    printf("Stopping at the top sensor\n");
     return true;
   }else{
     return false;
diff --git a/RobotMap.h b/RobotMap.h
index 987561e..0279c47 100644
--- a/RobotMap.h
+++ b/RobotMap.h
@@ -30,6 +30,8 @@
 #define COLLECTOR_LEFT_CAN 8
 #define COLLECTOR_BOTTOM_CAN 10
 #define COLLECTOR_RIGHT_CAN 9
+#define COLLECTOR_SONAR_DIO 6
+#define COLLECTOR_SONAR_ANALOG 3
 
 #endif
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index c7cd582..f1e8a22 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -6,6 +6,8 @@ Collector::Collector() : Subsystem("Collector"){
   collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN);
   collectorMotorRamp=new CANTalon(COLLECTOR_RAMP_CAN);
   collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
+  sonarAnalog=new AnalogInput(COLLECTOR_SONAR_ANALOG);
+  sonarDigital=new DigitalOutput(COLLECTOR_RIGHT_CAN);
 }
 void Collector::InitDefaultCommand(){
 }
@@ -14,5 +16,9 @@ void Collector::MoveRollers(double a){
   collectorMotorBottom->Set(a);
   collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
+  GetSonarDistance();
+}
+float Collector::GetSonarDistance(){
+  printf("Sonar Distance %f\n",sonarAnalog->GetAverageValue());
 }
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h
index ddb31ed..dfdf655 100644
--- a/Subsystems/Collector.h
+++ b/Subsystems/Collector.h
@@ -6,10 +6,13 @@ class Collector: public Subsystem
 {
   private:
     CANTalon *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRamp, *collectorMotorRight;
+    AnalogInput *sonarAnalog;
+    DigitalOutput *sonarDigital;
   public:
     Collector();
     void InitDefaultCommand();
     void MoveRollers(double);
+    float GetSonarDistance();
 };
 #endif
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp
index ffe24c3..0649a44 100644
--- a/Subsystems/Elevator.cpp
+++ b/Subsystems/Elevator.cpp
@@ -4,9 +4,11 @@ Elevator::Elevator(){
   motor=new CANTalon(ELEVATOR_CAN);
   elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
   elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
+  elevatorMiddle=new DigitalInput(ELEVATOR_MIDDLE_DIO);
   elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
   // Checks if the elevator is drifting
   useEncoder=false;
+  stoppedAtSensor=false;
 }
 void Elevator::InitDefaultCommand(){
 }
@@ -26,6 +28,9 @@ double Elevator::GetHeight(){
 bool Elevator::GetElevatorBottom(){
   return elevatorBottom->Get();
 }
+bool Elevator::GetElevatorMiddle(){
+  return elevatorMiddle->Get();
+}
 bool Elevator::GetElevatorTop(){
   return elevatorTop->Get();
 }
diff --git a/Subsystems/Elevator.h b/Subsystems/Elevator.h
index 5454063..505386d 100644
--- a/Subsystems/Elevator.h
+++ b/Subsystems/Elevator.h
@@ -8,15 +8,17 @@ class Elevator{
     CANTalon *motor;
     Encoder *elevatorEncoder;
     static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
-    DigitalInput *elevatorBottom, *elevatorTop;
+    DigitalInput *elevatorBottom, *elevatorMiddle, *elevatorTop;
     bool useEncoder;
   public:
     Elevator();
+    bool stoppedAtSensor;
     void InitDefaultCommand();
     void Run(double);
     void ResetEncoder();
     double GetHeight();
     bool GetElevatorTop();
+    bool GetElevatorMiddle();
     bool GetElevatorBottom();
     void SetUseEncoder(bool);
     bool GetUseEncoder();

From fd144410a962723b12ad026c0e944cec676f6525 Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Sun, 15 Feb 2015 13:36:19 +0000
Subject: [PATCH 14/18] Minor breakage (untested, needs testing, not working)

---
 Commands/Elevator/Raise.cpp | 19 ++++++++++---------
 RobotMap.h                  |  4 ++--
 Subsystems/Collector.cpp    |  2 +-
 3 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp
index c606bfc..d9d142f 100644
--- a/Commands/Elevator/Raise.cpp
+++ b/Commands/Elevator/Raise.cpp
@@ -10,15 +10,16 @@ void Raise::Execute(){
   DentRobot::elevator->Run(1.0);
 }
 bool Raise::IsFinished(){
-  if(!DentRobot::elevator->GetElevatorMiddle()){
-    DentRobot::elevator->stoppedAtSensor=true;
-  }
-  if ((DentRobot::elevator->stoppedAtSensor)){
-    printf("Stopped at the middle sensor\n");
-    DentRobot::elevator->stoppedAtSensor=false;
-    return true;
-  }else if (!DentRobot::elevator->GetElevatorTop()) {
-    printf("Stopping at the top sensor\n");
+  //if(!DentRobot::elevator->GetElevatorMiddle()){
+  //  DentRobot::elevator->stoppedAtSensor=true;
+  //}
+  //if ((DentRobot::elevator->stoppedAtSensor)){
+  //  printf("Stopped at the middle sensor\n");
+  //  DentRobot::elevator->stoppedAtSensor=false;
+  //  return true;
+  //}else if (!DentRobot::elevator->GetElevatorTop()) {
+  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
+    printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
     return true;
   }else{
     return false;
diff --git a/RobotMap.h b/RobotMap.h
index 0279c47..90aca69 100644
--- a/RobotMap.h
+++ b/RobotMap.h
@@ -6,8 +6,8 @@
 // Elevator
 #define ELEVATOR_CAN 1
 #define ELEVATOR_BOTTOM_DIO 9
-#define ELEVATOR_MIDDLE_DIO 8
-#define ELEVATOR_TOP_DIO 7
+#define ELEVATOR_MIDDLE_DIO 7
+#define ELEVATOR_TOP_DIO 8
 #define ELEVATOR_ENCODERA 4
 #define ELEVATOR_ENCODERB 9
 
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index f1e8a22..f585676 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -19,6 +19,6 @@ void Collector::MoveRollers(double a){
   GetSonarDistance();
 }
 float Collector::GetSonarDistance(){
-  printf("Sonar Distance %f\n",sonarAnalog->GetAverageValue());
+  printf("Sonar Distance %f\n",sonarAnalog->GetAverageVoltage());
 }
 // vim: ts=2:sw=2:et

From 324ae0641752074d2d12c03d55f05185e6c996e8 Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Sun, 15 Feb 2015 14:14:04 +0000
Subject: [PATCH 15/18] Fixed elevator sensors, removed sonar DIO

---
 RobotMap.h               | 1 -
 Subsystems/Collector.cpp | 1 -
 2 files changed, 2 deletions(-)

diff --git a/RobotMap.h b/RobotMap.h
index 90aca69..0ba4d3b 100644
--- a/RobotMap.h
+++ b/RobotMap.h
@@ -30,7 +30,6 @@
 #define COLLECTOR_LEFT_CAN 8
 #define COLLECTOR_BOTTOM_CAN 10
 #define COLLECTOR_RIGHT_CAN 9
-#define COLLECTOR_SONAR_DIO 6
 #define COLLECTOR_SONAR_ANALOG 3
 
 #endif
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index f585676..ad023e7 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -7,7 +7,6 @@ Collector::Collector() : Subsystem("Collector"){
   collectorMotorRamp=new CANTalon(COLLECTOR_RAMP_CAN);
   collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
   sonarAnalog=new AnalogInput(COLLECTOR_SONAR_ANALOG);
-  sonarDigital=new DigitalOutput(COLLECTOR_RIGHT_CAN);
 }
 void Collector::InitDefaultCommand(){
 }

From f01373de39096996b4fb56d02fd9b95f3dd72d01 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sun, 15 Feb 2015 17:10:20 -0500
Subject: [PATCH 16/18] Minor improvements

---
 Commands/Collector/RollIn.cpp  | 9 +++++++--
 Commands/Collector/RollOut.cpp | 2 +-
 Commands/Elevator/Raise.cpp    | 4 ++--
 OI.cpp                         | 6 ++++++
 OI.h                           | 4 +++-
 RobotMap.h                     | 4 ++--
 Subsystems/Collector.cpp       | 6 +++---
 Subsystems/Collector.h         | 2 +-
 8 files changed, 25 insertions(+), 12 deletions(-)

diff --git a/Commands/Collector/RollIn.cpp b/Commands/Collector/RollIn.cpp
index 5ff7287..d7addc7 100644
--- a/Commands/Collector/RollIn.cpp
+++ b/Commands/Collector/RollIn.cpp
@@ -6,8 +6,13 @@ void RollIn::Initialize(){
   SetTimeout(2.0);
 }
 void RollIn::Execute(){
-  //TODO check this value to move the motors in the right direction
-  DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+  double throttle=DentRobot::oi->GetLeftThrottle();
+  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4/5;
+  if(cvt>=1.0){
+    DentRobot::collector->MoveRollers(1.0);
+  }else{
+    DentRobot::collector->MoveRollers(cvt);
+  }
 }
 bool RollIn::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Collector/RollOut.cpp b/Commands/Collector/RollOut.cpp
index d4b9290..bc9ec90 100644
--- a/Commands/Collector/RollOut.cpp
+++ b/Commands/Collector/RollOut.cpp
@@ -8,7 +8,7 @@ void RollOut::Initialize(){
 void RollOut::Execute(){
   //TODO check this value to move the motors in the right direction
   // Devide by 2 twice because this speed should be half the collector speed
-  DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+  DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle());
 }
 bool RollOut::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp
index d9d142f..817ed4d 100644
--- a/Commands/Elevator/Raise.cpp
+++ b/Commands/Elevator/Raise.cpp
@@ -18,8 +18,8 @@ bool Raise::IsFinished(){
   //  DentRobot::elevator->stoppedAtSensor=false;
   //  return true;
   //}else if (!DentRobot::elevator->GetElevatorTop()) {
-  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
-    printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
+  if (!DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle()||IsTimedOut()){
+    printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle());
     return true;
   }else{
     return false;
diff --git a/OI.cpp b/OI.cpp
index 656da90..b39da72 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -46,4 +46,10 @@ Joystick* OI::GetRightStick(){
 Joystick* OI::GetLeftStick(){
   return leftStick;
 }
+double OI::GetRightThrottle(){
+  return (-rightStick->GetRawAxis(3)+1.0)/2;
+}
+double OI::GetLeftThrottle(){
+  return (-leftStick->GetRawAxis(3)+1.0)/2;
+}
 // vim: ts=2:sw=2:et
diff --git a/OI.h b/OI.h
index e62b157..1214378 100644
--- a/OI.h
+++ b/OI.h
@@ -10,9 +10,11 @@ class OI
     Joystick *leftStick, *rightStick;
   public:
     OI();
+    Command *raise, *lower, *binLower, *binRaise;
     Joystick* GetRightStick();
     Joystick* GetLeftStick();
-    Command *raise, *lower, *binLower, *binRaise;
+    double GetLeftThrottle();
+    double GetRightThrottle();
 };
 #endif
 // vim: ts=2:sw=2:et
diff --git a/RobotMap.h b/RobotMap.h
index 0ba4d3b..d797f4f 100644
--- a/RobotMap.h
+++ b/RobotMap.h
@@ -6,8 +6,8 @@
 // Elevator
 #define ELEVATOR_CAN 1
 #define ELEVATOR_BOTTOM_DIO 9
-#define ELEVATOR_MIDDLE_DIO 7
-#define ELEVATOR_TOP_DIO 8
+#define ELEVATOR_MIDDLE_DIO 8
+#define ELEVATOR_TOP_DIO 7
 #define ELEVATOR_ENCODERA 4
 #define ELEVATOR_ENCODERB 9
 
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index ad023e7..a2322d4 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -15,9 +15,9 @@ void Collector::MoveRollers(double a){
   collectorMotorBottom->Set(a);
   collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
-  GetSonarDistance();
+  printf("%f\n",GetSonarDistance());
 }
-float Collector::GetSonarDistance(){
-  printf("Sonar Distance %f\n",sonarAnalog->GetAverageVoltage());
+double Collector::GetSonarDistance(){
+  return sonarAnalog->GetAverageVoltage();
 }
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h
index dfdf655..e182789 100644
--- a/Subsystems/Collector.h
+++ b/Subsystems/Collector.h
@@ -12,7 +12,7 @@ class Collector: public Subsystem
     Collector();
     void InitDefaultCommand();
     void MoveRollers(double);
-    float GetSonarDistance();
+    double GetSonarDistance();
 };
 #endif
 // vim: ts=2:sw=2:et

From f6b6efacc5c2f7e1263adeb6a32aac36b2f107c4 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sun, 15 Feb 2015 17:54:59 -0500
Subject: [PATCH 17/18] Minor drive/auto improvements

---
 Commands/Autonomous/Autonomous.cpp | 1 +
 Commands/Autonomous/Turn.cpp       | 4 ++--
 Commands/Collector/CollectTote.cpp | 6 +-----
 Commands/Collector/RollIn.cpp      | 2 +-
 DentRobot.cpp                      | 2 +-
 Subsystems/Collector.cpp           | 4 ++--
 6 files changed, 8 insertions(+), 11 deletions(-)

diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index e13fac2..236a6b8 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -12,6 +12,7 @@ Autonomous::Autonomous(int seq){
     case 0:
       // Just for testing
       AddSequential(new CollectTote());
+      AddSequential(new Turn());
       //AddSequential(new Raise());
       //AddSequential(new Lower());
       //AddSequential(new Turn());
diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp
index b42bd40..eea1450 100644
--- a/Commands/Autonomous/Turn.cpp
+++ b/Commands/Autonomous/Turn.cpp
@@ -3,13 +3,13 @@
 // Drive for a short while then stop. Just for testing
 Turn::Turn() : Command("Turn"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(0.25);
+  SetTimeout(0.85);
 }
 void Turn::Initialize(){
 }
 void Turn::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.5,0.9,0.0);
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,1.0,0.9,0.0);
 }
 bool Turn::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
index 39e134a..6b728f3 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/CollectTote.cpp
@@ -3,11 +3,7 @@
 #include "../Autonomous/AutoDrive.h"
 #include "RollIn.h"
 CollectTote::CollectTote(){
-  AddParallel(new AutoDrive(0.5, -1.0));
-  AddSequential(new RollIn());
-  AddParallel(new AutoDrive(0.5, 1.0));
-  AddSequential(new RollIn());
-  AddParallel(new AutoDrive(0.5, -1.0));
+  AddParallel(new AutoDrive(1.0, -0.75));
   AddSequential(new RollIn());
 }
 // vim: ts=2:sw=2:et
diff --git a/Commands/Collector/RollIn.cpp b/Commands/Collector/RollIn.cpp
index d7addc7..e4384f2 100644
--- a/Commands/Collector/RollIn.cpp
+++ b/Commands/Collector/RollIn.cpp
@@ -7,7 +7,7 @@ void RollIn::Initialize(){
 }
 void RollIn::Execute(){
   double throttle=DentRobot::oi->GetLeftThrottle();
-  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4/5;
+  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4;
   if(cvt>=1.0){
     DentRobot::collector->MoveRollers(1.0);
   }else{
diff --git a/DentRobot.cpp b/DentRobot.cpp
index 875799e..42a8201 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -18,7 +18,7 @@ DentRobot::DentRobot(){
   CameraServer::GetInstance()->StartAutomaticCapture("cam0");
   //SmartDashboard::PutNumber("Auto Wait Time", 1.0);
   //SmartDashboard::PutNumber("Auto Sequence", 0);
-  printf("Initialized");
+  printf("Initialized\n");
 }
 void DentRobot::RobotInit(){
   //SmartDashboard::PutNumber("CodeVersion",0.001);
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index a2322d4..f9b1f17 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -12,10 +12,10 @@ void Collector::InitDefaultCommand(){
 }
 void Collector::MoveRollers(double a){
   collectorMotorLeft->Set(a);
-  collectorMotorBottom->Set(a);
+  collectorMotorBottom->Set(-a);
   collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
-  printf("%f\n",GetSonarDistance());
+  printf("Roller power: %f\n",a);
 }
 double Collector::GetSonarDistance(){
   return sonarAnalog->GetAverageVoltage();

From e47555aecf38a7fc3b6f1b997c2647818e5f0f34 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Mon, 16 Feb 2015 07:10:31 -0500
Subject: [PATCH 18/18] Updated code version

---
 DentRobot.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/DentRobot.cpp b/DentRobot.cpp
index 42a8201..e1662e8 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -21,7 +21,7 @@ DentRobot::DentRobot(){
   printf("Initialized\n");
 }
 void DentRobot::RobotInit(){
-  //SmartDashboard::PutNumber("CodeVersion",0.001);
+  //SmartDashboard::PutNumber("CodeVersion",1.0);
 }
 void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();