Make arm zeroing safer.

Now it forces the elevator to go to a predefined "safe" height
before it zeroes the arm to avoid crashing the fridge.

Change-Id: I8b5197a6a1e3d75372756015026c5ea8d1ddddef
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 7596a75..1bf1a4c 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -18,6 +18,8 @@
 namespace {
 constexpr double kZeroingVoltage = 4.0;
 constexpr double kElevatorZeroingVelocity = 0.10;
+// What speed we move to our safe height at.
+constexpr double kElevatorSafeHeightVelocity = 0.2;
 constexpr double kArmZeroingVelocity = 0.20;
 }  // namespace
 
@@ -308,6 +310,20 @@
         SetElevatorOffset(left_elevator_estimator_.offset(),
                           right_elevator_estimator_.offset());
         LOG(DEBUG, "Zeroed the elevator!\n");
+
+        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.
+          LOG(DEBUG, "Moving elevator to safe height.\n");
+          elevator_goal_ += kElevatorSafeHeightVelocity *
+                            ::aos::controls::kLoopFrequency.ToSeconds();
+          elevator_goal_velocity = kElevatorSafeHeightVelocity;
+
+          state_ = ZEROING_ELEVATOR;
+          break;
+        }
+
       } else if (!disable) {
         elevator_goal_velocity = elevator_zeroing_velocity();
         elevator_goal_ += elevator_goal_velocity *