Converted state feedback controller and plant over to being gain scheduled, and used them in the indexer. Also wrote the python to make it easy to design gain scheduled controllers.
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
index d9eb1f8..7dc86c7 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -40,7 +40,7 @@
initial_position_ = initial_position;
angle_adjust_plant_->X(0, 0) = initial_position_;
angle_adjust_plant_->X(1, 0) = 0.0;
- angle_adjust_plant_->Y = angle_adjust_plant_->C * angle_adjust_plant_->X;
+ angle_adjust_plant_->Y = angle_adjust_plant_->C() * angle_adjust_plant_->X;
last_position_ = angle_adjust_plant_->Y(0, 0);
calibration_value_[0] = 0.0;
calibration_value_[1] = 0.0;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
index 2c97ca4..c23a64b 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.cc
@@ -1,12 +1,13 @@
#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
-
-StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
A << 1.0, 0.00844804908295, 0.0, 0.706562970689;
Eigen::Matrix<double, 2, 1> B;
@@ -19,16 +20,28 @@
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop() {
+StateFeedbackController<2, 1, 1> MakeAngleAdjustController() {
Eigen::Matrix<double, 2, 1> L;
L << 1.60656297069, 51.0341417582;
Eigen::Matrix<double, 1, 2> K;
- K << 708.103935619, 16.2677296642;
- return StateFeedbackLoop<2, 1, 1>(L, K, MakeAngleAdjustPlant());
+ K << 311.565731672, 11.2839301509;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeAngleAdjustPlantCoefficients());
}
-} // namespace frc971
+StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeAngleAdjustPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeAngleAdjustController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
index 8085154..8db821f 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h
@@ -1,16 +1,20 @@
-#ifndef FRC971_CONTROL_LOOPS_ANGLEADJUST_ANGLEADJUST_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_ANGLEADJUST_ANGLEADJUST_MOTOR_PLANT_H_
+#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeAngleAdjustPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeAngleAdjustController();
+
StateFeedbackPlant<2, 1, 1> MakeAngleAdjustPlant();
StateFeedbackLoop<2, 1, 1> MakeAngleAdjustLoop();
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
-#endif // FRC971_CONTROL_LOOPS_ANGLEADJUST_ANGLEADJUST_MOTOR_PLANT_H_
+#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 4053eae..72597c6 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -253,6 +253,11 @@
status->ready_to_intake = false;
+ // Set the controller to use to be the one designed for the current number of
+ // discs in the hopper. This is safe since the controller prevents the index
+ // from being set out of bounds and picks the closest controller.
+ wrist_loop_->set_controller_index(frisbees_.size());
+
// Compute a safe index position that we can use.
if (position) {
wrist_loop_->Y << position->index_position;
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index 1172ee2..3e2e674 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -588,6 +588,7 @@
void Simulate() {
EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+ index_plant_->set_plant_index(frisbees.size());
index_plant_->U << my_index_loop_.output->index_voltage;
index_plant_->Update();
diff --git a/frc971/control_loops/index/index_motor_plant.cc b/frc971/control_loops/index/index_motor_plant.cc
index 51661fb..10fa60c 100644
--- a/frc971/control_loops/index/index_motor_plant.cc
+++ b/frc971/control_loops/index/index_motor_plant.cc
@@ -1,12 +1,29 @@
#include "frc971/control_loops/index/index_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex0DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00832470485812, 0.0, 0.68478614982;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.06201698456, 11.6687573378;
+ 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);
+}
-StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex1DiscPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
Eigen::Matrix<double, 2, 1> B;
@@ -19,16 +36,116 @@
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex2DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ 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> MakeIndex3DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00901822957243, 0.0, 0.810292182273;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0363437103863, 7.02270693014;
+ 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> MakeIndex4DiscPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00927953099869, 0.0, 0.859452713637;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0266707124098, 5.20285570613;
+ 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> MakeIndex0DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.58478614982, 48.4122215588;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1.90251621122, 0.0460029989298;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex0DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex1DiscController() {
Eigen::Matrix<double, 2, 1> L;
L << 1.64731520998, 56.0569452572;
Eigen::Matrix<double, 1, 2> K;
- K << 2.40538224198, 0.0619371641882;
- return StateFeedbackLoop<2, 1, 1>(L, K, MakeIndexPlant());
+ K << 2.37331047876, 0.0642434141389;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex1DiscPlantCoefficients());
}
-} // namespace frc971
+StateFeedbackController<2, 1, 1> MakeIndex2DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 2.37331047876, 0.0642434141389;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex2DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex3DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71029218227, 64.1044007344;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 3.16117420545, 0.0947502706704;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex3DiscPlantCoefficients());
+}
+
+StateFeedbackController<2, 1, 1> MakeIndex4DiscController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.75945271364, 70.6153894746;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 4.26688750446, 0.137549804289;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeIndex4DiscPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(5);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex0DiscPlantCoefficients());
+ plants[1] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex1DiscPlantCoefficients());
+ plants[2] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex2DiscPlantCoefficients());
+ plants[3] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex3DiscPlantCoefficients());
+ plants[4] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeIndex4DiscPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(5);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeIndex0DiscController());
+ controllers[1] = new StateFeedbackController<2, 1, 1>(MakeIndex1DiscController());
+ controllers[2] = new StateFeedbackController<2, 1, 1>(MakeIndex2DiscController());
+ controllers[3] = new StateFeedbackController<2, 1, 1>(MakeIndex3DiscController());
+ controllers[4] = new StateFeedbackController<2, 1, 1>(MakeIndex4DiscController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/index_motor_plant.h b/frc971/control_loops/index/index_motor_plant.h
index 4600ccf..e82db6a 100644
--- a/frc971/control_loops/index/index_motor_plant.h
+++ b/frc971/control_loops/index/index_motor_plant.h
@@ -6,11 +6,31 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex0DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex0DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex1DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex1DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex2DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex2DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex3DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex3DiscController();
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeIndex4DiscPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeIndex4DiscController();
+
StateFeedbackPlant<2, 1, 1> MakeIndexPlant();
StateFeedbackLoop<2, 1, 1> MakeIndexLoop();
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_INDEX_INDEX_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/index/transfer_motor_plant.cc b/frc971/control_loops/index/transfer_motor_plant.cc
index 9333f6a..0852b26 100644
--- a/frc971/control_loops/index/transfer_motor_plant.cc
+++ b/frc971/control_loops/index/transfer_motor_plant.cc
@@ -1,12 +1,13 @@
#include "frc971/control_loops/index/transfer_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
-
-StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeTransferPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
Eigen::Matrix<double, 2, 1> B;
@@ -19,16 +20,28 @@
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
+StateFeedbackController<2, 1, 1> MakeTransferController() {
Eigen::Matrix<double, 2, 1> L;
L << 1.64731520998, 56.0569452572;
Eigen::Matrix<double, 1, 2> K;
K << 1.06905877421, 0.0368709177253;
- return StateFeedbackLoop<2, 1, 1>(L, K, MakeTransferPlant());
+ return StateFeedbackController<2, 1, 1>(L, K, MakeTransferPlantCoefficients());
}
-} // namespace frc971
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeTransferPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeTransferController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index/transfer_motor_plant.h b/frc971/control_loops/index/transfer_motor_plant.h
index 565e8c7..596f9b3 100644
--- a/frc971/control_loops/index/transfer_motor_plant.h
+++ b/frc971/control_loops/index/transfer_motor_plant.h
@@ -6,11 +6,15 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeTransferPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeTransferController();
+
StateFeedbackPlant<2, 1, 1> MakeTransferPlant();
StateFeedbackLoop<2, 1, 1> MakeTransferLoop();
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_INDEX_TRANSFER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
index f49c305..bee58bc 100755
--- a/frc971/control_loops/python/angle_adjust.py
+++ b/frc971/control_loops/python/angle_adjust.py
@@ -44,7 +44,7 @@
self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
self.dt, self.C)
- self.PlaceControllerPoles([.5, .5])
+ self.PlaceControllerPoles([.45, .8])
self.rpl = .05
self.ipl = 0.008
@@ -92,11 +92,11 @@
if len(argv) != 3:
print "Expected .cc file name and .h file name"
else:
+ loop_writer = control_loop.ControlLoopWriter("AngleAdjust", [angle_adjust])
if argv[1][-3:] == '.cc':
- print '.cc file is second'
+ loop_writer.Write(argv[2], argv[1])
else:
- angle_adjust.DumpHeaderFile(argv[1])
- angle_adjust.DumpCppFile(argv[2], argv[1])
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index c5316e7..4ff2623 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -1,6 +1,136 @@
import controls
import numpy
+class ControlLoopWriter(object):
+ def __init__(self, gain_schedule_name, loops, namespaces=None):
+ """Constructs a control loop writer.
+
+ Args:
+ gain_schedule_name: string, Name of the overall controller.
+ loops: array[ControlLoop], a list of control loops to gain schedule
+ in order.
+ namespaces: array[string], a list of names of namespaces to nest in
+ order. If None, the default will be used.
+ """
+ self._gain_schedule_name = gain_schedule_name
+ self._loops = loops
+ if namespaces:
+ self._namespaces = namespaces
+ else:
+ self._namespaces = ['frc971', 'control_loops']
+
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
+
+ self._namespace_end = '\n'.join(
+ ['} // namespace %s' % name for name in reversed(self._namespaces)])
+
+ def _HeaderGuard(self, header_file):
+ return ('FRC971_CONTROL_LOOPS_' +
+ 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)
+
+ def _GenericType(self, typename):
+ """Returns a loop template using typename for the type."""
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+ return '%s<%d, %d, %d>' % (
+ typename, num_states, num_inputs, num_outputs)
+
+ def _ControllerType(self):
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
+
+ def _LoopType(self):
+ """Returns a template name for StateFeedbackLoop."""
+ return self._GenericType('StateFeedbackLoop')
+
+ def _PlantType(self):
+ """Returns a template name for StateFeedbackPlant."""
+ return self._GenericType('StateFeedbackPlant')
+
+ def _CoeffType(self):
+ """Returns a template name for StateFeedbackPlantCoefficients."""
+ return self._GenericType('StateFeedbackPlantCoefficients')
+
+ def WriteHeader(self, header_file):
+ """Writes the header file to the file named header_file."""
+ with open(header_file, 'w') as fd:
+ header_guard = self._HeaderGuard(header_file)
+ fd.write('#ifndef %s\n'
+ '#define %s\n\n' % (header_guard, header_guard))
+ fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ fd.write('\n')
+
+ fd.write(self._namespace_start)
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(loop.DumpPlantHeader())
+ fd.write('\n')
+ fd.write(loop.DumpControllerHeader())
+ fd.write('\n')
+
+ fd.write('%s Make%sPlant();\n\n' %
+ (self._PlantType(), self._gain_schedule_name))
+
+ fd.write('%s Make%sLoop();\n\n' %
+ (self._LoopType(), self._gain_schedule_name))
+
+ fd.write(self._namespace_end)
+ fd.write('\n\n')
+ fd.write("#endif // %s\n" % header_guard)
+
+ 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 \"frc971/control_loops/%s\"\n' % header_file_name)
+ fd.write('\n')
+ fd.write('#include <vector>\n')
+ fd.write('\n')
+ fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ fd.write('\n')
+ fd.write(self._namespace_start)
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(loop.DumpPlant())
+ fd.write('\n')
+
+ for loop in self._loops:
+ fd.write(loop.DumpController())
+ fd.write('\n')
+
+ fd.write('%s Make%sPlant() {\n' %
+ (self._PlantType(), self._gain_schedule_name))
+ fd.write(' ::std::vector<%s *> plants(%d);\n' % (
+ self._CoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' plants[%d] = new %s(%s);\n' %
+ (index, self._CoeffType(),
+ loop.PlantFunction()))
+ fd.write(' return %s(plants);\n' % self._PlantType())
+ fd.write('}\n\n')
+
+ fd.write('%s Make%sLoop() {\n' %
+ (self._LoopType(), self._gain_schedule_name))
+ fd.write(' ::std::vector<%s *> controllers(%d);\n' % (
+ self._ControllerType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' controllers[%d] = new %s(%s);\n' %
+ (index, self._ControllerType(),
+ loop.ControllerFunction()))
+ fd.write(' return %s(controllers);\n' % self._LoopType())
+ fd.write('}\n\n')
+
+ fd.write(self._namespace_end)
+ fd.write('\n')
+
+
class ControlLoop(object):
def __init__(self, name):
"""Constructs a control loop object.
@@ -10,20 +140,6 @@
"""
self._name = name
- self._namespace_start = ("namespace frc971 {\n"
- "namespace control_loops {\n\n")
-
- self._namespace_end = ("} // namespace frc971\n"
- "} // namespace control_loops\n")
-
- self._header_start = ("#ifndef FRC971_CONTROL_LOOPS_%s_%s_MOTOR_PLANT_H_\n"
- "#define FRC971_CONTROL_LOOPS_%s_%s_MOTOR_PLANT_H_\n\n"
- % (self._name.upper(), self._name.upper(),
- self._name.upper(), self._name.upper()))
-
- self._header_end = ("#endif // FRC971_CONTROL_LOOPS_%s_%s_MOTOR_PLANT_H_\n"
- % (self._name.upper(), self._name.upper()))
-
def ContinuousToDiscrete(self, A_continuous, B_continuous, dt, C):
"""Calculates the discrete time values for A and B as well as initializing
X and Y to the correct sizes.
@@ -80,44 +196,36 @@
string, The C++ commands required to populate a variable named matrix_name
with the contents of matrix.
"""
- ans = [" Eigen::Matrix<double, %d, %d> %s;\n" % (
+ ans = [' Eigen::Matrix<double, %d, %d> %s;\n' % (
matrix.shape[0], matrix.shape[1], matrix_name)]
first = True
for x in xrange(matrix.shape[0]):
for y in xrange(matrix.shape[1]):
element = matrix[x, y]
if first:
- ans.append(" %s << " % matrix_name)
+ ans.append(' %s << ' % matrix_name)
first = False
else:
- ans.append(", ")
+ ans.append(', ')
ans.append(str(element))
- ans.append(";\n")
- return "".join(ans)
+ ans.append(';\n')
+ return ''.join(ans)
- def _DumpPlantHeader(self, plant_name):
+ def DumpPlantHeader(self):
"""Writes out a c++ header declaration which will create a Plant object.
- Args:
- plant_name: string, the name of the plant. Used to create the name of the
- function. The function name will be Make<plant_name>Plant().
-
Returns:
string, The header declaration for the function.
"""
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return "StateFeedbackPlant<%d, %d, %d> Make%sPlant();\n" % (
- num_states, num_inputs, num_outputs, plant_name)
+ return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
+ num_states, num_inputs, num_outputs, self._name)
- def _DumpPlant(self, plant_name):
- """Writes out a c++ function which will create a Plant object.
-
- Args:
- plant_name: string, the name of the plant. Used to create the name of the
- function. The function name will be Make<plant_name>Plant().
+ def DumpPlant(self):
+ """Writes out a c++ function which will create a PlantCoefficients object.
Returns:
string, The function which will create the object.
@@ -125,28 +233,33 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ["StateFeedbackPlant<%d, %d, %d> Make%sPlant() {\n" % (
- num_states, num_inputs, num_outputs, plant_name)]
+ ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
+ ' Make%sPlantCoefficients() {\n' % (
+ num_states, num_inputs, num_outputs, self._name)]
- ans.append(self._DumpMatrix("A", self.A))
- ans.append(self._DumpMatrix("B", self.B))
- ans.append(self._DumpMatrix("C", self.C))
- ans.append(self._DumpMatrix("D", self.D))
- ans.append(self._DumpMatrix("U_max", self.U_max))
- ans.append(self._DumpMatrix("U_min", self.U_min))
+ ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('B', self.B))
+ ans.append(self._DumpMatrix('C', self.C))
+ ans.append(self._DumpMatrix('D', self.D))
+ ans.append(self._DumpMatrix('U_max', self.U_max))
+ ans.append(self._DumpMatrix('U_min', self.U_min))
- ans.append(" return StateFeedbackPlant<%d, %d, %d>"
- "(A, B, C, D, U_max, U_min);\n" % (num_states, num_inputs,
+ ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
+ '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
num_outputs))
- ans.append("}\n")
- return "".join(ans)
+ ans.append('}\n')
+ return ''.join(ans)
- def _DumpLoopHeader(self, loop_name):
- """Writes out a c++ header declaration which will create a Loop object.
+ def PlantFunction(self):
+ """Returns the name of the plant coefficient function."""
+ return 'Make%sPlantCoefficients()' % self._name
- Args:
- loop_name: string, the name of the loop. Used to create the name of the
- function. The function name will be Make<loop_name>Loop().
+ def ControllerFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sController()' % self._name
+
+ def DumpControllerHeader(self):
+ """Writes out a c++ header declaration which will create a Controller object.
Returns:
string, The header declaration for the function.
@@ -154,16 +267,11 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return "StateFeedbackLoop<%d, %d, %d> Make%sLoop();\n" % (
- num_states, num_inputs, num_outputs, loop_name)
+ return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
+ num_states, num_inputs, num_outputs, self.ControllerFunction())
- def _DumpLoop(self, loop_name):
- """Returns a c++ function which will create a Loop object.
-
- Args:
- loop_name: string, the name of the loop. Used to create the name of the
- function and create the plant. The function name will be
- Make<loop_name>Loop().
+ def DumpController(self):
+ """Returns a c++ function which will create a Controller object.
Returns:
string, The function which will create the object.
@@ -171,52 +279,14 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ["StateFeedbackLoop<%d, %d, %d> Make%sLoop() {\n" % (
- num_states, num_inputs, num_outputs, loop_name)]
+ ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
+ num_states, num_inputs, num_outputs, self.ControllerFunction())]
- ans.append(self._DumpMatrix("L", self.L))
- ans.append(self._DumpMatrix("K", self.K))
+ ans.append(self._DumpMatrix('L', self.L))
+ ans.append(self._DumpMatrix('K', self.K))
- ans.append(" return StateFeedbackLoop<%d, %d, %d>"
- "(L, K, Make%sPlant());\n" % (num_states, num_inputs,
- num_outputs, loop_name))
- ans.append("}\n")
- return "".join(ans)
-
- def DumpHeaderFile(self, file_name):
- """Writes the header file for creating a Plant and Loop object.
-
- Args:
- file_name: string, name of the file to write the header file to.
- """
- with open(file_name, "w") as fd:
- fd.write(self._header_start)
- fd.write("#include \"frc971/control_loops/state_feedback_loop.h\"\n")
- fd.write('\n')
- fd.write(self._namespace_start)
- fd.write(self._DumpPlantHeader(self._name))
- fd.write('\n')
- fd.write(self._DumpLoopHeader(self._name))
- fd.write('\n')
- fd.write(self._namespace_end)
- fd.write('\n')
- fd.write(self._header_end)
-
- def DumpCppFile(self, file_name, header_file_name):
- """Writes the C++ file for creating a Plant and Loop object.
-
- Args:
- file_name: string, name of the file to write the header file to.
- """
- with open(file_name, "w") as fd:
- fd.write("#include \"frc971/control_loops/%s\"\n" % header_file_name)
- fd.write('\n')
- fd.write("#include \"frc971/control_loops/state_feedback_loop.h\"\n")
- fd.write('\n')
- fd.write(self._namespace_start)
- fd.write('\n')
- fd.write(self._DumpPlant(self._name))
- fd.write('\n')
- fd.write(self._DumpLoop(self._name))
- fd.write('\n')
- fd.write(self._namespace_end)
+ ans.append(' return StateFeedbackController<%d, %d, %d>'
+ '(L, K, Make%sPlantCoefficients());\n' % (num_states, num_inputs,
+ num_outputs, self._name))
+ ans.append('}\n')
+ return ''.join(ans)
diff --git a/frc971/control_loops/python/index.py b/frc971/control_loops/python/index.py
index c65397b..f165afe 100755
--- a/frc971/control_loops/python/index.py
+++ b/frc971/control_loops/python/index.py
@@ -6,8 +6,8 @@
from matplotlib import pylab
class Index(control_loop.ControlLoop):
- def __init__(self):
- super(Index, self).__init__("Index")
+ def __init__(self, J=0.00013, name="Index"):
+ super(Index, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.4862
# Stall Current in Amps
@@ -17,7 +17,7 @@
# Free Current in Amps
self.free_current = 1.5
# Moment of inertia of the index in kg m^2
- self.J = 0.00013
+ self.J = J
# Resistance of the motor
self.R = 12.0 / self.stall_current + 0.024 + .003
# Motor velocity constant
@@ -43,7 +43,7 @@
self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
self.dt, self.C)
- self.PlaceControllerPoles([.5, .55])
+ self.PlaceControllerPoles([.40, .63])
self.rpl = .05
self.ipl = 0.008
@@ -80,15 +80,25 @@
pylab.plot(range(100), close_loop_x)
pylab.show()
+ # Set the constants for the number of discs that we expect to see.
+ # The c++ code expects that the index in the array will be the number of
+ # discs.
+ index0 = Index(0.00010, "Index0Disc")
+ index1 = Index(0.00013, "Index1Disc")
+ index2 = Index(0.00013, "Index2Disc")
+ index3 = Index(0.00018, "Index3Disc")
+ index4 = Index(0.00025, "Index4Disc")
+
# Write the generated constants out to a file.
if len(argv) != 3:
print "Expected .h file name and .c file name"
else:
+ loop_writer = control_loop.ControlLoopWriter(
+ "Index", [index0, index1, index2, index3, index4])
if argv[1][-3:] == '.cc':
- print '.cc file is second'
+ loop_writer.Write(argv[2], argv[1])
else:
- index.DumpHeaderFile(argv[1])
- index.DumpCppFile(argv[2], argv[1])
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 25a6795..83beb90 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -17,7 +17,7 @@
# Free Current in Amps
self.free_current = 1.4
# Moment of inertia of the shooter wheel in kg m^2
- self.J = 0.0040
+ self.J = 0.0032
# Resistance of the motor, divided by 2 to account for the 2 motors
self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
@@ -129,11 +129,11 @@
if len(argv) != 3:
print "Expected .h file name and .cc file name"
else:
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
if argv[1][-3:] == '.cc':
- print '.cc file is second'
+ loop_writer.Write(argv[2], argv[1])
else:
- shooter.DumpHeaderFile(argv[1])
- shooter.DumpCppFile(argv[2], argv[1])
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
diff --git a/frc971/control_loops/python/transfer.py b/frc971/control_loops/python/transfer.py
index 2ec7926..d7818a3 100755
--- a/frc971/control_loops/python/transfer.py
+++ b/frc971/control_loops/python/transfer.py
@@ -84,8 +84,11 @@
if len(argv) != 3:
print "Expected .cc file name and .h file name"
else:
- transfer.DumpHeaderFile(argv[1])
- transfer.DumpCppFile(argv[2], argv[1])
+ loop_writer = control_loop.ControlLoopWriter("Transfer", [transfer])
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrist.py
index 6fc9c27..20f9c40 100755
--- a/frc971/control_loops/python/wrist.py
+++ b/frc971/control_loops/python/wrist.py
@@ -85,11 +85,11 @@
if len(argv) != 3:
print "Expected .h file name and .cc file name"
else:
+ loop_writer = control_loop.ControlLoopWriter("Wrist", [wrist])
if argv[1][-3:] == '.cc':
- print '.cc file is second'
+ loop_writer.Write(argv[2], argv[1])
else:
- wrist.DumpHeaderFile(argv[1])
- wrist.DumpCppFile(argv[2], argv[1])
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
index 809d918..1a623f3 100644
--- a/frc971/control_loops/shooter/shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -1,16 +1,17 @@
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
-
-StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00993695674898, 0.0, 0.987417901985;
+ A << 1.0, 0.00992127884628, 0.0, 0.984297191531;
Eigen::Matrix<double, 2, 1> B;
- B << 0.00319456032937, 0.637566599616;
+ B << 0.00398899915072, 0.795700859132;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -19,16 +20,28 @@
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeShooterController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.08429719153, 29.2677479765;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 0.955132813139, 0.50205697652;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeShooterPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
}
StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.08741790198, 29.5581442884;
- Eigen::Matrix<double, 1, 2> K;
- K << 1.19203233114, 0.631478943582;
- return StateFeedbackLoop<2, 1, 1>(L, K, MakeShooterPlant());
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeShooterController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
}
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
index ac42b0b..3588605 100644
--- a/frc971/control_loops/shooter/shooter_motor_plant.h
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -6,11 +6,15 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeShooterController();
+
StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index f955796..7465672 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -9,7 +9,7 @@
#include "Eigen/Dense"
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-class StateFeedbackPlant {
+class StateFeedbackPlantCoefficients {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -20,23 +20,16 @@
const Eigen::Matrix<double, number_of_inputs, 1> U_min;
const Eigen::Matrix<double, number_of_inputs, 1> U_max;
- Eigen::Matrix<double, number_of_states, 1> X;
- Eigen::Matrix<double, number_of_outputs, 1> Y;
- Eigen::Matrix<double, number_of_inputs, 1> U;
-
- StateFeedbackPlant(const StateFeedbackPlant &other)
+ StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
: A(other.A),
B(other.B),
C(other.C),
D(other.D),
U_min(other.U_min),
U_max(other.U_max) {
- X.setZero();
- Y.setZero();
- U.setZero();
}
- StateFeedbackPlant(
+ StateFeedbackPlantCoefficients(
const Eigen::Matrix<double, number_of_states, number_of_states> &A,
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
@@ -49,18 +42,96 @@
D(D),
U_min(U_min),
U_max(U_max) {
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static const int kNumStates = number_of_states;
+ static const int kNumOutputs = number_of_outputs;
+ static const int kNumInputs = number_of_inputs;
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackPlant {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ ::std::vector<StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs> *> coefficients_;
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ return coefficients().A;
+ }
+ double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ return coefficients().B;
+ }
+ double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ return coefficients().C;
+ }
+ double C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+ return coefficients().D;
+ }
+ double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ return coefficients().U_min;
+ }
+ double U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return coefficients().U_max;
+ }
+ double U_max(int i, int j) const { return U_max()(i, j); }
+
+ const StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[plant_index_];
+ }
+
+ int plant_index() const { return plant_index_; }
+ void set_plant_index(int plant_index) {
+ if (plant_index < 0) {
+ plant_index_ = 0;
+ } else if (plant_index >= static_cast<int>(coefficients_.size())) {
+ plant_index_ = static_cast<int>(coefficients_.size());
+ } else {
+ plant_index_ = plant_index;
+ }
+ }
+
+ void Reset() {
X.setZero();
Y.setZero();
U.setZero();
}
+ Eigen::Matrix<double, number_of_states, 1> X;
+ Eigen::Matrix<double, number_of_outputs, 1> Y;
+ Eigen::Matrix<double, number_of_inputs, 1> U;
+
+ StateFeedbackPlant(
+ const ::std::vector<StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs,
+ number_of_outputs> *> &coefficients)
+ : coefficients_(coefficients),
+ plant_index_(0) {
+ Reset();
+ }
+
+ StateFeedbackPlant(StateFeedbackPlant &&other)
+ : plant_index_(0) {
+ Reset();
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
virtual ~StateFeedbackPlant() {}
// Assert that U is within the hardware range.
virtual void CheckU() {
for (int i = 0; i < kNumOutputs; ++i) {
- assert(U[i] <= U_max[i]);
- assert(U[i] >= U_min[i]);
+ assert(U(i, 0) <= U_max(i, 0));
+ assert(U(i, 0) >= U_min(i, 0));
}
}
@@ -69,8 +140,8 @@
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU();
- X = A * X + B * U;
- Y = C * X + D * U;
+ X = A() * X + B() * U;
+ Y = C() * X + D() * U;
}
protected:
@@ -78,6 +149,9 @@
static const int kNumStates = number_of_states;
static const int kNumOutputs = number_of_outputs;
static const int kNumInputs = number_of_inputs;
+
+ private:
+ int plant_index_;
};
// A Controller is a structure which holds a plant and the K and L matrices.
@@ -88,14 +162,14 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
const Eigen::Matrix<double, number_of_outputs, number_of_states> K;
- StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> plant;
+ StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs> plant;
StateFeedbackController(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
- const StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> &plant)
+ const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs> &plant)
: L(L),
K(K),
plant(plant) {
@@ -148,12 +222,12 @@
Eigen::Matrix<double, number_of_outputs, 1> Y;
::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> *> controllers;
+ number_of_outputs> *> controllers_;
const StateFeedbackController<
number_of_states, number_of_inputs, number_of_outputs>
&controller() const {
- return *controllers[controller_index_];
+ return *controllers_[controller_index_];
}
void Reset() {
@@ -165,19 +239,21 @@
Y.setZero();
}
- StateFeedbackLoop(const StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> &controller)
+ StateFeedbackLoop(
+ const StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> &controller)
: controller_index_(0) {
- controllers.push_back(
+ controllers_.push_back(
new StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs>(controller));
Reset();
}
StateFeedbackLoop(
- const ::std::vector<StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs> *> &controllers)
- : controllers(controllers),
+ const ::std::vector<StateFeedbackController<
+ number_of_states, number_of_inputs,
+ number_of_outputs> *> &controllers)
+ : controllers_(controllers),
controller_index_(0) {
Reset();
}
@@ -185,10 +261,10 @@
StateFeedbackLoop(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
- const StateFeedbackPlant<number_of_states, number_of_inputs,
+ const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &plant)
: controller_index_(0) {
- controllers.push_back(
+ controllers_.push_back(
new StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs>(L, K, plant));
@@ -234,7 +310,11 @@
// Sets the current controller to be index and verifies that it isn't out of
// range.
void set_controller_index(int index) {
- if (index >= 0 && index < controllers.size()) {
+ if (index < 0) {
+ controller_index_ = 0;
+ } else if (index >= static_cast<int>(controllers_.size())) {
+ controller_index_ = static_cast<int>(controllers_.size());
+ } else {
controller_index_ = index;
}
}
diff --git a/frc971/control_loops/update_angle_adjust.sh b/frc971/control_loops/update_angle_adjust.sh
new file mode 100755
index 0000000..575dd32
--- /dev/null
+++ b/frc971/control_loops/update_angle_adjust.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the angle adjust controller.
+
+./python/angle_adjust.py angle_adjust/angle_adjust_motor_plant.h angle_adjust/angle_adjust_motor_plant.cc
diff --git a/frc971/control_loops/update_index.sh b/frc971/control_loops/update_index.sh
new file mode 100755
index 0000000..3b819b4
--- /dev/null
+++ b/frc971/control_loops/update_index.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the index controller.
+
+./python/index.py index/index_motor_plant.h index/index_motor_plant.cc
diff --git a/frc971/control_loops/update_shooter.sh b/frc971/control_loops/update_shooter.sh
new file mode 100755
index 0000000..db98547
--- /dev/null
+++ b/frc971/control_loops/update_shooter.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the shooter controller.
+
+./python/shooter.py shooter/shooter_motor_plant.h shooter/shooter_motor_plant.cc
diff --git a/frc971/control_loops/update_transfer.sh b/frc971/control_loops/update_transfer.sh
new file mode 100755
index 0000000..d7820fa
--- /dev/null
+++ b/frc971/control_loops/update_transfer.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the transfer controller.
+
+./python/transfer.py index/transfer_motor_plant.h index/transfer_motor_plant.cc
diff --git a/frc971/control_loops/update_wrist.sh b/frc971/control_loops/update_wrist.sh
new file mode 100755
index 0000000..ea4c353
--- /dev/null
+++ b/frc971/control_loops/update_wrist.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+#
+# Updates the wrist controller.
+
+./python/wrist.py wrist/wrist_motor_plant.h wrist/wrist_motor_plant.cc
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
index 83703fa..e3e6039 100644
--- a/frc971/control_loops/wrist/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -38,7 +38,7 @@
initial_position_ = initial_position;
wrist_plant_->X(0, 0) = initial_position_;
wrist_plant_->X(1, 0) = 0.0;
- wrist_plant_->Y = wrist_plant_->C * wrist_plant_->X;
+ wrist_plant_->Y = wrist_plant_->C() * wrist_plant_->X;
last_position_ = wrist_plant_->Y(0, 0);
calibration_value_ = 0.0;
}
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.cc b/frc971/control_loops/wrist/wrist_motor_plant.cc
index 79d62db..4a2b0ad 100644
--- a/frc971/control_loops/wrist/wrist_motor_plant.cc
+++ b/frc971/control_loops/wrist/wrist_motor_plant.cc
@@ -1,12 +1,13 @@
#include "frc971/control_loops/wrist/wrist_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
-
-StateFeedbackPlant<2, 1, 1> MakeWristPlant() {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeWristPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
Eigen::Matrix<double, 2, 1> B;
@@ -19,16 +20,28 @@
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackLoop<2, 1, 1> MakeWristLoop() {
+StateFeedbackController<2, 1, 1> MakeWristController() {
Eigen::Matrix<double, 2, 1> L;
L << 1.71581823335, 64.8264890043;
Eigen::Matrix<double, 1, 2> K;
K << 124.10047341, 5.30734038612;
- return StateFeedbackLoop<2, 1, 1>(L, K, MakeWristPlant());
+ return StateFeedbackController<2, 1, 1>(L, K, MakeWristPlantCoefficients());
}
-} // namespace frc971
+StateFeedbackPlant<2, 1, 1> MakeWristPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeWristPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeWristLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeWristController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrist/wrist_motor_plant.h b/frc971/control_loops/wrist/wrist_motor_plant.h
index f2b3ac9..169e923 100644
--- a/frc971/control_loops/wrist/wrist_motor_plant.h
+++ b/frc971/control_loops/wrist/wrist_motor_plant.h
@@ -6,11 +6,15 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeWristPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeWristController();
+
StateFeedbackPlant<2, 1, 1> MakeWristPlant();
StateFeedbackLoop<2, 1, 1> MakeWristLoop();
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_WRIST_WRIST_MOTOR_PLANT_H_