Automatically generate shooter constants.
Change-Id: I06cbdcb80e9ccd003fb190ea2ae03217cfa34feb
diff --git a/y2014/control_loops/python/BUILD b/y2014/control_loops/python/BUILD
index ee99aa4..6536f93 100644
--- a/y2014/control_loops/python/BUILD
+++ b/y2014/control_loops/python/BUILD
@@ -31,3 +31,13 @@
'//frc971/control_loops/python:controls',
]
)
+
+py_binary(
+ name = 'shooter',
+ srcs = [
+ 'shooter.py',
+ ],
+ deps = [
+ '//frc971/control_loops/python:controls',
+ ]
+)
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index 69f2599..cfb7e35 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
-import control_loop
+from frc971.control_loops.python import control_loop
+import argparse
import numpy
import sys
from matplotlib import pylab
@@ -178,13 +179,25 @@
return new_u - old_u
def main(argv):
+ parser = argparse.ArgumentParser(description='Calculate shooter.')
+ parser.add_argument('--plot', action='store_true', default=False, help='If true, plot')
+ parser.add_argument('shootercc')
+ parser.add_argument('shooterh')
+ parser.add_argument('unaugmented_shootercc')
+ parser.add_argument('unaugmented_shooterh')
+
+ args = parser.parse_args(argv[1:])
+
# Simulate the response of the system to a goal.
sprung_shooter = SprungShooterDeltaU()
raw_sprung_shooter = SprungShooter()
close_loop_x = []
close_loop_u = []
goal_position = -0.3
- R = numpy.matrix([[goal_position], [0.0], [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] * goal_position]])
+ R = numpy.matrix([[goal_position],
+ [0.0],
+ [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] *
+ goal_position]])
voltage = numpy.matrix([[0.0]])
for _ in xrange(500):
U = sprung_shooter.K * (R - sprung_shooter.X_hat)
@@ -196,9 +209,10 @@
close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
close_loop_u.append(voltage[0, 0])
- pylab.plot(range(500), close_loop_x)
- pylab.plot(range(500), close_loop_u)
- pylab.show()
+ if args.plot:
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
+ pylab.show()
shooter = ShooterDeltaU()
raw_shooter = Shooter()
@@ -217,38 +231,33 @@
close_loop_x.append(raw_shooter.X[0, 0] * 10)
close_loop_u.append(voltage[0, 0])
- pylab.plot(range(500), close_loop_x)
- pylab.plot(range(500), close_loop_u)
- pylab.show()
+ if args.plot:
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
+ pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 5:
- print "Expected .h file name and .cc file name for"
- print "both the plant and unaugmented plant"
- else:
- unaug_sprung_shooter = SprungShooter("RawSprungShooter")
- unaug_shooter = Shooter("RawShooter")
- unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
- [unaug_sprung_shooter,
- unaug_shooter])
- if argv[3][-3:] == '.cc':
- unaug_loop_writer.Write(argv[4], argv[3])
- else:
- unaug_loop_writer.Write(argv[3], argv[4])
+ unaug_sprung_shooter = SprungShooter("RawSprungShooter")
+ unaug_shooter = Shooter("RawShooter")
+ namespaces = ['y2014', 'control_loops', 'shooter']
+ unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
+ [unaug_sprung_shooter,
+ unaug_shooter],
+ namespaces=namespaces)
+ unaug_loop_writer.Write(args.unaugmented_shooterh,
+ args.unaugmented_shootercc)
- sprung_shooter = SprungShooterDeltaU()
- shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter,
- shooter])
+ sprung_shooter = SprungShooterDeltaU()
+ shooter = ShooterDeltaU()
+ loop_writer = control_loop.ControlLoopWriter("Shooter",
+ [sprung_shooter, shooter],
+ namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
+ loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
sprung_shooter.max_extension))
- loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
+ loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
sprung_shooter.Ks))
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
+ loop_writer.Write(args.shooterh, args.shootercc)
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/shooter/BUILD b/y2014/control_loops/shooter/BUILD
index f04a02a..41d4228 100644
--- a/y2014/control_loops/shooter/BUILD
+++ b/y2014/control_loops/shooter/BUILD
@@ -25,6 +25,24 @@
],
)
+genrule(
+ name = 'genrule_shooter',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2014/control_loops/python:shooter) $(OUTS)',
+ tools = [
+ '//y2014/control_loops/python:shooter',
+ ],
+ tags = [
+ 'local',
+ ],
+ outs = [
+ 'shooter_motor_plant.cc',
+ 'shooter_motor_plant.h',
+ 'unaugmented_shooter_motor_plant.cc',
+ 'unaugmented_shooter_motor_plant.h',
+ ],
+)
+
cc_library(
name = 'shooter_lib',
srcs = [
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index 52589f7..7506a0c 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -15,6 +15,9 @@
namespace frc971 {
namespace control_loops {
+using ::y2014::control_loops::shooter::kSpringConstant;
+using ::y2014::control_loops::shooter::kMaxExtension;
+using ::y2014::control_loops::shooter::MakeShooterLoop;
using ::aos::time::Time;
void ZeroedStateFeedbackLoop::CapU() {
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index 71347cb..0efbbd0 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -102,7 +102,8 @@
private:
// The offset between what is '0' (0 rate on the spring) and the 0 (all the
// way cocked).
- constexpr static double kPositionOffset = kMaxExtension;
+ constexpr static double kPositionOffset =
+ ::y2014::control_loops::shooter::kMaxExtension;
// The accumulated voltage to apply to the motor.
double voltage_;
double last_voltage_;
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index 2beb705..d8c81e7 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -16,6 +16,9 @@
namespace control_loops {
namespace testing {
+using ::y2014::control_loops::shooter::kMaxExtension;
+using ::y2014::control_loops::shooter::MakeRawShooterPlant;
+
static const int kTestTeam = 1;
class TeamNumberEnvironment : public ::testing::Environment {
diff --git a/y2014/control_loops/shooter/shooter_motor_plant.cc b/y2014/control_loops/shooter/shooter_motor_plant.cc
deleted file mode 100644
index 7da6407..0000000
--- a/y2014/control_loops/shooter/shooter_motor_plant.cc
+++ /dev/null
@@ -1,77 +0,0 @@
-#include "y2014/control_loops/shooter/shooter_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients() {
- Eigen::Matrix<double, 3, 3> A;
- A << 0.999391114909, 0.00811316740387, 7.59766686183e-05, -0.113584343654, 0.64780421498, 0.0141730519709, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 1> B;
- B << 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 1, 3> C;
- C << 1.0, 0.0, 0.0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0.0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
- Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00811505488455, 7.59852687598e-05, 0.0, 0.648331305446, 0.0141763492481, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 1> B;
- B << 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 1, 3> C;
- C << 1.0, 0.0, 0.0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0.0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<3, 1, 1> MakeSprungShooterController() {
- Eigen::Matrix<double, 3, 1> L;
- L << 1.64719532989, 57.0572680832, 636.74290365;
- Eigen::Matrix<double, 1, 3> K;
- K << 450.571849185, 11.8404918938, 0.997195329889;
- Eigen::Matrix<double, 3, 3> A_inv;
- A_inv << 0.99918700445, -0.0125139220268, 0.00010144556732, 0.175194908375, 1.54148211958, -0.0218608169185, 0.0, 0.0, 1.0;
- return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeSprungShooterPlantCoefficients());
-}
-
-StateFeedbackController<3, 1, 1> MakeShooterController() {
- Eigen::Matrix<double, 3, 1> L;
- L << 1.64833130545, 57.2417604572, 636.668851906;
- Eigen::Matrix<double, 1, 3> K;
- K << 349.173113146, 8.65077618169, 0.848331305446;
- Eigen::Matrix<double, 3, 3> A_inv;
- A_inv << 1.0, -0.0125168333171, 0.000101457731824, 0.0, 1.5424212769, -0.021865902709, 0.0, 0.0, 1.0;
- return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeShooterPlantCoefficients());
-}
-
-StateFeedbackPlant<3, 1, 1> MakeShooterPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>> plants(2);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>(new StateFeedbackPlantCoefficients<3, 1, 1>(MakeSprungShooterPlantCoefficients()));
- plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>(new StateFeedbackPlantCoefficients<3, 1, 1>(MakeShooterPlantCoefficients()));
- return StateFeedbackPlant<3, 1, 1>(&plants);
-}
-
-StateFeedbackLoop<3, 1, 1> MakeShooterLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<3, 1, 1>>> controllers(2);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<3, 1, 1>>(new StateFeedbackController<3, 1, 1>(MakeSprungShooterController()));
- controllers[1] = ::std::unique_ptr<StateFeedbackController<3, 1, 1>>(new StateFeedbackController<3, 1, 1>(MakeShooterController()));
- return StateFeedbackLoop<3, 1, 1>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2014/control_loops/shooter/shooter_motor_plant.h b/y2014/control_loops/shooter/shooter_motor_plant.h
deleted file mode 100644
index 45d6ed1..0000000
--- a/y2014/control_loops/shooter/shooter_motor_plant.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef Y2014_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
-#define Y2014_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-static constexpr double kMaxExtension = 0.323850;
-
-static constexpr double kSpringConstant = 2800.000000;
-
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeSprungShooterController();
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeShooterController();
-
-StateFeedbackPlant<3, 1, 1> MakeShooterPlant();
-
-StateFeedbackLoop<3, 1, 1> MakeShooterLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2014_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.cc
deleted file mode 100644
index 8311948..0000000
--- a/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.cc
+++ /dev/null
@@ -1,77 +0,0 @@
-#include "y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 0.999391114909, 0.00811316740387, -0.113584343654, 0.64780421498;
- Eigen::Matrix<double, 2, 1> B;
- B << 7.59766686183e-05, 0.0141730519709;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00811505488455, 0.0, 0.648331305446;
- Eigen::Matrix<double, 2, 1> B;
- B << 7.59852687598e-05, 0.0141763492481;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
- Eigen::Matrix<double, 1, 1> D;
- D << 0;
- Eigen::Matrix<double, 1, 1> U_max;
- U_max << 12.0;
- Eigen::Matrix<double, 1, 1> U_min;
- U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<2, 1, 1> MakeRawSprungShooterController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.54719532989, 43.9345489758;
- Eigen::Matrix<double, 1, 2> K;
- K << 2126.06977433, 41.3223370936;
- Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 0.99918700445, -0.0125139220268, 0.175194908375, 1.54148211958;
- return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeRawSprungShooterPlantCoefficients());
-}
-
-StateFeedbackController<2, 1, 1> MakeRawShooterController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.54833130545, 44.1155797675;
- Eigen::Matrix<double, 1, 2> K;
- K << 2133.83569145, 41.3499425476;
- Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0, -0.0125168333171, 0.0, 1.5424212769;
- return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeRawShooterPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeRawShooterPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>> plants(2);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawSprungShooterPlantCoefficients()));
- plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawShooterPlantCoefficients()));
- return StateFeedbackPlant<2, 1, 1>(&plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeRawShooterLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 1, 1>>> controllers(2);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeRawSprungShooterController()));
- controllers[1] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeRawShooterController()));
- return StateFeedbackLoop<2, 1, 1>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace frc971
diff --git a/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h b/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h
deleted file mode 100644
index 5b7de85..0000000
--- a/y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h
+++ /dev/null
@@ -1,24 +0,0 @@
-#ifndef Y2014_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_
-#define Y2014_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace frc971 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawSprungShooterPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeRawSprungShooterController();
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeRawShooterController();
-
-StateFeedbackPlant<2, 1, 1> MakeRawShooterPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeRawShooterLoop();
-
-} // namespace control_loops
-} // namespace frc971
-
-#endif // Y2014_CONTROL_LOOPS_SHOOTER_UNAUGMENTED_SHOOTER_MOTOR_PLANT_H_