commit 3cd11c5123056db87b2c9a98672a4fb9cd1baf4c
author Adam Snaider <adsnaider@gmail.com> Mon Feb 16 02:16:09 2015 +0000
committer Adam Snaider <adsnaider@gmail.com> Sun Mar 01 01:26:29 2015 +0000
tree 96356c01bd0401274a5593b11f4653545c428c97
parent 0a02d7d5c7148d4cc5480ee72225dca7c325a04d

Added detection of bad index zeroing pulse.

Change-Id: I1607ce0d37b336136a4a278afc8a0f2897d1c597

frc971/control_loops/fridge/fridge.cc

diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 46d4f28..a4aa2e7 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -23,7 +23,6 @@
 constexpr double kArmZeroingVelocity = 0.20;
 }  // namespace
 
-
 template <int S>
 void CappedStateFeedbackLoop<S>::CapU() {
   VoltageCap(max_voltage_, this->U(0, 0), this->U(1, 0), &this->mutable_U(0, 0),
@@ -67,10 +66,9 @@
       right_arm_estimator_.offset_ratio_ready() < 1.0) {
     state_ = INITIALIZING;
   } else if (!left_elevator_estimator_.zeroed() ||
-      !right_elevator_estimator_.zeroed()) {
+             !right_elevator_estimator_.zeroed()) {
     state_ = ZEROING_ELEVATOR;
-  } else if (!left_arm_estimator_.zeroed() ||
-      !right_arm_estimator_.zeroed()) {
+  } else if (!left_arm_estimator_.zeroed() || !right_arm_estimator_.zeroed()) {
     state_ = ZEROING_ARM;
   } else {
     state_ = RUNNING;
@@ -330,7 +328,7 @@
       } else if (!disable) {
         elevator_goal_velocity = elevator_zeroing_velocity();
         elevator_goal_ += elevator_goal_velocity *
-            ::aos::controls::kLoopFrequency.ToSeconds();
+                          ::aos::controls::kLoopFrequency.ToSeconds();
       }
       break;
 
@@ -346,8 +344,8 @@
         LOG(DEBUG, "Zeroed the arm!\n");
       } else if (!disable) {
         arm_goal_velocity = arm_zeroing_velocity();
-        arm_goal_ += arm_goal_velocity *
-            ::aos::controls::kLoopFrequency.ToSeconds();
+        arm_goal_ +=
+            arm_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
       }
       break;
 
@@ -530,14 +528,13 @@
     status->grabbers.bottom_front = false;
     status->grabbers.bottom_back = false;
   }
-  zeroing::PopulateEstimatorState(left_arm_estimator_,
-                                  &status->left_arm_state);
+  zeroing::PopulateEstimatorState(left_arm_estimator_, &status->left_arm_state);
   zeroing::PopulateEstimatorState(right_arm_estimator_,
                                   &status->right_arm_state);
   zeroing::PopulateEstimatorState(left_elevator_estimator_,
-                         &status->left_elevator_state);
+                                  &status->left_elevator_state);
   zeroing::PopulateEstimatorState(right_elevator_estimator_,
-                         &status->right_elevator_state);
+                                  &status->right_elevator_state);
   status->estopped = (state_ == ESTOP);
   status->state = state_;
   last_state_ = state_;