commit 03be124fc89ff86faf6bf79d1f79c1cbfa2efd2b
author James Kuszmaul <jabukuszmaul@gmail.com> Fri Feb 21 14:52:04 2020 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Feb 21 15:41:17 2020 -0800
tree e553c6baace2cde9f2c8d522729a27c82a5ecf56
parent bd588a3def562906e9bbb41823020a039053a0ef

Expose dt in control loop plant parameters

Change-Id: I0da4cbde0117c78bd56ce910069a3e0c0bc1744f

frc971/control_loops/hybrid_state_feedback_loop_test.cc

diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 38dbd53..13f74fa 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -127,7 +127,7 @@
       Eigen::Matrix<double, 7, 2>::Identity(),
       Eigen::Matrix<double, 7, 4>::Identity(),
       Eigen::Matrix<double, 4, 1>::Constant(1),
-      Eigen::Matrix<double, 4, 1>::Constant(-1));
+      Eigen::Matrix<double, 4, 1>::Constant(-1), std::chrono::milliseconds(5));
 
   // Build a plant.
   ::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>

frc971/control_loops/python/control_loop.py

diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 919968e..347e1cd 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -183,6 +183,7 @@
             fd.write('#include \"%s/%s\"\n' % (os.path.join(*self._namespaces),
                                                header_file_name))
             fd.write('\n')
+            fd.write('#include <chrono>\n')
             fd.write('#include <vector>\n')
             fd.write('\n')
             fd.write(
@@ -389,8 +390,10 @@
             ans.append(self._DumpMatrix('A', self.A, scalar_type))
             ans.append(self._DumpMatrix('B', self.B, scalar_type))
             ans.append(
+                '  const std::chrono::nanoseconds dt(%d);\n' % (self.dt * 1e9))
+            ans.append(
                 '  return %s'
-                '(A, B, C, D, U_max, U_min);\n' % (plant_coefficient_type))
+                '(A, B, C, D, U_max, U_min, dt);\n' % (plant_coefficient_type))
         elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
             ans.append(
                 self._DumpMatrix('A_continuous', self.A_continuous,

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 81783cc..83a54f0 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -39,7 +39,8 @@
         C(other.C),
         D(other.D),
         U_min(other.U_min),
-        U_max(other.U_max) {}
+        U_max(other.U_max),
+        dt(other.dt) {}
 
   StateFeedbackPlantCoefficients(
       const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
@@ -47,8 +48,9 @@
       const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
       const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
       const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
-      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
-      : A(A), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min,
+      const std::chrono::nanoseconds dt)
+      : A(A), B(B), C(C), D(D), U_min(U_min), U_max(U_max), dt(dt) {}
 
   const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
   const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
@@ -56,6 +58,7 @@
   const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
   const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
   const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
+  const std::chrono::nanoseconds dt;
 };
 
 template <int number_of_states, int number_of_inputs, int number_of_outputs,