Automatically generate the drivetrain constants.
Change-Id: Ib96f7d55734d44d8ac9d918ec90edf3b1fb46cd3
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
new file mode 100644
index 0000000..df7d48b
--- /dev/null
+++ b/frc971/control_loops/python/BUILD
@@ -0,0 +1,9 @@
+package(default_visibility = ['//visibility:public'])
+
+py_library(
+ name = 'controls',
+ srcs = [
+ 'control_loop.py',
+ 'controls.py',
+ ],
+)
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 881880f..d3968a6 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -1,5 +1,6 @@
import controls
import numpy
+import os
class Constant(object):
def __init__ (self, name, formatt, value):
@@ -52,14 +53,14 @@
return self._namespaces[0]
def _HeaderGuard(self, header_file):
- return (self._TopDirectory().upper() + '_CONTROL_LOOPS_' +
- header_file.upper().replace('.', '_').replace('/', '_') +
- '_')
+ return ('_'.join([namespace.upper() for namespace in self._namespaces]) +
+ os.path.basename(header_file).upper()
+ .replace('.', '_').replace('/', '_') + '_')
def Write(self, header_file, cc_file):
"""Writes the loops to the specified files."""
self.WriteHeader(header_file)
- self.WriteCC(header_file, cc_file)
+ self.WriteCC(os.path.basename(header_file), cc_file)
def _GenericType(self, typename):
"""Returns a loop template using typename for the type."""
@@ -121,8 +122,8 @@
def WriteCC(self, header_file_name, cc_file):
"""Writes the cc file to the file named cc_file."""
with open(cc_file, 'w') as fd:
- fd.write('#include \"%s/control_loops/%s\"\n' %
- (self._TopDirectory(), header_file_name))
+ fd.write('#include \"%s/%s\"\n' %
+ (os.path.join(*self._namespaces), header_file_name))
fd.write('\n')
fd.write('#include <vector>\n')
fd.write('\n')
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index b66bd56..6494ce1 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -131,23 +131,13 @@
Returns:
KalmanGain, Covariance.
"""
- P = Q
+ I = numpy.matrix(numpy.eye(Q.shape[0]))
+ Z = numpy.matrix(numpy.zeros(Q.shape[0]))
- I = numpy.matrix(numpy.eye(P.shape[0]))
- At = A.T
- Ct = C.T
- i = 0
-
- while True:
- last_P = P
- P_prior = A * P * At + Q
- S = C * P_prior * Ct + R
- K = P_prior * Ct * numpy.linalg.inv(S)
- P = (I - K * C) * P_prior
-
- diff = P - last_P
- i += 1
- if numpy.linalg.norm(diff) / numpy.linalg.norm(P) < 1e-9:
- break
+ # Compute the steady state covariance matrix.
+ P_prior = scipy.linalg.solve_discrete_are(A.T, C.T, Q, R)
+ S = C * P_prior * C.T + R
+ K = numpy.linalg.lstsq(S.T, (P_prior * C.T).T)[0].T
+ P = (I - K * C) * P_prior
return K, P
diff --git a/y2014/constants.cc b/y2014/constants.cc
index 21ebb59..6e5e293 100644
--- a/y2014/constants.cc
+++ b/y2014/constants.cc
@@ -65,7 +65,7 @@
false,
0.5,
control_loops::MakeVelocityDrivetrainLoop,
- control_loops::MakeDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
0.02, // drivetrain done delta
5.0, // drivetrain max speed
@@ -103,7 +103,7 @@
false,
kRobotWidth,
control_loops::MakeVelocityDrivetrainLoop,
- control_loops::MakeDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
0.02, // drivetrain done delta
5.0, // drivetrain max speed
@@ -150,7 +150,7 @@
false,
kRobotWidth,
control_loops::MakeVelocityDrivetrainLoop,
- control_loops::MakeDrivetrainLoop,
+ ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
0.02, // drivetrain done delta
5.0, // drivetrain max speed
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index 2d0d64f..85aad66 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -24,15 +24,35 @@
],
)
+genrule(
+ name = 'genrule_drivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2014/control_loops/python:drivetrain) $(OUTS)',
+ tools = [
+ '//y2014/control_loops/python:drivetrain',
+ ],
+ tags = [
+ 'local',
+ ],
+ outs = [
+ 'drivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+)
+
cc_library(
name = 'polydrivetrain_plants',
srcs = [
'polydrivetrain_dog_motor_plant.cc',
'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.cc',
],
hdrs = [
'polydrivetrain_dog_motor_plant.h',
'drivetrain_dog_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.h',
],
deps = [
'//frc971/control_loops:state_feedback_loop',
diff --git a/y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
deleted file mode 100644
index 2bb5c94..0000000
--- a/y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ /dev/null
@@ -1,133 +0,0 @@
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00860955515291, 0.0, 0.000228184998733, 0.0, 0.735841675858, 0.0, 0.0410810558113, 0.0, 0.000228184998733, 1.0, 0.00860955515291, 0.0, 0.0410810558113, 0.0, 0.735841675858;
- Eigen::Matrix<double, 4, 2> B;
- B << 0.000272244648044, -4.46778919705e-05, 0.0517213538779, -0.00804353916233, -4.46778919705e-05, 0.000272244648044, -0.00804353916233, 0.0517213538779;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00860667098456, 0.0, 7.04111872002e-05, 0.0, 0.735048848179, 0.0, 0.0131811893199, 0.0, 0.000245343870066, 1.0, 0.00957169266049, 0.0, 0.045929121897, 0.0, 0.915703853642;
- Eigen::Matrix<double, 4, 2> B;
- B << 0.000272809358971, -2.57343985847e-05, 0.0518765869984, -0.00481755802263, -4.80375440247e-05, 0.00015654091672, -0.00899277497558, 0.0308091755839;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00957169266049, 0.0, 0.000245343870066, 0.0, 0.915703853642, 0.0, 0.045929121897, 0.0, 7.04111872002e-05, 1.0, 0.00860667098456, 0.0, 0.0131811893199, 0.0, 0.735048848179;
- Eigen::Matrix<double, 4, 2> B;
- B << 0.00015654091672, -4.80375440247e-05, 0.0308091755839, -0.00899277497558, -2.57343985847e-05, 0.000272809358971, -0.00481755802263, 0.0518765869984;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00957076892085, 0.0, 7.56192087769e-05, 0.0, 0.915439806567, 0.0, 0.0146814193986, 0.0, 7.56192087769e-05, 1.0, 0.00957076892085, 0.0, 0.0146814193986, 0.0, 0.915439806567;
- Eigen::Matrix<double, 4, 2> B;
- B << 0.000156878531877, -2.76378646165e-05, 0.0309056814511, -0.00536587314624, -2.76378646165e-05, 0.000156878531877, -0.00536587314624, 0.0309056814511;
- Eigen::Matrix<double, 2, 4> C;
- C << 1, 0, 0, 0, 0, 0, 1, 0;
- Eigen::Matrix<double, 2, 2> D;
- D << 0, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 12.0;
- Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -12.0;
- return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.03584167586, 0.0410810558113, 17.1117704011, 3.22861251708, 0.0410810558113, 1.03584167586, 3.22861251708, 17.1117704011;
- Eigen::Matrix<double, 2, 4> K;
- K << 128.210620632, 6.93828382074, 5.11036686771, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.0117194973377, 0.0, 0.000344183176608, 0.0, 1.36323698074, 0.0, -0.0761076958907, 0.0, 0.000344183176608, 1.0, -0.0117194973377, 0.0, -0.0761076958907, 0.0, 1.36323698074;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.02891982345, 0.0143715516939, 16.6997472571, 1.23741823594, 0.0143715516939, 1.22183287838, 2.40440177527, 33.5403677132;
- Eigen::Matrix<double, 2, 4> K;
- K << 127.841025245, 6.90618982868, -2.11442482189, 0.171361719101, 11.257083857, 1.47190974842, 138.457761234, 11.0770574926;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.011714710309, 0.0, 9.17355833725e-05, 0.0, 1.36167854796, 0.0, -0.0196008159867, 0.0, 0.00031964754384, 1.0, -0.0104574267731, 0.0, -0.0682979543713, 0.0, 1.09303924439;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.21584032636, 0.045928553155, 33.3376290177, 4.12652814156, 0.045928553155, 1.03491237546, 2.45838080322, 16.967272239;
- Eigen::Matrix<double, 2, 4> K;
- K << 138.457761234, 11.0770574926, 11.257083857, 1.47190974842, -2.1144248219, 0.171361719101, 127.841025245, 6.90618982868;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.0104574267731, 0.0, 0.00031964754384, 0.0, 1.09303924439, 0.0, -0.0682979543713, 0.0, 9.17355833725e-05, 1.0, -0.011714710309, 0.0, -0.0196008159867, 0.0, 1.36167854796;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.21543980657, 0.0146814193986, 33.1557840927, 1.47278696694, 0.0146814193986, 1.21543980657, 1.47278696694, 33.1557840927;
- Eigen::Matrix<double, 2, 4> K;
- K << 138.52410152, 11.0779399816, 3.96842371774, 0.882728086516, 3.96842371774, 0.882728086517, 138.52410152, 11.0779399816;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.010456196092, 0.0, 8.50876166887e-05, 0.0, 1.0926521463, 0.0, -0.0175234726538, 0.0, 8.50876166887e-05, 1.0, -0.010456196092, 0.0, -0.0175234726538, 0.0, 1.0926521463;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
-}
-
-StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(4);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients()));
- plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients()));
- plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients()));
- plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients()));
- return StateFeedbackPlant<4, 2, 2>(&plants);
-}
-
-StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(4);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController()));
- controllers[1] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController()));
- controllers[2] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController()));
- controllers[3] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController()));
- return StateFeedbackLoop<4, 2, 2>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h
deleted file mode 100644
index 64498dd..0000000
--- a/y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController();
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController();
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController();
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients();
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController();
-
-StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant();
-
-StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2014_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2014/control_loops/drivetrain/drivetrain_lib_test.cc b/y2014/control_loops/drivetrain/drivetrain_lib_test.cc
index e9291d8..e2db9de 100644
--- a/y2014/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -20,6 +20,8 @@
namespace control_loops {
namespace testing {
+using ::y2014::control_loops::drivetrain::MakeDrivetrainPlant;
+
class Environment : public ::testing::Environment {
public:
virtual ~Environment() {}
diff --git a/y2014/control_loops/python/BUILD b/y2014/control_loops/python/BUILD
new file mode 100644
index 0000000..232d128
--- /dev/null
+++ b/y2014/control_loops/python/BUILD
@@ -0,0 +1,11 @@
+package(default_visibility = ['//y2014:__subpackages__'])
+
+py_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//frc971/control_loops/python:controls',
+ ]
+)
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index ac05a57..5a2e478 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -1,7 +1,7 @@
#!/usr/bin/python
-import control_loop
-import controls
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
import numpy
import sys
from matplotlib import pylab
@@ -71,7 +71,7 @@
# Radius of the wheels, in meters.
self.r = .04445
# Resistance of the motor, divided by the number of motors.
- self.R = 12.0 / self.stall_current / 4
+ self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
@@ -120,7 +120,6 @@
self.D = numpy.matrix([[0, 0],
[0, 0]])
- #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.0]]))
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
@@ -128,7 +127,7 @@
self.hp = 0.65
self.lp = 0.83
self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
- print self.K
+ #print self.K
q_pos = 0.07
q_vel = 1.0
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
@@ -139,10 +138,10 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, (1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.A
- print self.B
- print self.K
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ #print self.A
+ #print self.B
+ #print self.K
+ #print numpy.linalg.eig(self.A - self.B * self.K)[0]
self.hlp = 0.3
self.llp = 0.4
@@ -152,6 +151,66 @@
self.U_min = numpy.matrix([[-12.0], [-12.0]])
self.InitializeState()
+
+class KFDrivetrain(Drivetrain):
+ def __init__(self, name="KFDrivetrain", left_low=True, right_low=True):
+ super(KFDrivetrain, self).__init__(name, left_low, right_low)
+
+ self.unaugmented_A_continuous = self.A_continuous
+ self.unaugmented_B_continuous = self.B_continuous
+
+ # The states are
+ # The practical voltage applied to the wheels is
+ # V_left = U_left + left_voltage_error
+ #
+ # [left position, left velocity, right position, right velocity,
+ # left voltage error, right voltage error, angular_error]
+ self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
+ self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
+ self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
+ self.A_continuous[0:4,4:6] = self.unaugmented_B_continuous
+ self.B_continuous[0:4,0:2] = self.unaugmented_B_continuous
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, self.rb],
+ [0, 0, 1, 0, 0, 0, -self.rb],
+ [0, -2.0 / self.rb, 0, 2.0 / self.rb, 0, 0, 0]])
+
+ self.D = numpy.matrix([[0, 0],
+ [0, 0],
+ [0, 0]])
+
+ q_pos = 0.08
+ q_vel = 0.40
+ q_voltage = 6.0
+ q_gyro = 0.1
+
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_gyro ** 2.0)]])
+
+ r_pos = 0.05
+ r_gyro = 0.001
+ self.R = numpy.matrix([[(r_pos ** 2.0), 0.0, 0.0],
+ [0.0, (r_pos ** 2.0), 0.0],
+ [0.0, 0.0, (r_gyro ** 2.0)]])
+
+ # Solving for kf gains.
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ self.L = self.A * self.KalmanGain
+
+ # We need a nothing controller for the autogen code to be happy.
+ self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+
+
def main(argv):
# Simulate the response of the system to a step input.
drivetrain = Drivetrain()
@@ -218,22 +277,37 @@
#pylab.show()
# Write the generated constants out to a file.
- print "Output one"
drivetrain_low_low = Drivetrain(name="DrivetrainLowLow", left_low=True, right_low=True)
drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh", left_low=True, right_low=False)
drivetrain_high_low = Drivetrain(name="DrivetrainHighLow", left_low=False, right_low=True)
drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh", left_low=False, right_low=False)
+ kf_drivetrain_low_low = KFDrivetrain(name="KFDrivetrainLowLow", left_low=True, right_low=True)
+ kf_drivetrain_low_high = KFDrivetrain(name="KFDrivetrainLowHigh", left_low=True, right_low=False)
+ kf_drivetrain_high_low = KFDrivetrain(name="KFDrivetrainHighLow", left_low=False, right_low=True)
+ kf_drivetrain_high_high = KFDrivetrain(name="KFDrivetrainHighHigh", left_low=False, right_low=False)
+
if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
+ namespaces = ['y2014', 'control_loops', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
- drivetrain_high_low, drivetrain_high_high])
+ drivetrain_high_low, drivetrain_high_high],
+ namespaces = namespaces)
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
dog_loop_writer.Write(argv[1], argv[2])
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
+ kf_drivetrain_high_low, kf_drivetrain_high_high],
+ namespaces = namespaces)
+ if argv[3][-3:] == '.cc':
+ kf_loop_writer.Write(argv[4], argv[3])
+ else:
+ kf_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
sys.exit(main(sys.argv))