From 1ebb5661b04ffff9a9b82deb00439bec0b5c55af Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 7 Feb 2015 17:34:44 -0500
Subject: [PATCH] Stop elevator with button 12 and when inverse button is
 pressed

---
 Commands/Collector/CollectTote.cpp |  1 -
 Commands/Collector/ReleaseTote.cpp |  1 -
 OI.cpp                             | 19 ++++++++++++++-----
 Subsystems/Collector.cpp           |  1 -
 Subsystems/Elevator.cpp            |  1 -
 5 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp
index cbd5923..eca5210 100644
--- a/Commands/Collector/CollectTote.cpp
+++ b/Commands/Collector/CollectTote.cpp
@@ -7,7 +7,6 @@ void CollectTote::Initialize(){
 }
 void CollectTote::Execute(){
   //TODO check this value to move the motors in the right direction
-  printf("collecting tote\n");
   DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool CollectTote::IsFinished(){
diff --git a/Commands/Collector/ReleaseTote.cpp b/Commands/Collector/ReleaseTote.cpp
index 222cfe7..e751c35 100644
--- a/Commands/Collector/ReleaseTote.cpp
+++ b/Commands/Collector/ReleaseTote.cpp
@@ -7,7 +7,6 @@ void ReleaseTote::Initialize(){
 }
 void ReleaseTote::Execute(){
   //TODO check this value to move the motors in the right direction
-  printf("releasing tote\n");
   // 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);
 }
diff --git a/OI.cpp b/OI.cpp
index f3a6d28..1a81fab 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -10,7 +10,6 @@ OI::OI() {
   // Joysticks
   leftStick=new Joystick(0);
   rightStick=new Joystick(1);
-
   // Collector
   JoystickButton *right1=new JoystickButton(rightStick, 1);
   JoystickButton *right2=new JoystickButton(rightStick, 2);
@@ -21,14 +20,24 @@ OI::OI() {
   left3->WhileHeld(new CollectTote());
   left4->WhileHeld(new ReleaseTote());
   // Elevator
+  Raise* raise=new Raise();
+  Lower* 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(new Lower());
-  right4->WhenPressed(new Lower());
-  right5->WhenPressed(new Raise());
-  right6->WhenPressed(new Raise());
+  right3->WhenPressed(lower);
+  right4->WhenPressed(lower);
+  right3->CancelWhenPressed(raise);
+  right4->CancelWhenPressed(raise);
+  right5->WhenPressed(raise);
+  right6->WhenPressed(raise);
+  right5->CancelWhenPressed(lower);
+  right6->CancelWhenPressed(lower);
+  // Cancel
+  JoystickButton *right12=new JoystickButton(rightStick, 12);
+  right12->CancelWhenPressed(raise);
+  right12->CancelWhenPressed(lower);
 }
 Joystick* OI::GetRightStick(){
   return rightStick;
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index fbcca5d..365809b 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -14,7 +14,6 @@ void Collector::MoveArms(double a){
   windowMotorRight->Set(-a);
 }
 void Collector::MoveRollers(double a){
-  printf("Collector: %f\n", a);
   collectorMotorLeft->Set(a);
   collectorMotorRight->Set(-a);
 }
diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp
index 281fb5e..f0ff41e 100644
--- a/Subsystems/Elevator.cpp
+++ b/Subsystems/Elevator.cpp
@@ -12,7 +12,6 @@ Elevator::Elevator(){
 void Elevator::InitDefaultCommand(){
 }
 void Elevator::Run(double power){
-  printf("Elevator Power: %f\n",power);
   motor->Set(power);
 }
 void Elevator::SetOffset(double ht){