commit 5e872c876c13aac7d5115f875a2f08527f549069
author Austin Schuh <austin@peloton-tech.com> Tue Feb 17 22:59:08 2015 -0800
committer Austin Schuh <austin@peloton-tech.com> Tue Feb 17 22:59:08 2015 -0800
tree cb83814eb64700a57c0668580e646ffbba3a2c33
parent d6fd9f28fa71d20299e48b2ec4d5001d1138691b

Fixed broken tests.

Change-Id: I04d64d0f1f8f93dc7fb7750c1a439afb9cc3a75e

frc971/control_loops/fridge/fridge.cc

diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 1bf1a4c..9452637 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -311,7 +311,7 @@
                           right_elevator_estimator_.offset());
         LOG(DEBUG, "Zeroed the elevator!\n");
 
-        if (elevator() > values.fridge.arm_zeroing_height &&
+        if (elevator() < values.fridge.arm_zeroing_height &&
             state_ != INITIALIZING) {
           // Move the elevator to a safe height before we start zeroing the arm,
           // so that we don't crash anything.
@@ -517,6 +517,8 @@
   status->zeroed = state_ == RUNNING;
   status->angle = arm_loop_->X_hat(0, 0);
   status->height = elevator_loop_->X_hat(0, 0);
+  status->goal_angle = arm_goal_;
+  status->goal_height = elevator_goal_;
   if (unsafe_goal) {
     status->grabbers = unsafe_goal->grabbers;
   } else {