diff --git a/.gitignore b/.gitignore
index 88fbacb..75156d3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,7 @@
 *.o
 bin
 wpilib
+CMakeCache.txt
+CMakeFiles
+cmake_install.cmake
+vision
diff --git a/CommandBase.cpp b/CommandBase.cpp
index c4defc3..ef64dae 100644
--- a/CommandBase.cpp
+++ b/CommandBase.cpp
@@ -3,10 +3,12 @@
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
 #include "Subsystems/BinElevator.h"
+#include "Subsystems/Pneumatics.h"
 Drivetrain* CommandBase::drivetrain = NULL;
 Collector* CommandBase::collector = NULL;
 Elevator* CommandBase::elevator = NULL;
 BinElevator* CommandBase::binElevator = NULL;
+Pneumatics* CommandBase::pneumatics=NULL;
 OI* CommandBase::oi = NULL;
 CommandBase::CommandBase(char const *name) : Command(name) {
 }
@@ -17,6 +19,7 @@ void CommandBase::init(){
   collector = new Collector();
   elevator = new Elevator();
   binElevator = new BinElevator();
+  pneumatics = new Pneumatics();
   oi = new OI();
 }
 // vim: ts=2:sw=2:et
diff --git a/CommandBase.h b/CommandBase.h
index 7443ebd..cb7453b 100644
--- a/CommandBase.h
+++ b/CommandBase.h
@@ -6,6 +6,7 @@
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
 #include "Subsystems/BinElevator.h"
+#include "Subsystems/Pneumatics.h"
 #include "OI.h"
 #include "WPILib.h"
 
@@ -18,6 +19,7 @@ class CommandBase: public Command {
     static Collector *collector;
     static Elevator *elevator;
     static BinElevator *binElevator;
+    static Pneumatics *pneumatics;
     static OI *oi;
 };
 #endif
diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index 24fce24..ba14f03 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -1,32 +1,23 @@
 #include "AutoDrive.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-AutoDrive::AutoDrive(double wait) : Command("AutoDrive"){
+AutoDrive::AutoDrive(double duration, double xtmp=0, double ytmp=-0.75) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(wait);
-  power=5;
-}
-AutoDrive::AutoDrive(double wait, double p) : Command("AutoDrive"){
-  Requires(DentRobot::drivetrain);
-  SetTimeout(wait);
-  power=p;
+  SetTimeout(duration);
+  x=xtmp;
+  y=ytmp;
 }
 void AutoDrive::Initialize(){
 }
 void AutoDrive::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  //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(x, y, 0.0, 0.9, 0.0);
 }
 bool AutoDrive::IsFinished(){
   return IsTimedOut();
 }
 void AutoDrive::End(){
-  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9, 0.0);
 }
 void AutoDrive::Interrupted(){
   End();
diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h
index f2d4785..6fa66ef 100644
--- a/Commands/Autonomous/AutoDrive.h
+++ b/Commands/Autonomous/AutoDrive.h
@@ -8,10 +8,10 @@
 
 class AutoDrive: public Command{
   private:
-    double power;
+    double x, y;
   public:
     AutoDrive(double);
-    AutoDrive(double, double);
+    AutoDrive(double, double, double);
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index 236a6b8..d3339a1 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -1,45 +1,59 @@
 #include "Autonomous.h"
+#include "Commands/CommandGroup.h"
 #include "../../DentRobot.h"
 #include "../Elevator/Raise.h"
 #include "../Elevator/Lower.h"
+#include "../BinElevator/BinRaise.h"
+#include "../BinElevator/BinLower.h"
 #include "AutoDrive.h"
 #include "Turn.h"
 #include "../Collector/RollIn.h"
-#include "../Collector/CollectTote.h"
+#include "CollectTote.h"
+#include "ReleaseTote.h"
 Autonomous::Autonomous(int seq){
   //SmartDashboard::GetNumber("Auto Wait Time");
   switch(seq){
     case 0:
       // Just for testing
-      AddSequential(new CollectTote());
-      AddSequential(new Turn());
-      //AddSequential(new Raise());
-      //AddSequential(new Lower());
-      //AddSequential(new Turn());
-      //AddParallel(new AutoDrive(0.5));
-      //AddSequential(new RollIn());
-      //AddSequential(new Turn());
+      // Strafe at .25 power
+      AddSequential(new AutoDrive(0.5, 0.25, 0.0));
       break;
     case 1:
-      // Drive forward a bit, turn around, collect tote then bin
-      AddSequential(new AutoDrive(0.5));
-      AddSequential(new Turn());
-      AddSequential(new Turn());
-      AddSequential(new RollIn());
-      AddSequential(new Raise());
+      // Drive to Auto Zone (TM)
+      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8));
       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 RollIn());
+      // Collect a tote, turn, drive to Auto Zone (TM)
+      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
+      AddSequential(new BinRaise(1.2));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
+      AddSequential(new BinLower(1.0));
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       break;
     case 3:
-      // Wait a desigated value, drive to Auto Zone (TM)
-      //Wait(SmartDashboard::GetNumber("Auto Wait Time"));
-      AddSequential(new AutoDrive(2.0));
+      // Collect three totes, drive to Auto Zone (TM)
+      printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
+      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      printf("Done");
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
+      AddSequential(new CollectTote());
+      AddSequential(new Raise());
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
+      AddSequential(new CollectTote());
+      AddSequential(new Lower());
+      AddSequential(new Raise());
+      printf("Three totes?: %d\n", SmartDashboard::GetBoolean("Three totes"));
+      if(SmartDashboard::GetBoolean("Three totes")){
+        AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
+        AddSequential(new CollectTote());
+        AddSequential(new Lower());
+        AddSequential(new Raise());
+      }
+      AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, 0.75));
+      AddSequential(new ReleaseTote());
       break;
     default:
       printf("Invalid seq: %d\n", seq);
diff --git a/Commands/Autonomous/CollectTote.cpp b/Commands/Autonomous/CollectTote.cpp
new file mode 100644
index 0000000..5ee7deb
--- /dev/null
+++ b/Commands/Autonomous/CollectTote.cpp
@@ -0,0 +1,9 @@
+#include "CollectTote.h"
+#include "../../DentRobot.h"
+#include "AutoDrive.h"
+#include "../Collector/RollIn.h"
+CollectTote::CollectTote(){
+  AddParallel(new AutoDrive(1.0, -0.75, 0.0));
+  AddSequential(new RollIn(1.0));
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/Collector/CollectTote.h b/Commands/Autonomous/CollectTote.h
similarity index 100%
rename from Commands/Collector/CollectTote.h
rename to Commands/Autonomous/CollectTote.h
diff --git a/Commands/Collector/ReleaseTote.cpp b/Commands/Autonomous/ReleaseTote.cpp
similarity index 57%
rename from Commands/Collector/ReleaseTote.cpp
rename to Commands/Autonomous/ReleaseTote.cpp
index c10ddd2..8d01114 100644
--- a/Commands/Collector/ReleaseTote.cpp
+++ b/Commands/Autonomous/ReleaseTote.cpp
@@ -1,9 +1,9 @@
 #include "ReleaseTote.h"
 #include "../../DentRobot.h"
-#include "RollOut.h"
-#include "../Autonomous/AutoDrive.h"
+#include "AutoDrive.h"
+#include "../Collector/RollOut.h"
 ReleaseTote::ReleaseTote(){
   AddParallel(new RollOut());
-  AddParallel(new AutoDrive(0.5, 0.75));
+  AddParallel(new AutoDrive(0.5, 0, 0.75));
 }
 // vim: ts=2:sw=2:et
diff --git a/Commands/Collector/ReleaseTote.h b/Commands/Autonomous/ReleaseTote.h
similarity index 100%
rename from Commands/Collector/ReleaseTote.h
rename to Commands/Autonomous/ReleaseTote.h
diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp
index eea1450..11a51d9 100644
--- a/Commands/Autonomous/Turn.cpp
+++ b/Commands/Autonomous/Turn.cpp
@@ -1,22 +1,22 @@
 #include "Turn.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-Turn::Turn() : Command("Turn"){
+Turn::Turn(int k) : Command("Turn"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(0.85);
+  SetTimeout(k);
 }
 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,1.0,0.9,0.0);
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9, 0.0);
 }
 bool Turn::IsFinished(){
   return IsTimedOut();
 }
 void Turn::End(){
   // Stop driving
-  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9, 0.0);
 }
 void Turn::Interrupted(){
   End();
diff --git a/Commands/Autonomous/Turn.h b/Commands/Autonomous/Turn.h
index 2adc701..b128c20 100644
--- a/Commands/Autonomous/Turn.h
+++ b/Commands/Autonomous/Turn.h
@@ -7,8 +7,10 @@
 #include "WPILib.h"
 
 class Turn: public Command{
+  private:
+    int degrees;
   public:
-    Turn();
+    Turn(int);
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/Commands/BinElevator/BinCloseArms.cpp b/Commands/BinElevator/BinCloseArms.cpp
new file mode 100644
index 0000000..1cb292f
--- /dev/null
+++ b/Commands/BinElevator/BinCloseArms.cpp
@@ -0,0 +1,21 @@
+#include "BinCloseArms.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+BinCloseArms::BinCloseArms() : Command("BinCloseArms"){
+}
+void BinCloseArms::Initialize(){
+  //Should never need to use this
+  SetTimeout(0.5);
+}
+void BinCloseArms::Execute(){
+  DentRobot::pneumatics->SetOpen(true);
+}
+bool BinCloseArms::IsFinished(){
+  return true;
+}
+void BinCloseArms::End(){
+}
+void BinCloseArms::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/BinElevator/BinCloseArms.h b/Commands/BinElevator/BinCloseArms.h
new file mode 100644
index 0000000..b6322b9
--- /dev/null
+++ b/Commands/BinElevator/BinCloseArms.h
@@ -0,0 +1,18 @@
+#ifndef BINCLOSEARMS_H
+#define BINCLOSEARMS_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class BinCloseArms: public Command{
+  public:
+    BinCloseArms();
+    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
index b4001d8..ee2bc43 100644
--- a/Commands/BinElevator/BinLower.cpp
+++ b/Commands/BinElevator/BinLower.cpp
@@ -1,10 +1,11 @@
 #include "BinLower.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinLower::BinLower() : Command("BinLower"){
+BinLower::BinLower(float t) : Command("BinLower"){
+  timeout=t;
 }
 void BinLower::Initialize(){
-  SetTimeout(2.5);
+  SetTimeout(timeout);
 }
 void BinLower::Execute(){
   DentRobot::binElevator->Run(-1.0);
diff --git a/Commands/BinElevator/BinLower.h b/Commands/BinElevator/BinLower.h
index a3504b4..49813b0 100644
--- a/Commands/BinElevator/BinLower.h
+++ b/Commands/BinElevator/BinLower.h
@@ -5,8 +5,10 @@
 #include "WPILib.h"
 
 class BinLower: public Command{
+  private:
+    float timeout;
   public:
-    BinLower();
+    BinLower(float);
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/Commands/BinElevator/BinOpenArms.cpp b/Commands/BinElevator/BinOpenArms.cpp
new file mode 100644
index 0000000..c8936a4
--- /dev/null
+++ b/Commands/BinElevator/BinOpenArms.cpp
@@ -0,0 +1,21 @@
+#include "BinOpenArms.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+BinOpenArms::BinOpenArms() : Command("BinOpenArms"){
+}
+void BinOpenArms::Initialize(){
+  //Should never need to use this
+  SetTimeout(0.5);
+}
+void BinOpenArms::Execute(){
+  DentRobot::pneumatics->SetOpen(true);
+}
+bool BinOpenArms::IsFinished(){
+  return true;
+}
+void BinOpenArms::End(){
+}
+void BinOpenArms::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/BinElevator/BinOpenArms.h b/Commands/BinElevator/BinOpenArms.h
new file mode 100644
index 0000000..533d71c
--- /dev/null
+++ b/Commands/BinElevator/BinOpenArms.h
@@ -0,0 +1,18 @@
+#ifndef BINOPENARMS_H
+#define BINOPENARMS_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class BinOpenArms: public Command{
+  public:
+    BinOpenArms();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et
diff --git a/Commands/BinElevator/BinRaise.cpp b/Commands/BinElevator/BinRaise.cpp
index 1fe0148..21f46f7 100644
--- a/Commands/BinElevator/BinRaise.cpp
+++ b/Commands/BinElevator/BinRaise.cpp
@@ -1,10 +1,11 @@
 #include "BinRaise.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-BinRaise::BinRaise() : Command("BinRaise"){
+BinRaise::BinRaise(float t) : Command("BinRaise"){
+  timeout=t;
 }
 void BinRaise::Initialize(){
-  SetTimeout(3.0);
+  SetTimeout(timeout);
 }
 void BinRaise::Execute(){
   DentRobot::binElevator->Run(1.0);
diff --git a/Commands/BinElevator/BinRaise.h b/Commands/BinElevator/BinRaise.h
index ea73454..d173430 100644
--- a/Commands/BinElevator/BinRaise.h
+++ b/Commands/BinElevator/BinRaise.h
@@ -5,8 +5,10 @@
 #include "WPILib.h"
 
 class BinRaise: public Command{
+  private:
+    float timeout;
   public:
-    BinRaise();
+    BinRaise(float);
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
deleted file mode 100644
index 6b728f3..0000000
--- a/Commands/Collector/CollectTote.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include "CollectTote.h"
-#include "../../DentRobot.h"
-#include "../Autonomous/AutoDrive.h"
-#include "RollIn.h"
-CollectTote::CollectTote(){
-  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 e4384f2..4d3e7c3 100644
--- a/Commands/Collector/RollIn.cpp
+++ b/Commands/Collector/RollIn.cpp
@@ -1,17 +1,17 @@
 #include "RollIn.h"
-RollIn::RollIn() : Command("RollIn"){
+RollIn::RollIn(double k) : Command("RollIn"){
+  rawSpeed=k;
 }
 void RollIn::Initialize(){
   printf("Initialized RollIn\n");
   SetTimeout(2.0);
 }
 void RollIn::Execute(){
-  double throttle=DentRobot::oi->GetLeftThrottle();
-  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4;
+  double cvt=(rawSpeed)*DentRobot::collector->GetSonarDistance()/0.4;
   if(cvt>=1.0){
     DentRobot::collector->MoveRollers(1.0);
   }else{
-    DentRobot::collector->MoveRollers(cvt);
+    DentRobot::collector->MoveRollers(cvt*1.5);
   }
 }
 bool RollIn::IsFinished(){
diff --git a/Commands/Collector/RollIn.h b/Commands/Collector/RollIn.h
index 8a222aa..25e4924 100644
--- a/Commands/Collector/RollIn.h
+++ b/Commands/Collector/RollIn.h
@@ -7,8 +7,10 @@
 #include "WPILib.h"
 
 class RollIn: public Command{
+  private:
+    double rawSpeed;
   public:
-    RollIn();
+    RollIn(double);
     void Initialize();
     void Execute();
     bool IsFinished();
diff --git a/Commands/Collector/RollOut.cpp b/Commands/Collector/RollOut.cpp
index bc9ec90..d3f5150 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->GetLeftThrottle());
+  DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle() * 0.8);
 }
 bool RollOut::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Drivetrain/Drive.cpp b/Commands/Drivetrain/Drive.cpp
index a8c1241..553302f 100644
--- a/Commands/Drivetrain/Drive.cpp
+++ b/Commands/Drivetrain/Drive.cpp
@@ -6,11 +6,11 @@ Drive::Drive() : Command("Drive"){
 void Drive::Initialize(){
 }
 void Drive::Execute(){
-  double x,y,z;
-  //Code to lock the x axis when not holding button 1
+  double x, y, z;
   x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
+  y = -DentRobot::oi->GetLeftStick()->GetRawAxis(1);
   z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
-  y = DentRobot::oi->GetLeftStick()->GetRawAxis(1);
+  //Code to lock the x axis when not holding button 1
   //if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
   //  x=0;
   //}
@@ -18,7 +18,7 @@ void Drive::Execute(){
   //  y=0;
   //}
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
-  DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
+  DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, 0.0);
 }
 bool Drive::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Elevator/Lower.cpp b/Commands/Elevator/Lower.cpp
index 378156d..637a88b 100644
--- a/Commands/Elevator/Lower.cpp
+++ b/Commands/Elevator/Lower.cpp
@@ -4,7 +4,7 @@
 Lower::Lower() : Command("Lower"){
 }
 void Lower::Initialize(){
-  SetTimeout(2.5);
+  SetTimeout(3.0);
 }
 void Lower::Execute(){
   DentRobot::elevator->Run(-1.0);
diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp
index 817ed4d..ce42ab8 100644
--- a/Commands/Elevator/Raise.cpp
+++ b/Commands/Elevator/Raise.cpp
@@ -4,7 +4,7 @@
 Raise::Raise() : Command("Raise"){
 }
 void Raise::Initialize(){
-  SetTimeout(3.0);
+  SetTimeout(3.5);
 }
 void Raise::Execute(){
   DentRobot::elevator->Run(1.0);
diff --git a/Commands/Test/CheckDrive.cpp b/Commands/Test/CheckDrive.cpp
new file mode 100644
index 0000000..8ce3b65
--- /dev/null
+++ b/Commands/Test/CheckDrive.cpp
@@ -0,0 +1,38 @@
+#include "CheckDrive.h"
+#include <cmath>
+#include "Commands/CommandGroup.h"
+#include "../../DentRobot.h"
+#include "../../RobotMap.h"
+CheckDrive::CheckDrive(int motorID) : CommandGroup("CheckDrive"){
+  Requires(DentRobot::drivetrain);
+  motor = motorID;
+}
+void CheckDrive::Initialize(){
+  SetTimeout(1.0);
+}
+void CheckDrive::Execute(){
+  switch(motor){
+    case DRIVE_FRONT_LEFT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT, 1);
+      break;
+    case DRIVE_FRONT_RIGHT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT, 1);
+      break;
+    case DRIVE_BACK_LEFT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT, 1);
+      break;
+    case DRIVE_BACK_RIGHT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT, 1);
+      break;
+    default:
+      break;
+  }
+}
+bool CheckDrive::IsFinished(){
+  return false;
+}
+void CheckDrive::End(){
+}
+void CheckDrive::Interrupted(){
+  End();
+}
diff --git a/Commands/Test/CheckDrive.h b/Commands/Test/CheckDrive.h
new file mode 100644
index 0000000..ec76d13
--- /dev/null
+++ b/Commands/Test/CheckDrive.h
@@ -0,0 +1,20 @@
+#ifndef CHECKDRIVE_H
+#define CHECKDRIVE_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CheckDrive: public CommandGroup{
+  private:
+    int motor;
+  public:
+    CheckDrive(int);
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
diff --git a/Commands/Test/CheckRobot.cpp b/Commands/Test/CheckRobot.cpp
new file mode 100644
index 0000000..871c51b
--- /dev/null
+++ b/Commands/Test/CheckRobot.cpp
@@ -0,0 +1,13 @@
+#include "Commands/CommandGroup.h"
+#include "../../DentRobot.h"
+#include "../../RobotMap.h"
+#include "../Elevator/Raise.h"
+#include "CheckRobot.h"
+#include "CheckDrive.h"
+CheckRobot::CheckRobot(){
+  AddSequential(new CheckDrive(DRIVE_FRONT_LEFT_CAN));
+  AddSequential(new CheckDrive(DRIVE_FRONT_RIGHT_CAN));
+  AddSequential(new CheckDrive(DRIVE_BACK_LEFT_CAN));
+  AddSequential(new CheckDrive(DRIVE_BACK_RIGHT_CAN));
+}
+// vim: ts=2:sw=2:et
diff --git a/Commands/Test/CheckRobot.h b/Commands/Test/CheckRobot.h
new file mode 100644
index 0000000..d0df68e
--- /dev/null
+++ b/Commands/Test/CheckRobot.h
@@ -0,0 +1,14 @@
+#ifndef CHECKROBOT_H
+#define CHECKROBOT_H
+
+#include "Commands/CommandGroup.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CheckRobot: public CommandGroup{
+  public:
+    CheckRobot();
+};
+#endif
+// vim: ts=2:sw=2:et
diff --git a/DentRobot.cpp b/DentRobot.cpp
index e1662e8..0bd011e 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -1,5 +1,6 @@
 #include "DentRobot.h"
 #include "OI.h"
+#include "RobotMap.h"
 #include "Commands/Autonomous/Autonomous.h"
 OI* DentRobot::oi=NULL;
 Collector* DentRobot::collector=NULL;
@@ -7,31 +8,53 @@ Drivetrain* DentRobot::drivetrain=NULL;
 Elevator* DentRobot::elevator=NULL;
 BinElevator* DentRobot::binElevator=NULL;
 CommandGroup* DentRobot::aut=NULL;
+Pneumatics* DentRobot::pneumatics=NULL;
 DentRobot::DentRobot(){
   oi=new OI();
   collector=new Collector();
   drivetrain=new Drivetrain();
   elevator=new Elevator();
   binElevator=new BinElevator();
-  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\n");
+  pneumatics=new Pneumatics();
+  //CameraServer::GetInstance()->SetQuality(25);
+  //CameraServer::GetInstance()->StartAutomaticCapture("cam0");
+  printf("The robot is on\n");
 }
 void DentRobot::RobotInit(){
-  //SmartDashboard::PutNumber("CodeVersion",1.0);
+  SmartDashboard::PutNumber("CodeVersion", CODE_VERSION);
+  // Autonomous
+  // Sequence of autonomous command
+  SmartDashboard::PutNumber("Auto Sequence", 2.0);
+  SmartDashboard::PutNumber("Auto Wait Time", 3.0);
+  // If the robot will be picking up three totes in sequence 3
+  SmartDashboard::PutBoolean("Three totes", true);
+  // TODO: Calibrate the following two values
+  // Distance (in time) to auto zone
+  SmartDashboard::PutNumber("Auto Zone Distance", 2.8);
+  // Distance (in time) to auto tote (used in sequence 3)
+  SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
+  SmartDashboard::PutNumber("TurnAmount", 2.0);
+
+  // Elevators
+  SmartDashboard::PutBoolean("Bin Elevator Bottom", false);
+  SmartDashboard::PutBoolean("Bin Elevator Top", false);
+  SmartDashboard::PutBoolean("Elevator Bottom", false);
+  SmartDashboard::PutBoolean("Elevator Top", false);
+  //Drive speed
+  SmartDashboard::PutNumber("DriveSpeedReductionThresh",2);
 }
 void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::AutonomousInit(){
+  aut=new Autonomous(SmartDashboard::GetNumber("Auto Sequence"));
+  printf("Enabling Auto Sequence %f\n", SmartDashboard::GetNumber("Auto Sequence"));
   if(aut != NULL){
     aut->Start();
   }
 }
 void DentRobot::AutonomousPeriodic(){
+  printf("Running auto.\n");
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::TeleopInit(){
diff --git a/DentRobot.h b/DentRobot.h
index 68b17ba..59658f2 100644
--- a/DentRobot.h
+++ b/DentRobot.h
@@ -6,6 +6,7 @@
 #include "Subsystems/BinElevator.h"
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
+#include "Subsystems/Pneumatics.h"
 #include "Commands/Autonomous/Autonomous.h"
 class DentRobot: public IterativeRobot {
   private:
@@ -17,6 +18,7 @@ class DentRobot: public IterativeRobot {
     static Drivetrain* drivetrain;
     static Elevator* elevator;
     static BinElevator* binElevator;
+    static Pneumatics* pneumatics;
     static CommandGroup* aut;
     void RobotInit();
     void DisabledPeriodic();
diff --git a/Makefile b/Makefile
index 9bf4a49..56d6863 100644
--- a/Makefile
+++ b/Makefile
@@ -18,10 +18,10 @@ all : $(OBJECTS)
 clean:
 	$(CLEANSER) $(OBJECTS) bin/FRCUserProgram
 
-deploy: all
+deploy:
 	@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: all
+debug:
 	@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 b39da72..3dcfaa2 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -5,7 +5,11 @@
 #include "Commands/Collector/RollOut.h"
 #include "Commands/BinElevator/BinLower.h"
 #include "Commands/BinElevator/BinRaise.h"
-
+#include "Commands/BinElevator/BinCloseArms.h"
+#include "Commands/BinElevator/BinOpenArms.h"
+#include "Commands/Autonomous/CollectTote.h"
+#include "Commands/Autonomous/ReleaseTote.h"
+#include "Commands/Test/CheckRobot.h"
 OI::OI() {
   // Joysticks
   leftStick=new Joystick(0);
@@ -13,7 +17,7 @@ OI::OI() {
   // Collector
   JoystickButton *left1=new JoystickButton(leftStick, 1);
   JoystickButton *left2=new JoystickButton(leftStick, 2);
-  left1->WhileHeld(new RollIn());
+  left1->WhileHeld(new RollIn(GetLeftThrottle()));
   left2->WhileHeld(new RollOut());
   // Elevator
   raise=new Raise();
@@ -27,8 +31,12 @@ OI::OI() {
   // 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(new BinOpenArms());
+  //right8->WhenPressed(new BinCloseArms());
+  binRaise=new BinRaise(3.0);
+  binLower=new BinLower(2.0);
   right3->WhenPressed(binLower);
   right3->CancelWhenPressed(binRaise);
   right5->WhenPressed(binRaise);
@@ -39,6 +47,10 @@ OI::OI() {
   right12->CancelWhenPressed(lower);
   right12->CancelWhenPressed(binRaise);
   right12->CancelWhenPressed(binLower);
+  // Basic motor test
+  CheckRobot* checkRobot=new CheckRobot();
+  JoystickButton *left7=new JoystickButton(leftStick, 7);
+  left7->WhenPressed(checkRobot);
 }
 Joystick* OI::GetRightStick(){
   return rightStick;
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..f7b8d0f
--- /dev/null
+++ b/README.md
@@ -0,0 +1,29 @@
+# Dent
+
+Dent was designed to have a fast mecanum [drivetrain](Subsystems/Drivetrain.cpp) with ground clearance to traverse the scoring platforms with ease—all while carrying a stack of totes. A main [internal elevator](Subsystems/Elevator.cpp) lifts totes up to six high within the robot, allowing us to move quickly to anywhere on the field without tipping. The [intake system](Subsystems/Collector.cpp) features a ramp leading to the floor with an active roller pulling the totes up to two collector wheels on either side of the robot, both pulling the totes in, and centering them simultaneously. 
+
+But Dent does not stop there; a [taller elevator](Subsystems/BinElevator.cpp) on the back of the robot allows us to lift either recycle containers or totes to a greater height. With this, we can create stacks both internally and externally, with each system providing a backup of the other in case anything breaks.
+
+Dent is programmed in C++ and uses many sensors to determine what to do. An [ultrasonic sensor](Subsystems/Collector.cpp#L9) mounted on the back of the robot looking forward automatically slows down the collector wheels as the totes fly into the internal elevator. Homemade [hall effect sensors](Subsystems/Elevator.cpp#L6-L8) line the elevator shafts of both elevators, allowing the driver to raise totes and containers to pre-programmed heights. 
+
+All aspects of Dent’s design come together to produce a robot ready to rank in qualifications, and still provide a fast and capable design for elimination rounds. With all parts made an code written for Dent in-house, this truly is a robot designed by, built by, and programmed by the students on Team 2059, [The Hitchhikers](http://team2059.org/).
+
+
+### Controls
+##### Driver Main Joystick (USB 0)
+- X-Axis - Drive forwards and backwards
+- Y-Axis - Strafes left and right
+- Z-Axis - Turns left and right
+- Throttle-Axis - Adjusts collector speed
+- Button 1 - Collects totes
+- Button 2 - Dispenses totes
+- Button 7 - Enable robot test
+
+##### Driver Secondary Joystick (USB 1)
+- Button 3 - Lowers bin elevator
+- Button 4 - Lowers tote elevator
+- Button 5 - Raises bin elevator
+- Button 6 - Raises tote elevator
+- Button 7 - Opens bin arms
+- Button 8 - Closes bin arms
+- Button 12 - Universal cancel button
\ No newline at end of file
diff --git a/RobotMap.h b/RobotMap.h
index d797f4f..2f25d55 100644
--- a/RobotMap.h
+++ b/RobotMap.h
@@ -3,6 +3,8 @@
 
 #include "WPILib.h"
 
+#define CODE_VERSION 1.0
+
 // Elevator
 #define ELEVATOR_CAN 1
 #define ELEVATOR_BOTTOM_DIO 9
@@ -12,23 +14,25 @@
 #define ELEVATOR_ENCODERB 9
 
 // BinElevator
-#define BINELEVATOR_CAN 11
+#define BINELEVATOR_CAN 10
 #define BINELEVATOR_BOTTOM_DIO 6
 #define BINELEVATOR_COLELCT_BIN_DIO 7
-#define BINELEVATOR_TOP_DIO 8
+#define BINELEVATOR_TOP_DIO 12
 #define BINELEVATOR_ENCODERA 10
 #define BINELEVATOR_ENCODERB 11
+#define BINELEVATOR_SOLDENOID_ONE   6
+#define BINELEVATOR_SOLDENOID_TWO   7
 
 // Drivetrain
-#define DRIVE_FRONT_LEFT_CAN 2
+#define DRIVE_FRONT_LEFT_CAN 8
 #define DRIVE_BACK_LEFT_CAN 3
 #define DRIVE_FRONT_RIGHT_CAN 4
 #define DRIVE_BACK_RIGHT_CAN 5
 
 // Collector
 #define COLLECTOR_RAMP_CAN 7
-#define COLLECTOR_LEFT_CAN 8
-#define COLLECTOR_BOTTOM_CAN 10
+#define COLLECTOR_LEFT_CAN 2
+#define COLLECTOR_BOTTOM_CAN 11
 #define COLLECTOR_RIGHT_CAN 9
 #define COLLECTOR_SONAR_ANALOG 3
 
diff --git a/Subsystems/BinElevator.cpp b/Subsystems/BinElevator.cpp
index daff79b..ffc79ee 100644
--- a/Subsystems/BinElevator.cpp
+++ b/Subsystems/BinElevator.cpp
@@ -2,7 +2,7 @@
 #include "../RobotMap.h"
 BinElevator::BinElevator(){
   motor=new CANTalon(BINELEVATOR_CAN);
-  elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA,BINELEVATOR_ENCODERB,false);
+  elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA, BINELEVATOR_ENCODERB, false);
   elevatorBottom=new DigitalInput(BINELEVATOR_BOTTOM_DIO);
   elevatorTop=new DigitalInput(BINELEVATOR_TOP_DIO);
 }
@@ -18,9 +18,11 @@ double BinElevator::GetHeight(){
   return elevatorEncoder->Get();
 }
 bool BinElevator::GetElevatorBottom(){
+  SmartDashboard::PutBoolean("Bin Elevator Bottom", elevatorBottom->Get());
   return elevatorBottom->Get();
 }
 bool BinElevator::GetElevatorTop(){
+  SmartDashboard::PutBoolean("Bin Elevator Top", elevatorTop->Get());
   return elevatorTop->Get();
 }
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index f9b1f17..7e5296a 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -15,7 +15,7 @@ void Collector::MoveRollers(double a){
   collectorMotorBottom->Set(-a);
   collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
-  printf("Roller power: %f\n",a);
+  printf("Roller power: %f\n", a);
 }
 double Collector::GetSonarDistance(){
   return sonarAnalog->GetAverageVoltage();
diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp
index 6440679..c56b463 100644
--- a/Subsystems/Drivetrain.cpp
+++ b/Subsystems/Drivetrain.cpp
@@ -12,12 +12,35 @@ void Drivetrain::InitDefaultCommand(){
   SetDefaultCommand(new Drive());
 }
 void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){
-  double correctY = (sensitivity*(pow(y,3))+(1-sensitivity)*y);
-  double correctX = -(sensitivity*(pow(x,3))+(1-sensitivity)*x);
-  double correctZ = -z *.5;
+  double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
+  double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
+  double correctZ = -z * 0.5;
+  if (DentRobot::oi->GetLeftStick()->GetRawButton(9)){
+    correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
+  }
   rightFront->Set((-correctX + correctY - correctZ));
   leftFront->Set((correctX + correctY + correctZ)*-1);
   rightRear->Set((correctX + correctY - correctZ));
   leftRear->Set((-correctX + correctY + correctZ)*-1);
 }
+
+//Used in pretest
+void Drivetrain::TestMotor(e_motors motor, float power){
+  switch(motor){
+    case FRONTRIGHT:
+      rightFront->Set(power);
+      break;
+    case FRONTLEFT:
+      leftFront->Set(power);
+      break;
+    case BACKRIGHT:
+      rightRear->Set(power);
+      break;
+    case BACKLEFT:
+      leftRear->Set(power);
+      break;
+    default:
+      break;
+  }
+}
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h
index 3ab69b3..4550de8 100644
--- a/Subsystems/Drivetrain.h
+++ b/Subsystems/Drivetrain.h
@@ -8,9 +8,16 @@ class Drivetrain: public Subsystem{
     RobotDrive *drive;
   public:
     Drivetrain();
+    enum e_motors{
+      FRONTRIGHT,
+      FRONTLEFT,
+      BACKRIGHT,
+      BACKLEFT
+    };
     void InitDefaultCommand();
-    void DriveMecanum(float,float,float,float,float);
+    void DriveMecanum(float, float, float, float, float);
     void DriveArcade(float, float);
+    void TestMotor(e_motors, float);
 };
 #endif
 // vim: ts=2:sw=2:et
diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp
index 0649a44..af1e32a 100644
--- a/Subsystems/Elevator.cpp
+++ b/Subsystems/Elevator.cpp
@@ -2,7 +2,7 @@
 #include "../RobotMap.h"
 Elevator::Elevator(){
   motor=new CANTalon(ELEVATOR_CAN);
-  elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
+  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);
@@ -26,12 +26,14 @@ double Elevator::GetHeight(){
   return elevatorEncoder->Get();
 }
 bool Elevator::GetElevatorBottom(){
+  SmartDashboard::PutBoolean("Elevator Bottom", !elevatorBottom->Get());
   return elevatorBottom->Get();
 }
 bool Elevator::GetElevatorMiddle(){
   return elevatorMiddle->Get();
 }
 bool Elevator::GetElevatorTop(){
+  SmartDashboard::PutBoolean("Elevator Top", !elevatorTop->Get());
   return elevatorTop->Get();
 }
 void Elevator::SetUseEncoder(bool param){
diff --git a/Subsystems/Pneumatics.cpp b/Subsystems/Pneumatics.cpp
new file mode 100644
index 0000000..93dab86
--- /dev/null
+++ b/Subsystems/Pneumatics.cpp
@@ -0,0 +1,19 @@
+#include "Pneumatics.h"
+#include "../RobotMap.h"
+
+Pneumatics::Pneumatics() : Subsystem("Pneumatics"){
+  solenoid1 = new Solenoid(BINELEVATOR_SOLDENOID_ONE);
+  solenoid2 = new Solenoid(BINELEVATOR_SOLDENOID_TWO);
+}
+void Pneumatics::InitDefaultCommand(){
+}
+void Pneumatics::SetOpen(bool state){
+  if(state){
+    solenoid1->Set(true);
+    solenoid2->Set(false);
+  }else{
+    solenoid1->Set(false);
+    solenoid2->Set(true);
+  }
+}
+// vim: ts=2:sw=2:et
diff --git a/Subsystems/Pneumatics.h b/Subsystems/Pneumatics.h
new file mode 100644
index 0000000..9f57b72
--- /dev/null
+++ b/Subsystems/Pneumatics.h
@@ -0,0 +1,15 @@
+#ifndef PNEUMATICS_H
+#define PNEUMATICS_H
+
+#include "WPILib.h"
+class Pneumatics: public Subsystem
+{
+  private:
+    Solenoid *solenoid1, *solenoid2;
+  public:
+    Pneumatics();
+    void InitDefaultCommand();
+    void SetOpen(bool);
+};
+#endif
+// vim: ts=2:sw=2:et