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