diff --git a/.OI.swp b/.OI.swp
new file mode 100644
index 0000000..2c43cc8
Binary files /dev/null and b/.OI.swp differ
diff --git a/.env b/.env
deleted file mode 100644
index fe3ac67..0000000
--- a/.env
+++ /dev/null
@@ -1,15 +0,0 @@
-function mb(){
-  vagrant ssh -c "cd /vagrant/src;make clean 2>&1 >/dev/null;make"
-}
-function mc(){
-  vagrant ssh -c "cd /vagrant/src;make clean 2>&1 >/dev/null"
-}
-function mk(){
-  vagrant ssh -c "cd /vagrant/src;make"
-}
-function me(){
-  vagrant ssh -c "cd /vagrant/src;make deploy"
-}
-function ma(){
-  vagrant ssh -c "cd /vagrant/src;make clean 2>&1 >/dev/null;make;make deploy"
-}
diff --git a/.gitmodules b/.gitmodules
deleted file mode 100644
index daab3dd..0000000
--- a/.gitmodules
+++ /dev/null
@@ -1,3 +0,0 @@
-[submodule "src/hhlib"]
-	path = src/hhlib
-  url = git@github.com:team2059/hhlib.git
diff --git a/CommandBase.cpp b/CommandBase.cpp
index 90e3d27..c4defc3 100644
--- a/CommandBase.cpp
+++ b/CommandBase.cpp
@@ -2,9 +2,11 @@
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
+#include "Subsystems/BinElevator.h"
 Drivetrain* CommandBase::drivetrain = NULL;
 Collector* CommandBase::collector = NULL;
 Elevator* CommandBase::elevator = NULL;
+BinElevator* CommandBase::binElevator = NULL;
 OI* CommandBase::oi = NULL;
 CommandBase::CommandBase(char const *name) : Command(name) {
 }
@@ -14,6 +16,7 @@ void CommandBase::init(){
   drivetrain = new Drivetrain();
   collector = new Collector();
   elevator = new Elevator();
+  binElevator = new BinElevator();
   oi = new OI();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/CommandBase.h b/CommandBase.h
index c1cb716..7443ebd 100644
--- a/CommandBase.h
+++ b/CommandBase.h
@@ -5,6 +5,7 @@
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
+#include "Subsystems/BinElevator.h"
 #include "OI.h"
 #include "WPILib.h"
 
@@ -16,7 +17,8 @@ class CommandBase: public Command {
     static Drivetrain *drivetrain;
     static Collector *collector;
     static Elevator *elevator;
+    static BinElevator *binElevator;
     static OI *oi;
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index e7f85fc..4d46525 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -1,16 +1,15 @@
 #include "AutoDrive.h"
-#include <cmath>
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
 AutoDrive::AutoDrive() : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(1.0);
+  SetTimeout(0.5);
 }
 void AutoDrive::Initialize(){
 }
 void AutoDrive::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(0.5,0,0,0.9,0);
+  DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
 }
 bool AutoDrive::IsFinished(){
   return IsTimedOut();
@@ -20,4 +19,4 @@ void AutoDrive::End(){
 void AutoDrive::Interrupted(){
   End();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h
index eaab4a2..26f2aa3 100644
--- a/Commands/Autonomous/AutoDrive.h
+++ b/Commands/Autonomous/AutoDrive.h
@@ -16,4 +16,4 @@ class AutoDrive: public Command{
     void Interrupted();
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index 43686c6..c1bff87 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -1,11 +1,22 @@
 #include "Autonomous.h"
 #include "Commands/CommandGroup.h"
 #include "../../DentRobot.h"
-#include "AutoDrive.h"
-#include "../../DentRobot.h"
 #include "../Elevator/Raise.h"
+#include "../Elevator/Lower.h"
+#include "AutoDrive.h"
+#include "Turn.h"
+#include "../Collector/CloseCollector.h"
+#include "../Collector/OpenCollector.h"
+#include "../Collector/CollectTote.h"
 Autonomous::Autonomous(){
-  AddSequential(new AutoDrive());
   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());
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Autonomous/Autonomous.h b/Commands/Autonomous/Autonomous.h
index 88cf308..43ca69c 100644
--- a/Commands/Autonomous/Autonomous.h
+++ b/Commands/Autonomous/Autonomous.h
@@ -11,4 +11,4 @@ class Autonomous: public CommandGroup{
     Autonomous();
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp
new file mode 100644
index 0000000..174f6cc
--- /dev/null
+++ b/Commands/Autonomous/Turn.cpp
@@ -0,0 +1,22 @@
+#include "Turn.h"
+#include "../../DentRobot.h"
+// Drive for a short while then stop. Just for testing
+Turn::Turn() : Command("Turn"){
+  Requires(DentRobot::drivetrain);
+  SetTimeout(0.25);
+}
+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);
+}
+bool Turn::IsFinished(){
+  return IsTimedOut();
+}
+void Turn::End(){
+}
+void Turn::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/Autonomous/Turn.h b/Commands/Autonomous/Turn.h
new file mode 100644
index 0000000..2adc701
--- /dev/null
+++ b/Commands/Autonomous/Turn.h
@@ -0,0 +1,19 @@
+#ifndef TURN_H
+#define TURN_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class Turn: public Command{
+  public:
+    Turn();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
+// vim: ts=2:sw=2:et
diff --git a/Commands/BinElevator/BinLower.cpp b/Commands/BinElevator/BinLower.cpp
new file mode 100644
index 0000000..158f98a
--- /dev/null
+++ b/Commands/BinElevator/BinLower.cpp
@@ -0,0 +1,26 @@
+#include "BinLower.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+BinLower::BinLower() : Command("BinLower"){
+}
+void BinLower::Initialize(){
+  SetTimeout(2.5);
+}
+void BinLower::Execute(){
+  DentRobot::binElevator->Run(-1.0);
+}
+bool BinLower::IsFinished(){
+  if (!DentRobot::binElevator->GetElevatorBottom()||IsTimedOut()){
+    printf("Robot stoped BinLowering. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorBottom());
+    return true;
+  }else{
+    return false;
+  }
+}
+void BinLower::End(){
+  DentRobot::binElevator->Run(0.0f);
+}
+void BinLower::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/Elevator/Calibrate.h b/Commands/BinElevator/BinLower.h
similarity index 61%
rename from Commands/Elevator/Calibrate.h
rename to Commands/BinElevator/BinLower.h
index 5d70334..a3504b4 100644
--- a/Commands/Elevator/Calibrate.h
+++ b/Commands/BinElevator/BinLower.h
@@ -1,12 +1,12 @@
-#ifndef CALIBRATE_H
-#define CALIBRATE_H
+#ifndef BINLOWER_H
+#define BINLOWER_H
 
 #include "Commands/Command.h"
 #include "WPILib.h"
 
-class Calibrate: public Command{
+class BinLower: public Command{
   public:
-    Calibrate();
+    BinLower();
     void Initialize();
     void Execute();
     bool IsFinished();
@@ -14,4 +14,4 @@ class Calibrate: public Command{
     void Interrupted();
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/BinElevator/BinRaise.cpp b/Commands/BinElevator/BinRaise.cpp
new file mode 100644
index 0000000..aa765a4
--- /dev/null
+++ b/Commands/BinElevator/BinRaise.cpp
@@ -0,0 +1,26 @@
+#include "BinRaise.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+BinRaise::BinRaise() : Command("BinRaise"){
+}
+void BinRaise::Initialize(){
+  SetTimeout(3.0);
+}
+void BinRaise::Execute(){
+  DentRobot::binElevator->Run(1.0);
+}
+bool BinRaise::IsFinished(){
+  if (!DentRobot::binElevator->GetElevatorTop()||IsTimedOut()){
+    printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorTop());
+    return true;
+  }else{
+    return false;
+  }
+}
+void BinRaise::End(){
+  DentRobot::binElevator->Run(0.0f);
+}
+void BinRaise::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/BinElevator/BinRaise.h b/Commands/BinElevator/BinRaise.h
new file mode 100644
index 0000000..ea73454
--- /dev/null
+++ b/Commands/BinElevator/BinRaise.h
@@ -0,0 +1,18 @@
+#ifndef BINRAISE_H
+#define BINRAISE_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class BinRaise: public Command{
+  public:
+    BinRaise();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CloseCollector.cpp b/Commands/Collector/CloseCollector.cpp
index 3b7a866..2418732 100644
--- a/Commands/Collector/CloseCollector.cpp
+++ b/Commands/Collector/CloseCollector.cpp
@@ -3,13 +3,21 @@ CloseCollector::CloseCollector() : Command("CloseCollector"){
   Requires(DentRobot::collector);
 }
 void CloseCollector::Initialize(){
-  SetTimeout(0.5);
+  printf("Initialized collector: 0.5\n");
+  SetTimeout(2.5);
 }
 void CloseCollector::Execute(){
-  DentRobot::collector->MoveArms(0.2f);
+  //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(){
-  return DentRobot::collector->ArmSensor();
+  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);
@@ -17,4 +25,4 @@ void CloseCollector::End(){
 void CloseCollector::Interrupted(){
   End();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CloseCollector.h b/Commands/Collector/CloseCollector.h
index 6be493a..cd26268 100644
--- a/Commands/Collector/CloseCollector.h
+++ b/Commands/Collector/CloseCollector.h
@@ -17,4 +17,4 @@ class CloseCollector: public Command{
 };
 
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
index eca5210..acabd4e 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/CollectTote.cpp
@@ -3,6 +3,7 @@ CollectTote::CollectTote() : Command("CollectTote"){
   Requires(DentRobot::collector);
 }
 void CollectTote::Initialize(){
+  printf("Initialized CollectTote\n");
   SetTimeout(2.0);
 }
 void CollectTote::Execute(){
@@ -10,7 +11,7 @@ void CollectTote::Execute(){
   DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool CollectTote::IsFinished(){
-  return DentRobot::collector->BoxCollected();
+  return DentRobot::collector->BoxCollected()||IsTimedOut();
 }
 void CollectTote::End(){
   DentRobot::collector->MoveRollers(0.0);
@@ -18,4 +19,4 @@ void CollectTote::End(){
 void CollectTote::Interrupted(){
   End();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CollectTote.h b/Commands/Collector/CollectTote.h
index de2d7db..03074fd 100644
--- a/Commands/Collector/CollectTote.h
+++ b/Commands/Collector/CollectTote.h
@@ -17,4 +17,4 @@ class CollectTote: public Command{
 };
 
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/OpenCollector.cpp b/Commands/Collector/OpenCollector.cpp
index 1ec498e..5cebc3e 100644
--- a/Commands/Collector/OpenCollector.cpp
+++ b/Commands/Collector/OpenCollector.cpp
@@ -6,11 +6,12 @@ void OpenCollector::Initialize(){
   SetTimeout(0.5);
 }
 void OpenCollector::Execute(){
-  //TODO check this value to move the motors in the right direction
-  DentRobot::collector->MoveArms(-0.2f);
+  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 DentRobot::collector->ArmSensor();
+  return IsTimedOut();
 }
 void OpenCollector::End(){
   DentRobot::collector->MoveArms(0.0f);
@@ -18,4 +19,4 @@ void OpenCollector::End(){
 void OpenCollector::Interrupted(){
   End();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/OpenCollector.h b/Commands/Collector/OpenCollector.h
index 47ae5f5..2686251 100644
--- a/Commands/Collector/OpenCollector.h
+++ b/Commands/Collector/OpenCollector.h
@@ -17,4 +17,4 @@ class OpenCollector: public Command{
 };
 
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/ReleaseTote.cpp b/Commands/Collector/ReleaseTote.cpp
index e751c35..744efcb 100644
--- a/Commands/Collector/ReleaseTote.cpp
+++ b/Commands/Collector/ReleaseTote.cpp
@@ -19,4 +19,4 @@ void ReleaseTote::End(){
 void ReleaseTote::Interrupted(){
   End();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/ReleaseTote.h b/Commands/Collector/ReleaseTote.h
index 521b7da..722680f 100644
--- a/Commands/Collector/ReleaseTote.h
+++ b/Commands/Collector/ReleaseTote.h
@@ -17,4 +17,4 @@ class ReleaseTote: public Command{
 };
 
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Drivetrain/Drive.cpp b/Commands/Drivetrain/Drive.cpp
index dfa803b..3b91bae 100644
--- a/Commands/Drivetrain/Drive.cpp
+++ b/Commands/Drivetrain/Drive.cpp
@@ -1,5 +1,4 @@
 #include "Drive.h"
-#include <cmath>
 #include "../../DentRobot.h"
 Drive::Drive() : Command("Drive"){
   Requires(DentRobot::drivetrain);
@@ -12,12 +11,12 @@ void Drive::Execute(){
   x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
   z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
   y = DentRobot::oi->GetLeftStick()->GetRawAxis(1);
-  if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
-    x=0;
-  }
-  if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){
-    y=0;
-  }
+  //if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
+  //  x=0;
+  //}
+  //if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){
+  //  y=0;
+  //}
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
   DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
 }
@@ -29,4 +28,4 @@ void Drive::End(){
 void Drive::Interrupted(){
   End();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Drivetrain/Drive.h b/Commands/Drivetrain/Drive.h
index 17583fa..17e0ab8 100644
--- a/Commands/Drivetrain/Drive.h
+++ b/Commands/Drivetrain/Drive.h
@@ -16,4 +16,4 @@ class Drive: public Command{
     void Interrupted();
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Elevator/Calibrate.cpp b/Commands/Elevator/Calibrate.cpp
deleted file mode 100644
index 87f2c25..0000000
--- a/Commands/Elevator/Calibrate.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-#include "Calibrate.h"
-#include "../../DentRobot.h"
-// Lowers elevator until it hits the limit switch then sets the height of the elevator to the height of the limit switches
-Calibrate::Calibrate() : Command("Calibrate"){
-}
-void Calibrate::Initialize(){
-}
-void Calibrate::Execute(){
-  // Lower collector until End()
-  DentRobot::elevator->Run(-0.4f);
-}
-bool Calibrate::IsFinished(){
-  if(DentRobot::elevator->GetElevatorBottom()){
-    DentRobot::elevator->ResetEncoder();
-    DentRobot::elevator->SetOffset(0.99);
-    return true;
-  }
-  return false;
-}
-void Calibrate::End(){
-  DentRobot::elevator->Run(0.0f);
-}
-void Calibrate::Interrupted(){
-  End();
-}
-// vim: ts2:sw=2:et
diff --git a/Commands/Elevator/Lower.cpp b/Commands/Elevator/Lower.cpp
index 35d2159..378156d 100644
--- a/Commands/Elevator/Lower.cpp
+++ b/Commands/Elevator/Lower.cpp
@@ -4,7 +4,7 @@
 Lower::Lower() : Command("Lower"){
 }
 void Lower::Initialize(){
-  SetTimeout(1.0);
+  SetTimeout(2.5);
 }
 void Lower::Execute(){
   DentRobot::elevator->Run(-1.0);
@@ -23,4 +23,4 @@ void Lower::End(){
 void Lower::Interrupted(){
   End();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Elevator/Lower.h b/Commands/Elevator/Lower.h
index 3a83b57..92c8535 100644
--- a/Commands/Elevator/Lower.h
+++ b/Commands/Elevator/Lower.h
@@ -14,4 +14,4 @@ class Lower: public Command{
     void Interrupted();
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp
index 050b571..cd8885f 100644
--- a/Commands/Elevator/Raise.cpp
+++ b/Commands/Elevator/Raise.cpp
@@ -4,7 +4,7 @@
 Raise::Raise() : Command("Raise"){
 }
 void Raise::Initialize(){
-  SetTimeout(2.5);
+  SetTimeout(3.0);
 }
 void Raise::Execute(){
   DentRobot::elevator->Run(1.0);
@@ -18,9 +18,13 @@ 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(){
   End();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Commands/Elevator/Raise.h b/Commands/Elevator/Raise.h
index abc519b..b19f470 100644
--- a/Commands/Elevator/Raise.h
+++ b/Commands/Elevator/Raise.h
@@ -15,4 +15,4 @@ class Raise: public Command{
 };
 
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/DentRobot.cpp b/DentRobot.cpp
index 77c6a8f..e801009 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -1,16 +1,21 @@
 #include "DentRobot.h"
+#include "OI.h"
 #include "Commands/Autonomous/Autonomous.h"
 OI* DentRobot::oi=NULL;
 Collector* DentRobot::collector=NULL;
 Drivetrain* DentRobot::drivetrain=NULL;
 Elevator* DentRobot::elevator=NULL;
+BinElevator* DentRobot::binElevator=NULL;
 CommandGroup* DentRobot::aut=NULL;
 DentRobot::DentRobot(){
   oi=new OI();
   collector=new Collector();
   drivetrain=new Drivetrain();
   elevator=new Elevator();
+  binElevator=new BinElevator();
   aut=new Autonomous();
+  CameraServer::GetInstance()->SetQuality(25);
+  CameraServer::GetInstance()->StartAutomaticCapture("cam0");
   printf("Initialized");
 }
 void DentRobot::RobotInit(){
@@ -28,14 +33,19 @@ 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(){
 }
 START_ROBOT_CLASS(DentRobot);
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/DentRobot.h b/DentRobot.h
index e089405..68b17ba 100644
--- a/DentRobot.h
+++ b/DentRobot.h
@@ -3,6 +3,7 @@
 #include "WPILib.h"
 #include "OI.h"
 #include "Subsystems/Elevator.h"
+#include "Subsystems/BinElevator.h"
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
 #include "Commands/Autonomous/Autonomous.h"
@@ -15,6 +16,7 @@ class DentRobot: public IterativeRobot {
     static Collector* collector;
     static Drivetrain* drivetrain;
     static Elevator* elevator;
+    static BinElevator* binElevator;
     static CommandGroup* aut;
     void RobotInit();
     void DisabledPeriodic();
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:
diff --git a/OI.cpp b/OI.cpp
index be74011..c9819f0 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -14,15 +14,15 @@ OI::OI() {
   // Collector
   JoystickButton *right1=new JoystickButton(rightStick, 1);
   JoystickButton *right2=new JoystickButton(rightStick, 2);
-  JoystickButton *left3=new JoystickButton(leftStick, 3);
-  JoystickButton *left4=new JoystickButton(leftStick, 4);
-  right1->WhileHeld(new OpenCollector());
-  right2->WhileHeld(new CloseCollector());
-  left3->WhileHeld(new CollectTote());
-  left4->WhileHeld(new ReleaseTote());
+  JoystickButton *left1=new JoystickButton(leftStick, 1);
+  JoystickButton *left2=new JoystickButton(leftStick, 2);
+  right1->WhileHeld(new CloseCollector());
+  right2->WhileHeld(new OpenCollector());
+  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);
@@ -50,4 +50,4 @@ Joystick* OI::GetRightStick(){
 Joystick* OI::GetLeftStick(){
   return leftStick;
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/OI.h b/OI.h
index 6bdbe19..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: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/RobotMap.h b/RobotMap.h
index 3e866a5..a3abe10 100644
--- a/RobotMap.h
+++ b/RobotMap.h
@@ -8,9 +8,17 @@
 #define ELEVATOR_BOTTOM_DIO 0
 #define ELEVATOR_COLELCT_TOTE_DIO 1
 #define ELEVATOR_READY_TOTE_DIO 2
-#define ELEVATOR_COLELCT_CAN_DIO 3
-#define ELEVATOR_READY_CAN_DIO 4
 #define ELEVATOR_TOP_DIO 5
+#define ELEVATOR_ENCODERA 0
+#define ELEVATOR_ENCODERB 1
+
+// 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
 
 // Drivetrain
 #define DRIVE_FRONT_LEFT_CAN 2
@@ -22,7 +30,8 @@
 #define COLLECTOR_WINDOW_LEFT_CAN 6
 #define COLLECTOR_WINDOW_RIGHT_CAN 7
 #define COLLECTOR_LEFT_CAN 8
+#define COLLECTOR_BOTTOM_CAN 10
 #define COLLECTOR_RIGHT_CAN 9
 
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Subsystems/BinElevator.cpp b/Subsystems/BinElevator.cpp
new file mode 100644
index 0000000..daff79b
--- /dev/null
+++ b/Subsystems/BinElevator.cpp
@@ -0,0 +1,26 @@
+#include "BinElevator.h"
+#include "../RobotMap.h"
+BinElevator::BinElevator(){
+  motor=new CANTalon(BINELEVATOR_CAN);
+  elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA,BINELEVATOR_ENCODERB,false);
+  elevatorBottom=new DigitalInput(BINELEVATOR_BOTTOM_DIO);
+  elevatorTop=new DigitalInput(BINELEVATOR_TOP_DIO);
+}
+void BinElevator::InitDefaultCommand(){
+}
+void BinElevator::Run(double power){
+  motor->Set(power);
+}
+void BinElevator::ResetEncoder(){
+  elevatorEncoder->Reset();
+}
+double BinElevator::GetHeight(){
+  return elevatorEncoder->Get();
+}
+bool BinElevator::GetElevatorBottom(){
+  return elevatorBottom->Get();
+}
+bool BinElevator::GetElevatorTop(){
+  return elevatorTop->Get();
+}
+// vim: ts=2:sw=2:et
diff --git a/Subsystems/BinElevator.h b/Subsystems/BinElevator.h
new file mode 100644
index 0000000..8814152
--- /dev/null
+++ b/Subsystems/BinElevator.h
@@ -0,0 +1,22 @@
+#ifndef BINELEVATOR_H
+#define BINELEVATOR_H
+
+#include "WPILib.h"
+#include "Commands/PIDSubsystem.h"
+class BinElevator{
+  private:
+    CANTalon *motor;
+    Encoder *elevatorEncoder;
+    static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
+    DigitalInput *elevatorBottom, *elevatorTop;
+  public:
+    BinElevator();
+    void InitDefaultCommand();
+    void Run(double);
+    void ResetEncoder();
+    double GetHeight();
+    bool GetElevatorTop();
+    bool GetElevatorBottom();
+};
+#endif
+// vim: ts=2:sw=2:et
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index 365809b..fcc7959 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -5,6 +5,7 @@ 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() {
@@ -15,6 +16,7 @@ void Collector::MoveArms(double a){
 }
 void Collector::MoveRollers(double a){
   collectorMotorLeft->Set(a);
+  collectorMotorBottom->Set(a);
   collectorMotorRight->Set(-a);
 }
 bool Collector::ArmSensor(){
@@ -25,4 +27,4 @@ bool Collector::BoxCollected(){
   return false;
   //return boxSwitch->Get();
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h
index 24dcc1d..92262ac 100644
--- a/Subsystems/Collector.h
+++ b/Subsystems/Collector.h
@@ -5,7 +5,7 @@
 class Collector: public Subsystem
 {
   private:
-    CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorRight;
+    CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRight;
   public:
     Collector();
     void InitDefaultCommand();
@@ -15,4 +15,4 @@ class Collector: public Subsystem
     bool BoxCollected();
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp
index e6077da..0e4970f 100644
--- a/Subsystems/Drivetrain.cpp
+++ b/Subsystems/Drivetrain.cpp
@@ -40,4 +40,4 @@ void Drivetrain::TestMotor(e_motors motor, float power){
       break;
   }
 }
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h
index 0d0e088..15b9d61 100644
--- a/Subsystems/Drivetrain.h
+++ b/Subsystems/Drivetrain.h
@@ -20,4 +20,4 @@ class Drivetrain: public Subsystem{
     void TestMotor(e_motors,float);
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp
index f0ff41e..ffe24c3 100644
--- a/Subsystems/Elevator.cpp
+++ b/Subsystems/Elevator.cpp
@@ -2,26 +2,26 @@
 #include "../RobotMap.h"
 Elevator::Elevator(){
   motor=new CANTalon(ELEVATOR_CAN);
-  elevatorEncoder=new Encoder(0,1,false);
-  offset=0;
-  height=0;
+  elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
   elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
   elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
-  //SetAbsoluteTolerance(0.004);
+  // 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::SetOffset(double ht){
-  offset=ht;
-}
 void Elevator::ResetEncoder(){
   elevatorEncoder->Reset();
 }
 double Elevator::GetHeight(){
-  return elevatorEncoder->Get()+offset;
+  return elevatorEncoder->Get();
 }
 bool Elevator::GetElevatorBottom(){
   return elevatorBottom->Get();
@@ -29,4 +29,10 @@ bool Elevator::GetElevatorBottom(){
 bool Elevator::GetElevatorTop(){
   return elevatorTop->Get();
 }
-// vim: ts2:sw=2:et
+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 44c57de..5454063 100644
--- a/Subsystems/Elevator.h
+++ b/Subsystems/Elevator.h
@@ -8,17 +8,18 @@ class Elevator{
     CANTalon *motor;
     Encoder *elevatorEncoder;
     static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
-    double offset, height;
     DigitalInput *elevatorBottom, *elevatorTop;
+    bool useEncoder;
   public:
     Elevator();
     void InitDefaultCommand();
     void Run(double);
-    void SetOffset(double);
     void ResetEncoder();
     double GetHeight();
     bool GetElevatorTop();
     bool GetElevatorBottom();
+    void SetUseEncoder(bool);
+    bool GetUseEncoder();
 };
 #endif
-// vim: ts2:sw=2:et
+// vim: ts=2:sw=2:et
diff --git a/hhlib b/hhlib
deleted file mode 160000
index 787733b..0000000
--- a/hhlib
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 787733b3d4204d061620c7906af0cf4777c47e34