commit 9e37c3288ea51563b46f0945a4494e070fdd05c6
author Austin Schuh <austin@peloton-tech.com> Mon Feb 16 15:47:49 2015 -0800
committer Austin Schuh <austin@peloton-tech.com> Mon Feb 16 15:51:49 2015 -0800
tree d2f4b888222d5b9655a6f7621782293cdbefa414
parent b966c437ba08912098d83c01282aa795f0653f82

Added estimator state to status.

Change-Id: I541c772bfd159e5db5a8237aa948358e7d296e5b

frc971/control_loops/fridge/fridge.cc

diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index a76f06a..dae8374 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -187,10 +187,10 @@
       elevator_zeroing_velocity_ = kElevatorZeroingVelocity;
     }
   } else if (elevator_zeroing_velocity_ > 0 &&
-             estimated_elevator() > average_elevator + 2 * pulse_width) {
+             estimated_elevator() > average_elevator + 1.1 * pulse_width) {
     elevator_zeroing_velocity_ = -kElevatorZeroingVelocity;
   } else if (elevator_zeroing_velocity_ < 0 &&
-             estimated_elevator() < average_elevator - 2 * pulse_width) {
+             estimated_elevator() < average_elevator - 1.1 * pulse_width) {
     elevator_zeroing_velocity_ = kElevatorZeroingVelocity;
   }
   return elevator_zeroing_velocity_;
@@ -211,15 +211,24 @@
       arm_zeroing_velocity_ = kArmZeroingVelocity;
     }
   } else if (arm_zeroing_velocity_ > 0.0 &&
-             estimated_arm() > average_arm + 2.0 * pulse_width) {
+             estimated_arm() > average_arm + 1.1 * pulse_width) {
     arm_zeroing_velocity_ = -kArmZeroingVelocity;
   } else if (arm_zeroing_velocity_ < 0.0 &&
-             estimated_arm() < average_arm - 2.0 * pulse_width) {
+             estimated_arm() < average_arm - 1.1 * pulse_width) {
     arm_zeroing_velocity_ = kArmZeroingVelocity;
   }
   return arm_zeroing_velocity_;
 }
 
+namespace {
+void PopulateEstimatorState(const zeroing::ZeroingEstimator &estimator,
+                            EstimatorState *state) {
+  state->error = estimator.error();
+  state->zeroed = estimator.zeroed();
+  state->position = estimator.position();
+}
+}  // namespace
+
 void Fridge::RunIteration(const control_loops::FridgeQueue::Goal *unsafe_goal,
                           const control_loops::FridgeQueue::Position *position,
                           control_loops::FridgeQueue::Output *output,
@@ -509,6 +518,12 @@
     status->grabbers.bottom_front = false;
     status->grabbers.bottom_back = false;
   }
+  PopulateEstimatorState(left_arm_estimator_, &status->left_arm_state);
+  PopulateEstimatorState(right_arm_estimator_, &status->right_arm_state);
+  PopulateEstimatorState(left_elevator_estimator_,
+                         &status->left_elevator_state);
+  PopulateEstimatorState(right_elevator_estimator_,
+                         &status->right_elevator_state);
   status->estopped = (state_ == ESTOP);
   status->state = state_;
   last_state_ = state_;

frc971/control_loops/fridge/fridge.q

diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index e8105d6..cf69efc 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -12,6 +12,16 @@
   bool bottom_back;
 };
 
+// The internal state of the zeroing estimator.
+struct EstimatorState {
+  // If true, there has been a fatal error for the estimator.
+  bool error;
+  // If the joint has seen an index pulse and is zeroed.
+  bool zeroed;
+  // The estimated position of the joint.
+  double position;
+};
+
 queue_group FridgeQueue {
   implements aos.control_loops.ControlLoop;
 
@@ -60,6 +70,11 @@
 
     // The internal state of the state machine.
     int32_t state;
+
+    EstimatorState left_elevator_state;
+    EstimatorState right_elevator_state;
+    EstimatorState left_arm_state;
+    EstimatorState right_arm_state;
   };
 
   message Output {