From fd144410a962723b12ad026c0e944cec676f6525 Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Sun, 15 Feb 2015 13:36:19 +0000
Subject: [PATCH] Minor breakage (untested, needs testing, not working)

---
 Commands/Elevator/Raise.cpp | 19 ++++++++++---------
 RobotMap.h                  |  4 ++--
 Subsystems/Collector.cpp    |  2 +-
 3 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp
index c606bfc..d9d142f 100644
--- a/Commands/Elevator/Raise.cpp
+++ b/Commands/Elevator/Raise.cpp
@@ -10,15 +10,16 @@ void Raise::Execute(){
   DentRobot::elevator->Run(1.0);
 }
 bool Raise::IsFinished(){
-  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");
+  //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()) {
+  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
+    printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
     return true;
   }else{
     return false;
diff --git a/RobotMap.h b/RobotMap.h
index 0279c47..90aca69 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 8
-#define ELEVATOR_TOP_DIO 7
+#define ELEVATOR_MIDDLE_DIO 7
+#define ELEVATOR_TOP_DIO 8
 #define ELEVATOR_ENCODERA 4
 #define ELEVATOR_ENCODERB 9
 
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index f1e8a22..f585676 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -19,6 +19,6 @@ void Collector::MoveRollers(double a){
   GetSonarDistance();
 }
 float Collector::GetSonarDistance(){
-  printf("Sonar Distance %f\n",sonarAnalog->GetAverageValue());
+  printf("Sonar Distance %f\n",sonarAnalog->GetAverageVoltage());
 }
 // vim: ts=2:sw=2:et