commit ae9d4b7adda8031f12cb58c849e9191a70a05bfe
author Brian Silverman <brians> Sat Mar 16 20:11:29 2013 -0700
committer Brian Silverman <brians> Sat Mar 30 16:35:10 2013 -0700
tree d877de09c8b0d1f5215f8f6205691f37a04423d6
parent 7fb8e229ffb15eaea7cca0636e3b0d9007ce81c3

combination of bug fixes of Austin's and temp logs

frc971/control_loops/zeroed_joint.h

diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 582d48b..cd124de 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -77,19 +77,25 @@
   double limit = is_ready ?
       12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
 
-  voltage_ = std::min(limit, voltage_);
-  voltage_ = std::max(-limit, voltage_);
-  U(0, 0) = voltage_ - old_voltage;
-
   // Make sure that reality and the observer can't get too far off.  There is a
   // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
   // against last cycle's voltage.
   if (X_hat(2, 0) > last_voltage_ + 2.0) {
-    X_hat(2, 0) = last_voltage_ + 2.0;
+    //X_hat(2, 0) = last_voltage_ + 2.0;
+    voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
+    LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
   } else if (X_hat(2, 0) < last_voltage_ -2.0) {
-    X_hat(2, 0) = last_voltage_ - 2.0;
+    //X_hat(2, 0) = last_voltage_ - 2.0;
+    voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
+    LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
   }
 
+  voltage_ = std::min(limit, voltage_);
+  voltage_ = std::max(-limit, voltage_);
+  U(0, 0) = voltage_ - old_voltage;
+  LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
+  LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
+
   last_voltage_ = voltage_;
 }
 
@@ -380,6 +386,9 @@
   // Update the observer.
   loop_->Update(position != NULL, !output_enabled);
 
+  LOG(DEBUG, "X_hat={%f, %f, %f}\n",
+      loop_->X_hat(0, 0), loop_->X_hat(1, 0), loop_->X_hat(2, 0));
+
   capped_goal_ = false;
   // Verify that the zeroing goal hasn't run away.
   switch (state_) {