Converted drivetrain to codegen coeffs at buildtime
Change-Id: I41a77751f71d2f990ac49b19c85bc166b10a563b
diff --git a/y2015_bot3/control_loops/drivetrain/BUILD b/y2015_bot3/control_loops/drivetrain/BUILD
index ad1ec39..330626a 100644
--- a/y2015_bot3/control_loops/drivetrain/BUILD
+++ b/y2015_bot3/control_loops/drivetrain/BUILD
@@ -39,6 +39,19 @@
],
)
+genrule(
+ name = 'genrule_drivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2015_bot3/control_loops/python:drivetrain) $(OUTS)',
+ tools = [
+ '//y2015_bot3/control_loops/python:drivetrain',
+ ],
+ outs = [
+ 'drivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.cc',
+ ],
+)
+
cc_library(
name = 'drivetrain_lib',
srcs = [
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain.cc b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
index a3e7c24..81a4be5 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
@@ -143,7 +143,7 @@
DrivetrainMotorsSS()
: loop_(new LimitedDrivetrainLoop(
- ::y2015_bot3::control_loops::MakeDrivetrainLoop())),
+ ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop())),
filtered_offset_(0.0),
gyro_(0.0),
left_goal_(0.0),
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
deleted file mode 100644
index 25e728a..0000000
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ /dev/null
@@ -1,133 +0,0 @@
-#include "y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace y2015_bot3 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
- Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
- 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.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
- 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.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
- 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.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
- Eigen::Matrix<double, 4, 2> B;
- B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
- 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.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
- Eigen::Matrix<double, 2, 4> K;
- K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
- Eigen::Matrix<double, 2, 4> K;
- K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
- Eigen::Matrix<double, 2, 4> K;
- K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
- return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
-}
-
-StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
- Eigen::Matrix<double, 4, 2> L;
- L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
- Eigen::Matrix<double, 2, 4> K;
- K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
- Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
- 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 y2015_bot3
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
deleted file mode 100644
index 041e06e..0000000
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef Y2015_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define Y2015_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace y2015_bot3 {
-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 y2015_bot3
-
-#endif // Y2015_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_lib_test.cc b/y2015_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
index 4e66302..a3c86e3 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -46,7 +46,7 @@
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation()
: drivetrain_plant_(
- new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
+ new StateFeedbackPlant<4, 2, 2>(drivetrain::MakeDrivetrainPlant())),
my_drivetrain_queue_(".y2015_bot3.control_loops.drivetrain", 0x8a8dde77,
".y2015_bot3.control_loops.drivetrain.goal",
".y2015_bot3.control_loops.drivetrain.position",
diff --git a/y2015_bot3/control_loops/python/BUILD b/y2015_bot3/control_loops/python/BUILD
index 663124f..346cafd 100644
--- a/y2015_bot3/control_loops/python/BUILD
+++ b/y2015_bot3/control_loops/python/BUILD
@@ -11,3 +11,14 @@
'//frc971/control_loops/python:controls',
]
)
+
+py_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
diff --git a/y2015_bot3/control_loops/python/drivetrain.py b/y2015_bot3/control_loops/python/drivetrain.py
index 9742167..a1df0ec 100755
--- a/y2015_bot3/control_loops/python/drivetrain.py
+++ b/y2015_bot3/control_loops/python/drivetrain.py
@@ -1,11 +1,12 @@
#!/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
+import glog
class CIM(control_loop.ControlLoop):
def __init__(self):
@@ -123,7 +124,7 @@
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]]))
+ #glog.debug('THE NUMBER I WANT %s', 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)
@@ -131,7 +132,7 @@
self.hp = 0.65
self.lp = 0.83
self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
- print self.K
+ glog.info('K %s', str(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],
@@ -142,10 +143,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]
+ glog.info('A %s', str(self.A))
+ glog.info('B %s', str(self.B))
+ glog.info('K %s', str(self.K))
+ glog.info('Poles are %s', str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.hlp = 0.3
self.llp = 0.4
@@ -221,23 +222,20 @@
#pylab.show()
# Write the generated constants out to a file.
- print "Output one"
+ glog.info('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)
- if len(argv) != 5:
- print "Expected .h file name and .cc file name"
+ if len(argv) != 3:
+ glog.fatal('Expected .h file name and .cc file name %s', str(len(argv)))
else:
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
drivetrain_high_low, drivetrain_high_high],
- namespaces=['y2015_bot3', 'control_loops'])
- if argv[1][-3:] == '.cc':
- dog_loop_writer.Write(argv[2], argv[1])
- else:
- dog_loop_writer.Write(argv[1], argv[2])
+ namespaces=['y2015_bot3', 'control_loops', 'drivetrain'])
+ dog_loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2015_bot3/control_loops/update_drivetrain.sh b/y2015_bot3/control_loops/update_drivetrain.sh
deleted file mode 100755
index b5757d3..0000000
--- a/y2015_bot3/control_loops/update_drivetrain.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/bash
-#
-# Updates the drivetrain controllers.
-
-cd $(dirname $0)
-
-export PYTHONPATH=../../frc971/control_loops/python
-
-./python/drivetrain.py drivetrain/drivetrain_dog_motor_plant.h \
- drivetrain/drivetrain_dog_motor_plant.cc \
- drivetrain/drivetrain_clutch_motor_plant.h \
- drivetrain/drivetrain_clutch_motor_plant.cc
diff --git a/y2015_bot3/control_loops/update_polydrivetrain.sh b/y2015_bot3/control_loops/update_polydrivetrain.sh
deleted file mode 100755
index b1bb2b4..0000000
--- a/y2015_bot3/control_loops/update_polydrivetrain.sh
+++ /dev/null
@@ -1,14 +0,0 @@
-#!/bin/bash
-#
-# Updates the polydrivetrain controllers and CIM models.
-
-cd $(dirname $0)
-
-export PYTHONPATH=../../frc971/control_loops/python
-
-./python/polydrivetrain.py drivetrain/polydrivetrain_dog_motor_plant.h \
- drivetrain/polydrivetrain_dog_motor_plant.cc \
- drivetrain/polydrivetrain_clutch_motor_plant.h \
- drivetrain/polydrivetrain_clutch_motor_plant.cc \
- drivetrain/polydrivetrain_cim_plant.h \
- drivetrain/polydrivetrain_cim_plant.cc