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();