commit c2b7774e944971de75d95a59434b75e4b828d008
author Austin Schuh <austin.linux@gmail.com> Thu Nov 26 16:18:27 2015 -0800
committer Austin Schuh <austin.linux@gmail.com> Thu Nov 26 16:18:29 2015 -0800
tree cdf895e245a58e8e53760d86cea53bddc658051f
parent e6df5ee5520e9fbac439f5e048ce9ac9ba17512b

UpdateObserver now takes U as an argument.

This supports running UpdateObserver without a control loop better.

Change-Id: I62a304fa192604bc5b400c8bbf5191971a8534cc

frc971/control_loops/state_feedback_loop.h

diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 6ebd32e..d1b24a5 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -385,11 +385,11 @@
       CapU();
     }
 
-    UpdateObserver();
+    UpdateObserver(U_);
   }
 
-  void UpdateObserver() {
-    X_hat_ = A() * X_hat() + B() * U();
+  void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u) {
+    X_hat_ = A() * X_hat() + B() * new_u;
   }
 
   // Sets the current controller to be index, clamped to be within range.

y2014/control_loops/drivetrain/ssdrivetrain.cc

diff --git a/y2014/control_loops/drivetrain/ssdrivetrain.cc b/y2014/control_loops/drivetrain/ssdrivetrain.cc
index 53cc2dd..2551ddb 100644
--- a/y2014/control_loops/drivetrain/ssdrivetrain.cc
+++ b/y2014/control_loops/drivetrain/ssdrivetrain.cc
@@ -157,7 +157,7 @@
       loop_->mutable_U().setZero();
       loop_->mutable_U_uncapped().setZero();
     }
-    loop_->UpdateObserver();
+    loop_->UpdateObserver(loop_->U());
   }
   ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
   LOG_MATRIX(DEBUG, "E", E);

y2014_bot3/control_loops/drivetrain/drivetrain.cc

diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.cc b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
index 047cb71..8f26161 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
@@ -183,7 +183,7 @@
         loop_->mutable_U().setZero();
         loop_->mutable_U_uncapped().setZero();
       }
-      loop_->UpdateObserver();
+      loop_->UpdateObserver(loop_->U());
     }
     ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
     LOG_MATRIX(DEBUG, "E", E);

y2015/control_loops/drivetrain/drivetrain.cc

diff --git a/y2015/control_loops/drivetrain/drivetrain.cc b/y2015/control_loops/drivetrain/drivetrain.cc
index 22a5d48..9ed927a 100644
--- a/y2015/control_loops/drivetrain/drivetrain.cc
+++ b/y2015/control_loops/drivetrain/drivetrain.cc
@@ -192,7 +192,7 @@
         loop_->mutable_U().setZero();
         loop_->mutable_U_uncapped().setZero();
       }
-      loop_->UpdateObserver();
+      loop_->UpdateObserver(loop_->U());
     }
     ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
     LOG_MATRIX(DEBUG, "E", E);

y2015_bot3/control_loops/drivetrain/drivetrain.cc

diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain.cc b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
index b126ad2..f7e039e 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
@@ -187,7 +187,7 @@
         loop_->mutable_U().setZero();
         loop_->mutable_U_uncapped().setZero();
       }
-      loop_->UpdateObserver();
+      loop_->UpdateObserver(loop_->U());
     }
     ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
     LOG_MATRIX(DEBUG, "E", E);