Converted 2015_bot3 elevator to generate statespace files.
Change-Id: Idddc0f18ca0c36aa69d3dfef08d44c0f2b45502a
diff --git a/y2015_bot3/control_loops/elevator/BUILD b/y2015_bot3/control_loops/elevator/BUILD
index b249477..c5c1a73 100644
--- a/y2015_bot3/control_loops/elevator/BUILD
+++ b/y2015_bot3/control_loops/elevator/BUILD
@@ -13,6 +13,21 @@
],
)
+genrule(
+ name = 'genrule_elevator3',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2015_bot3/control_loops/python:elevator3) $(OUTS)',
+ tools = [
+ '//y2015_bot3/control_loops/python:elevator3',
+ ],
+ outs = [
+ 'elevator_motor_plant.h',
+ 'elevator_motor_plant.cc',
+ 'integral_elevator_motor_plant.h',
+ 'integral_elevator_motor_plant.cc',
+ ],
+)
+
cc_library(
name = 'elevator_lib',
srcs = [
diff --git a/y2015_bot3/control_loops/elevator/elevator.cc b/y2015_bot3/control_loops/elevator/elevator.cc
index 2fef56f..16ba064 100644
--- a/y2015_bot3/control_loops/elevator/elevator.cc
+++ b/y2015_bot3/control_loops/elevator/elevator.cc
@@ -26,7 +26,8 @@
Elevator::Elevator(control_loops::ElevatorQueue *elevator)
: aos::controls::ControlLoop<control_loops::ElevatorQueue>(elevator),
- loop_(new SimpleCappedStateFeedbackLoop(MakeIntegralElevatorLoop())),
+ loop_(new SimpleCappedStateFeedbackLoop(
+ elevator::MakeIntegralElevatorLoop())),
profile_(::aos::controls::kLoopFrequency) {}
bool Elevator::CheckZeroed() {
diff --git a/y2015_bot3/control_loops/elevator/elevator_lib_test.cc b/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
index 2f6b019..5e0d134 100644
--- a/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
+++ b/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
@@ -27,7 +27,7 @@
class ElevatorSimulator {
public:
ElevatorSimulator()
- : plant_(new StateFeedbackPlant<2, 1, 1>(MakeElevatorPlant())),
+ : plant_(new StateFeedbackPlant<2, 1, 1>(elevator::MakeElevatorPlant())),
position_sim_(),
queue_(".y2015_bot3.control_loops.elevator_queue", 0xca8daa3b,
".y2015_bot3.control_loops.elevator_queue.goal",
diff --git a/y2015_bot3/control_loops/elevator/elevator_motor_plant.cc b/y2015_bot3/control_loops/elevator/elevator_motor_plant.cc
deleted file mode 100644
index 7fff4ff..0000000
--- a/y2015_bot3/control_loops/elevator/elevator_motor_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2015_bot3/control_loops/elevator/elevator_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace y2015_bot3 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00457884608813, 0.0, 0.836406580139;
- Eigen::Matrix<double, 2, 1> B;
- B << 5.90742803242e-05, 0.0229468687616;
- 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> MakeElevatorController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 0.937593947018, 16.1192495093;
- Eigen::Matrix<double, 1, 2> K;
- K << 597.919635715, 17.5389953523;
- Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0, -0.0054744261904, 0.0, 1.19559078533;
- return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeElevatorPlantCoefficients());
-}
-
-StateFeedbackPlant<2, 1, 1> MakeElevatorPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>> plants(1);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 1, 1>>(new StateFeedbackPlantCoefficients<2, 1, 1>(MakeElevatorPlantCoefficients()));
- return StateFeedbackPlant<2, 1, 1>(&plants);
-}
-
-StateFeedbackLoop<2, 1, 1> MakeElevatorLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 1, 1>>> controllers(1);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 1, 1>>(new StateFeedbackController<2, 1, 1>(MakeElevatorController()));
- return StateFeedbackLoop<2, 1, 1>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace y2015_bot3
diff --git a/y2015_bot3/control_loops/elevator/elevator_motor_plant.h b/y2015_bot3/control_loops/elevator/elevator_motor_plant.h
deleted file mode 100644
index 7be05e6..0000000
--- a/y2015_bot3/control_loops/elevator/elevator_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2015_BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
-#define Y2015_BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace y2015_bot3 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients();
-
-StateFeedbackController<2, 1, 1> MakeElevatorController();
-
-StateFeedbackPlant<2, 1, 1> MakeElevatorPlant();
-
-StateFeedbackLoop<2, 1, 1> MakeElevatorLoop();
-
-} // namespace control_loops
-} // namespace y2015_bot3
-
-#endif // Y2015_BOT3_CONTROL_LOOPS_ELEVATOR_ELEVATOR_MOTOR_PLANT_H_
diff --git a/y2015_bot3/control_loops/elevator/integral_elevator_motor_plant.cc b/y2015_bot3/control_loops/elevator/integral_elevator_motor_plant.cc
deleted file mode 100644
index 8573618..0000000
--- a/y2015_bot3/control_loops/elevator/integral_elevator_motor_plant.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "y2015_bot3/control_loops/elevator/integral_elevator_motor_plant.h"
-
-#include <vector>
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace y2015_bot3 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients() {
- Eigen::Matrix<double, 3, 3> A;
- A << 1.0, 0.00457884608813, 5.90742803242e-05, 0.0, 0.836406580139, 0.0229468687616, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 1> B;
- B << 5.90742803242e-05, 0.0229468687616, 0.0;
- Eigen::Matrix<double, 1, 3> C;
- C << 1.0, 0.0, 0.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<3, 1, 1>(A, B, C, D, U_max, U_min);
-}
-
-StateFeedbackController<3, 1, 1> MakeIntegralElevatorController() {
- Eigen::Matrix<double, 3, 1> L;
- L << 0.897632535808, 18.4574278406, 53.6443529697;
- Eigen::Matrix<double, 1, 3> K;
- K << 597.919635715, 17.5389953523, 1.0;
- Eigen::Matrix<double, 3, 3> A_inv;
- A_inv << 1.0, -0.0054744261904, 6.65466590119e-05, 0.0, 1.19559078533, -0.0274350648434, 0.0, 0.0, 1.0;
- return StateFeedbackController<3, 1, 1>(L, K, A_inv, MakeIntegralElevatorPlantCoefficients());
-}
-
-StateFeedbackPlant<3, 1, 1> MakeIntegralElevatorPlant() {
- ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>> plants(1);
- plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<3, 1, 1>>(new StateFeedbackPlantCoefficients<3, 1, 1>(MakeIntegralElevatorPlantCoefficients()));
- return StateFeedbackPlant<3, 1, 1>(&plants);
-}
-
-StateFeedbackLoop<3, 1, 1> MakeIntegralElevatorLoop() {
- ::std::vector< ::std::unique_ptr<StateFeedbackController<3, 1, 1>>> controllers(1);
- controllers[0] = ::std::unique_ptr<StateFeedbackController<3, 1, 1>>(new StateFeedbackController<3, 1, 1>(MakeIntegralElevatorController()));
- return StateFeedbackLoop<3, 1, 1>(&controllers);
-}
-
-} // namespace control_loops
-} // namespace y2015_bot3
diff --git a/y2015_bot3/control_loops/elevator/integral_elevator_motor_plant.h b/y2015_bot3/control_loops/elevator/integral_elevator_motor_plant.h
deleted file mode 100644
index 553c0c3..0000000
--- a/y2015_bot3/control_loops/elevator/integral_elevator_motor_plant.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef Y2015_BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
-#define Y2015_BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
-
-#include "frc971/control_loops/state_feedback_loop.h"
-
-namespace y2015_bot3 {
-namespace control_loops {
-
-StateFeedbackPlantCoefficients<3, 1, 1> MakeIntegralElevatorPlantCoefficients();
-
-StateFeedbackController<3, 1, 1> MakeIntegralElevatorController();
-
-StateFeedbackPlant<3, 1, 1> MakeIntegralElevatorPlant();
-
-StateFeedbackLoop<3, 1, 1> MakeIntegralElevatorLoop();
-
-} // namespace control_loops
-} // namespace y2015_bot3
-
-#endif // Y2015_BOT3_CONTROL_LOOPS_ELEVATOR_INTEGRAL_ELEVATOR_MOTOR_PLANT_H_
diff --git a/y2015_bot3/control_loops/python/BUILD b/y2015_bot3/control_loops/python/BUILD
new file mode 100644
index 0000000..663124f
--- /dev/null
+++ b/y2015_bot3/control_loops/python/BUILD
@@ -0,0 +1,13 @@
+package(default_visibility = ['//y2015_bot3:__subpackages__'])
+
+py_binary(
+ name = 'elevator3',
+ srcs = [
+ 'elevator3.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
diff --git a/y2015_bot3/control_loops/python/elevator3.py b/y2015_bot3/control_loops/python/elevator3.py
index 853b6b3..0c62e5f 100755
--- a/y2015_bot3/control_loops/python/elevator3.py
+++ b/y2015_bot3/control_loops/python/elevator3.py
@@ -1,12 +1,20 @@
#!/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
import matplotlib
from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+
class Elevator(control_loop.ControlLoop):
def __init__(self, name="Elevator", mass=None):
super(Elevator, self).__init__(name)
@@ -72,16 +80,14 @@
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print 'K', self.K
- print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+ glog.info('K %s', str(self.K))
+ glog.info('Poles are %s', str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.30
self.ipl = 0.10
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
- # print 'L is', self.L
-
q_pos = 0.05
q_vel = 2.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
@@ -93,9 +99,8 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- # print 'Kal', self.KalmanGain
self.L = self.A * self.KalmanGain
- print 'KalL is', self.L
+ glog.info('KalL is %s', str(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
@@ -212,7 +217,7 @@
# print "Time: ", self.t[-1]
# break
- print "Time: ", self.t[-1]
+ glog.debug('Time: %f', self.t[-1])
def Plot(self):
@@ -232,6 +237,8 @@
def main(argv):
+ argv = FLAGS(argv)
+
loaded_mass = 7+4.0
#loaded_mass = 0
#observer_elevator = None
@@ -248,7 +255,7 @@
for i in xrange(0, 7):
elevator = Elevator(mass=i*totemass + loaded_mass)
- print 'Actual poles are', numpy.linalg.eig(elevator.A - elevator.B * elevator_controller.K[0, 0:2])[0]
+ glog.info('Actual poles are %s', str(numpy.linalg.eig(elevator.A - elevator.B * elevator_controller.K[0, 0:2])[0]))
elevator.X = initial_X
scenario_plotter.run_test(elevator, goal=up_R, controller_elevator=elevator_controller,
@@ -256,28 +263,23 @@
scenario_plotter.run_test(elevator, goal=down_R, controller_elevator=elevator_controller,
observer_elevator=observer_elevator, iterations=200)
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
# Write the generated constants out to a file.
if len(argv) != 5:
- print "Expected .h file name and .cc file name for the Elevator and integral elevator."
+ glog.fatal('Expected .h file name and .cc file name for the Elevator and integral elevator.')
else:
design_mass = 4*totemass + loaded_mass
elevator = Elevator("Elevator", mass=design_mass)
loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator],
- namespaces=['y2015_bot3', 'control_loops'])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
+ namespaces=['y2015_bot3', 'control_loops', 'elevator'])
+ loop_writer.Write(argv[1], argv[2])
integral_elevator = IntegralElevator("IntegralElevator", mass=design_mass)
integral_loop_writer = control_loop.ControlLoopWriter("IntegralElevator", [integral_elevator],
- namespaces=['y2015_bot3', 'control_loops'])
- if argv[3][-3:] == '.cc':
- integral_loop_writer.Write(argv[4], argv[3])
- else:
- integral_loop_writer.Write(argv[3], argv[4])
+ namespaces=['y2015_bot3', 'control_loops', 'elevator'])
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2015_bot3/control_loops/update_elevator.sh b/y2015_bot3/control_loops/update_elevator.sh
deleted file mode 100755
index 4f0e359..0000000
--- a/y2015_bot3/control_loops/update_elevator.sh
+++ /dev/null
@@ -1,10 +0,0 @@
-#!/bin/bash
-#
-# Updates the elevator controllers.
-
-cd $(dirname $0)
-
-export PYTHONPATH=../../frc971/control_loops/python
-
-./python/elevator3.py elevator/elevator_motor_plant.h \
- elevator/elevator_motor_plant.cc elevator/integral_elevator_motor_plant.cc elevator/integral_elevator_motor_plant.h