commit e1bb13e7aeaeef8d8e0d63011039c5e7f7488eec
author Daniel Petti <djpetti@me.com> Tue Feb 17 13:59:15 2015 -0800
committer Daniel Petti <djpetti@me.com> Tue Feb 17 16:20:10 2015 -0800
tree d4e18b16cd0e8d770c6a98f3f43d4654e1e2c65f
parent 9fb2ab1b0c164fb5ddf9f9cf535269be1833501d

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

frc971/constants.cc

diff --git a/frc971/constants.cc b/frc971/constants.cc
index c68990f..43f5adf 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -55,6 +55,8 @@
     kElevatorGearboxOutputPulleyTeeth * kElevatorGearboxOutputPitch /
     (2.0 * M_PI);
 
+const double kArmZeroingHeight = 0.2;
+
 const double kMaxAllowedLeftRightArmDifference = 0.04;  // radians
 const double kMaxAllowedLeftRightElevatorDifference = 0.01;  // meters
 
@@ -147,6 +149,8 @@
             {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
             {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
             0.0, 0.0, 0.0, 0.0,
+
+            kArmZeroingHeight,
           },
           // End "sensor" values.
 
@@ -214,6 +218,8 @@
             {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
             {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
             0.0, 0.0, 0.0, 0.0,
+
+            kArmZeroingHeight,
           },
           // End "sensor" values.
 
@@ -280,6 +286,8 @@
            -0.078959636363636357 - 0.024646,
            -3.4952331578947375 + 0.011776,
            3.5263507647058816 - 0.018921 + 0.006545,
+
+           kArmZeroingHeight,
           },
           // TODO(sensors): End "sensor" values.