Added control loops for all subsystems and made tests run.
Change-Id: I66542db4355a89f6d24c1ad4772004182197c863
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 83a4a50..64ec631 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -3,6 +3,7 @@
#include <memory>
#include "gtest/gtest.h"
+#include "aos/common/controls/sensor_generation.q.h"
#include "aos/common/queue.h"
#include "aos/common/queue_testutils.h"
#include "frc971/control_loops/claw/claw.q.h"
@@ -23,7 +24,7 @@
ClawSimulation()
: claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeClawPlant())),
claw_queue_(".frc971.control_loops.claw_queue",
- 0x78d8e372, ".frc971.control_loops.claw_queue.goal",
+ 0x9d7452fb, ".frc971.control_loops.claw_queue.goal",
".frc971.control_loops.claw_queue.position",
".frc971.control_loops.claw_queue.output",
".frc971.control_loops.claw_queue.status") {
@@ -31,6 +32,13 @@
// Sends a queue message with the position.
void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<aos::controls::SensorGeneration>
+ sensor_generation_msg =
+ ::aos::controls::sensor_generation.MakeMessage();
+ sensor_generation_msg->reader_pid = 0;
+ sensor_generation_msg->cape_resets = 0;
+ sensor_generation_msg.Send();
+
::aos::ScopedMessagePtr<control_loops::ClawQueue::Position> position =
claw_queue_.position.MakeMessage();
position.Send();
@@ -51,7 +59,7 @@
class ClawTest : public ::testing::Test {
protected:
ClawTest() : claw_queue_(".frc971.control_loops.claw_queue",
- 0x78d8e372,
+ 0x9d7452fb,
".frc971.control_loops.claw_queue.goal",
".frc971.control_loops.claw_queue.position",
".frc971.control_loops.claw_queue.output",
@@ -66,6 +74,7 @@
virtual ~ClawTest() {
::aos::robot_state.Clear();
+ ::aos::controls::sensor_generation.Clear();
}
// Update the robot state. Without this, the Iteration of the control loop
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index cf09b6c..1bb4c10 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -1,5 +1,7 @@
#include "frc971/control_loops/claw/claw_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
@@ -7,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 1, 1> MakeClawPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00993695674898, 0.0, 0.987417901985;
+ A << 1.0, 0.00482455476758, 0.0, 0.930652495326;
Eigen::Matrix<double, 2, 1> B;
- B << 0.00319456032937, 0.637566599616;
+ B << 6.97110924671e-05, 0.0275544125308;
Eigen::Matrix<double, 1, 2> C;
C << 1, 0;
Eigen::Matrix<double, 1, 1> D;
@@ -23,24 +25,25 @@
StateFeedbackController<2, 1, 1> MakeClawController() {
Eigen::Matrix<double, 2, 1> L;
- L << 1.03584167586, 0.0410810558113;
- Eigen::Matrix<double, 2, 1> K;
- K << 128.210620632, 6.93828382074;
- return StateFeedbackController<2, 1, 1>(L, K, MakeClawPlantCoefficients());
+ L << 1.53065249533, 111.171516288;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 284.338418915, 17.4107932965;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.0, -0.00518405612386, 0.0, 1.07451492907;
+ return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeClawPlantCoefficients());
}
StateFeedbackPlant<2, 1, 1> MakeClawPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(4);
- plants[0] =
- new StateFeedbackPlantCoefficients<2, 1, 1>(MakeClawPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
+ ::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>(MakeClawPlantCoefficients()));
+ return StateFeedbackPlant<2, 1, 1>(&plants);
}
StateFeedbackLoop<2, 1, 1> MakeClawLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeClawController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
+ ::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>(MakeClawController()));
+ return StateFeedbackLoop<2, 1, 1>(&controllers);
}
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/claw_motor_plant.h b/frc971/control_loops/claw/claw_motor_plant.h
index cef7903..1502be3 100644
--- a/frc971/control_loops/claw/claw_motor_plant.h
+++ b/frc971/control_loops/claw/claw_motor_plant.h
@@ -6,11 +6,15 @@
namespace frc971 {
namespace control_loops {
+StateFeedbackPlantCoefficients<2, 1, 1> MakeClawPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeClawController();
+
StateFeedbackPlant<2, 1, 1> MakeClawPlant();
StateFeedbackLoop<2, 1, 1> MakeClawLoop();
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index c33562d..c10254b 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -75,8 +75,10 @@
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.1103668677, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
+ 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() {
@@ -84,15 +86,19 @@
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;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
+ 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.11442482191, 0.171361719101, 127.841025245, 6.90618982869;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
+ 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() {
@@ -100,25 +106,27 @@
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;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
+ 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<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<4, 2, 2>(plants);
+ ::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<StateFeedbackController<4, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController());
- return StateFeedbackLoop<4, 2, 2>(controllers);
+ ::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
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc
index 1287483..b463dde 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc
@@ -28,19 +28,21 @@
L << 0.604537580221;
Eigen::Matrix<double, 1, 1> K;
K << 0.0378646293422;
- return StateFeedbackController<1, 1, 1>(L, K, MakeCIMPlantCoefficients());
+ Eigen::Matrix<double, 1, 1> A_inv;
+ A_inv << 1.62723978514;
+ return StateFeedbackController<1, 1, 1>(L, K, A_inv, MakeCIMPlantCoefficients());
}
StateFeedbackPlant<1, 1, 1> MakeCIMPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients());
- return StateFeedbackPlant<1, 1, 1>(plants);
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>(new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients()));
+ return StateFeedbackPlant<1, 1, 1>(&plants);
}
StateFeedbackLoop<1, 1, 1> MakeCIMLoop() {
- ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<1, 1, 1>(MakeCIMController());
- return StateFeedbackLoop<1, 1, 1>(controllers);
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<1, 1, 1>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<1, 1, 1>>(new StateFeedbackController<1, 1, 1>(MakeCIMController()));
+ return StateFeedbackLoop<1, 1, 1>(&controllers);
}
} // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index b31cf85..1c908c6 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -76,7 +76,9 @@
L << 0.715841675858, 0.0410810558113, 0.0410810558113, 0.715841675858;
Eigen::Matrix<double, 2, 2> K;
K << 2.81809403994, 1.23253744933, 1.23253744933, 2.81809403994;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.36323698074, -0.0761076958907, -0.0761076958907, 1.36323698074;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
@@ -84,7 +86,9 @@
L << 0.715885457343, 0.0459077351335, 0.0459077351335, 0.894867244478;
Eigen::Matrix<double, 2, 2> K;
K << 2.81810038978, 1.23928174475, 2.31332592354, 10.6088017388;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.36167854796, -0.0196008159867, -0.0682979543713, 1.09303924439;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
@@ -92,7 +96,9 @@
L << 0.902328849033, 0.014581304798, 0.014581304798, 0.708423852788;
Eigen::Matrix<double, 2, 2> K;
K << 10.6088017388, 2.31332592354, 1.23928174475, 2.81810038978;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.09303924439, -0.0682979543713, -0.0196008159867, 1.36167854796;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
@@ -100,25 +106,27 @@
L << 0.895439806567, 0.0146814193986, 0.0146814193986, 0.895439806567;
Eigen::Matrix<double, 2, 2> K;
K << 10.6088022944, 2.31694961514, 2.31694961514, 10.6088022944;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.0926521463, -0.0175234726538, -0.0175234726538, 1.0926521463;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<2, 2, 2>(plants);
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>> plants(4);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients()));
+ plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients()));
+ plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients()));
+ plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients()));
+ return StateFeedbackPlant<2, 2, 2>(&plants);
}
StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
- ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
- return StateFeedbackLoop<2, 2, 2>(controllers);
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 2, 2>>> controllers(4);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController()));
+ controllers[1] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController()));
+ controllers[2] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController()));
+ controllers[3] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController()));
+ return StateFeedbackLoop<2, 2, 2>(&controllers);
}
} // namespace control_loops
diff --git a/frc971/control_loops/fridge/arm_motor_plant.cc b/frc971/control_loops/fridge/arm_motor_plant.cc
index 242aa57..ef09d98 100644
--- a/frc971/control_loops/fridge/arm_motor_plant.cc
+++ b/frc971/control_loops/fridge/arm_motor_plant.cc
@@ -1,46 +1,49 @@
#include "frc971/control_loops/fridge/arm_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeArmPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00993695674898, 0.0, 0.987417901985;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.00319456032937, 0.637566599616;
- 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<4, 2, 2> MakeArmPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00479642025454, 0.0, 0.0, 0.0, 0.919688585028, 0.0, 0.0, 0.0, 0.0, 0.986224520755, 0.00477375853056, 0.0, 0.0, -5.42143988176, 0.906292554417;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 2.46496779984e-05, 2.46496779984e-05, 0.00972420175808, 0.00972420175808, 2.45917395578e-05, -2.45917395578e-05, 0.0096782576655, -0.0096782576655;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 1, 0, 1, 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<2, 1, 1> MakeArmController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.03584167586, 0.0410810558113;
- Eigen::Matrix<double, 2, 1> K;
- K << 128.210620632, 6.93828382074;
- return StateFeedbackController<2, 1, 1>(L, K, MakeArmPlantCoefficients());
+StateFeedbackController<4, 2, 2> MakeArmController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 0.759844292514, 0.759844292514, 54.2541762188, 54.2541762188, 0.746258537586, -0.746258537586, 49.8002281115, -49.8002281115;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 320.979606093, 21.0129517956, 245.279302877, 31.6949206914, 320.979606094, 21.0129517956, -245.279302875, -31.6949206915;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00521526561559, 0.0, 0.0, 0.0, 1.08732457517, 0.0, 0.0, 0.0, 0.0, 0.985434166708, -0.00519062496618, 0.0, 0.0, 5.89486481623, 1.07234615805;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeArmPlantCoefficients());
}
-StateFeedbackPlant<2, 1, 1> MakeArmPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(4);
- plants[0] =
- new StateFeedbackPlantCoefficients<2, 1, 1>(MakeArmPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
+StateFeedbackPlant<4, 2, 2> MakeArmPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeArmPlantCoefficients()));
+ return StateFeedbackPlant<4, 2, 2>(&plants);
}
-StateFeedbackLoop<2, 1, 1> MakeArmLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeArmController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
+StateFeedbackLoop<4, 2, 2> MakeArmLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeArmController()));
+ return StateFeedbackLoop<4, 2, 2>(&controllers);
}
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/fridge/arm_motor_plant.h b/frc971/control_loops/fridge/arm_motor_plant.h
index 7deb5e6..4874ad5 100644
--- a/frc971/control_loops/fridge/arm_motor_plant.h
+++ b/frc971/control_loops/fridge/arm_motor_plant.h
@@ -6,11 +6,15 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlant<2, 1, 1> MakeArmPlant();
+StateFeedbackPlantCoefficients<4, 2, 2> MakeArmPlantCoefficients();
-StateFeedbackLoop<2, 1, 1> MakeArmLoop();
+StateFeedbackController<4, 2, 2> MakeArmController();
-} // namespace frc971
+StateFeedbackPlant<4, 2, 2> MakeArmPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeArmLoop();
+
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/fridge/elevator_motor_plant.cc b/frc971/control_loops/fridge/elevator_motor_plant.cc
index cabd106..93d00af 100644
--- a/frc971/control_loops/fridge/elevator_motor_plant.cc
+++ b/frc971/control_loops/fridge/elevator_motor_plant.cc
@@ -1,47 +1,49 @@
#include "frc971/control_loops/fridge/elevator_motor_plant.h"
+#include <vector>
+
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeElevatorPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00993695674898, 0.0, 0.987417901985;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.00319456032937, 0.637566599616;
- 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<4, 2, 2> MakeElevatorPlantCoefficients() {
+ Eigen::Matrix<double, 4, 4> A;
+ A << 1.0, 0.00431150605903, 0.0, 0.0, 0.0, 0.737863444837, 0.0, 0.0, 0.0, 0.0, 0.989566706498, 0.00429496790999, 0.0, 0.0, -3.96458576306, 0.728435659238;
+ Eigen::Matrix<double, 4, 2> B;
+ B << 3.79928442185e-05, 3.79928442185e-05, 0.0144653608576, 0.0144653608576, 3.79213511069e-05, -3.79213511069e-05, 0.0144098743777, -0.0144098743777;
+ Eigen::Matrix<double, 2, 4> C;
+ C << 1, 0, 1, 0, 1, 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<2, 1, 1> MakeElevatorController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.03584167586, 0.0410810558113;
- Eigen::Matrix<double, 2, 1> K;
- K << 128.210620632, 6.93828382074;
- return StateFeedbackController<2, 1, 1>(L, K, MakeElevatorPlantCoefficients());
+StateFeedbackController<4, 2, 2> MakeElevatorController() {
+ Eigen::Matrix<double, 4, 2> L;
+ L << 0.668931722418, 0.668931722418, 33.8393453814, 33.8393453814, 0.659001182868, -0.659001182868, 30.8170494953, -30.8170494953;
+ Eigen::Matrix<double, 2, 4> K;
+ K << 323.659330424, 11.4297509698, 518.471207663, 11.9234359025, 323.659330423, 11.4297509698, -518.471207664, -11.9234359025;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00584323032833, 0.0, 0.0, 0.0, 1.35526432024, 0.0, 0.0, 0.0, 0.0, 0.98722285856, -0.005820816765, 0.0, 0.0, 5.37306162923, 1.34112444982;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeElevatorPlantCoefficients());
}
-StateFeedbackPlant<2, 1, 1> MakeElevatorPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(
- MakeElevatorPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
+StateFeedbackPlant<4, 2, 2> MakeElevatorPlant() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeElevatorPlantCoefficients()));
+ return StateFeedbackPlant<4, 2, 2>(&plants);
}
-StateFeedbackLoop<2, 1, 1> MakeElevatorLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] =
- new StateFeedbackController<2, 1, 1>(MakeElevatorController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
+StateFeedbackLoop<4, 2, 2> MakeElevatorLoop() {
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeElevatorController()));
+ return StateFeedbackLoop<4, 2, 2>(&controllers);
}
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/fridge/elevator_motor_plant.h b/frc971/control_loops/fridge/elevator_motor_plant.h
index dd91fb1..7efd116 100644
--- a/frc971/control_loops/fridge/elevator_motor_plant.h
+++ b/frc971/control_loops/fridge/elevator_motor_plant.h
@@ -6,11 +6,15 @@
namespace frc971 {
namespace control_loops {
-StateFeedbackPlant<2, 1, 1> MakeElevatorPlant();
+StateFeedbackPlantCoefficients<4, 2, 2> MakeElevatorPlantCoefficients();
-StateFeedbackLoop<2, 1, 1> MakeElevatorLoop();
+StateFeedbackController<4, 2, 2> MakeElevatorController();
-} // namespace frc971
+StateFeedbackPlant<4, 2, 2> MakeElevatorPlant();
+
+StateFeedbackLoop<4, 2, 2> MakeElevatorLoop();
+
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 94e8271..a226915 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -11,10 +11,8 @@
Fridge::Fridge(control_loops::FridgeQueue *fridge)
: aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
- left_arm_loop_(new StateFeedbackLoop<2, 1, 1>(MakeArmLoop())),
- right_arm_loop_(new StateFeedbackLoop<2, 1, 1>(MakeArmLoop())),
- left_elev_loop_(new StateFeedbackLoop<2, 1, 1>(MakeArmLoop())),
- right_elev_loop_(new StateFeedbackLoop<2, 1, 1>(MakeArmLoop())) {}
+ arm_loop_(new StateFeedbackLoop<4, 2, 2>(MakeArmLoop())),
+ elev_loop_(new StateFeedbackLoop<4, 2, 2>(MakeElevatorLoop())) {}
void Fridge::RunIteration(
const control_loops::FridgeQueue::Goal * /*goal*/,
@@ -22,7 +20,7 @@
control_loops::FridgeQueue::Output * /*output*/,
control_loops::FridgeQueue::Status * /*status*/) {
- LOG(DEBUG, "Hi Brian!");
+ LOG(DEBUG, "Hi Brian!\n");
}
} // namespace control_loops
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index c73a85c..3d0fa6c 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -36,10 +36,8 @@
private:
// The state feedback control loop or loops to talk to.
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> left_arm_loop_;
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> right_arm_loop_;
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> left_elev_loop_;
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> right_elev_loop_;
+ ::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> arm_loop_;
+ ::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> elev_loop_;
};
} // namespace control_loops
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 2c1151f..1eb6185 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -3,6 +3,7 @@
#include <memory>
#include "gtest/gtest.h"
+#include "aos/common/controls/sensor_generation.q.h"
#include "aos/common/queue.h"
#include "aos/common/queue_testutils.h"
#include "frc971/control_loops/fridge/fridge.q.h"
@@ -21,37 +22,37 @@
public:
// Constructs a simulation.
FridgeSimulation()
- : left_arm_plant_(new StateFeedbackPlant<2, 1, 1>(MakeArmPlant())),
- right_arm_plant_(new StateFeedbackPlant<2, 1, 1>(MakeArmPlant())),
- left_elev_plant_(new StateFeedbackPlant<2, 1, 1>(MakeElevatorPlant())),
- right_elev_plant_(new StateFeedbackPlant<2, 1, 1>(MakeElevatorPlant())),
- fridge_queue_(".frc971.control_loops.fridge",
- 0x78d8e372, ".frc971.control_loops.fridge.goal",
- ".frc971.control_loops.fridge.position",
- ".frc971.control_loops.fridge.output",
- ".frc971.control_loops.fridge.status") {
- }
+ : arm_plant_(new StateFeedbackPlant<4, 2, 2>(MakeArmPlant())),
+ elev_plant_(new StateFeedbackPlant<4, 2, 2>(MakeElevatorPlant())),
+ fridge_queue_(".frc971.control_loops.fridge_queue", 0xe4e05855,
+ ".frc971.control_loops.fridge_queue.goal",
+ ".frc971.control_loops.fridge_queue.position",
+ ".frc971.control_loops.fridge_queue.output",
+ ".frc971.control_loops.fridge_queue.status") {}
// Sends a queue message with the position.
void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<aos::controls::SensorGeneration>
+ sensor_generation_msg =
+ ::aos::controls::sensor_generation.MakeMessage();
+ sensor_generation_msg->reader_pid = 0;
+ sensor_generation_msg->cape_resets = 0;
+ sensor_generation_msg.Send();
+
::aos::ScopedMessagePtr<control_loops::FridgeQueue::Position> position =
- fridge_queue_.position.MakeMessage();
+ fridge_queue_.position.MakeMessage();
position.Send();
}
// Simulates for a single timestep.
void Simulate() {
EXPECT_TRUE(fridge_queue_.output.FetchLatest());
- left_arm_plant_->Update();
- right_arm_plant_->Update();
- left_elev_plant_->Update();
- right_elev_plant_->Update();
+ arm_plant_->Update();
+ elev_plant_->Update();
}
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> left_arm_plant_;
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> right_arm_plant_;
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> left_elev_plant_;
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> right_elev_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> arm_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> elev_plant_;
private:
FridgeQueue fridge_queue_;
@@ -59,14 +60,14 @@
class FridgeTest : public ::testing::Test {
protected:
- FridgeTest() : fridge_queue_(".frc971.control_loops.fridge_queue",
- 0x78d8e372,
- ".frc971.control_loops.fridge_queue.goal",
- ".frc971.control_loops.fridge_queue.position",
- ".frc971.control_loops.fridge_queue.output",
- ".frc971.control_loops.fridge_queue.status"),
- fridge_(&fridge_queue_),
- fridge_plant_() {
+ FridgeTest()
+ : fridge_queue_(".frc971.control_loops.fridge_queue", 0xe4e05855,
+ ".frc971.control_loops.fridge_queue.goal",
+ ".frc971.control_loops.fridge_queue.position",
+ ".frc971.control_loops.fridge_queue.output",
+ ".frc971.control_loops.fridge_queue.status"),
+ fridge_(&fridge_queue_),
+ fridge_plant_() {
// Flush the robot state queue so we can use clean shared memory for this
// test.
::aos::robot_state.Clear();
@@ -75,6 +76,7 @@
virtual ~FridgeTest() {
::aos::robot_state.Clear();
+ ::aos::controls::sensor_generation.Clear();
}
// Update the robot state. Without this, the Iteration of the control loop
diff --git a/frc971/control_loops/python/arm.py b/frc971/control_loops/python/arm.py
new file mode 100755
index 0000000..c910f4c
--- /dev/null
+++ b/frc971/control_loops/python/arm.py
@@ -0,0 +1,249 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import polytope
+import polydrivetrain
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+
+class Arm(control_loop.ControlLoop):
+ def __init__(self, name="Arm", mass=None):
+ super(Arm, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.476
+ # Stall Current in Amps
+ self.stall_current = 80.730
+ # Free Speed in RPM
+ self.free_speed = 13906.0
+ # Free Current in Amps
+ self.free_current = 5.820
+ # Mass of the arm
+ if mass is None:
+ self.mass = 13.0
+ else:
+ self.mass = mass
+
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = (44.0 / 12.0) * (54.0 / 14.0) * (54.0 / 14.0) * (44.0 / 20.0) * (72.0 / 16.0)
+ # Fridge arm length
+ self.r = 32 * 0.0254
+ # Control loop time step
+ self.dt = 0.005
+
+ # Arm moment of inertia
+ self.J = self.r * self.mass
+
+ # Arm left/right spring constant (N*m / radian)
+ self.spring = 3000.0
+
+ # State is [average position, average velocity,
+ # position difference/2, velocity difference/2]
+ # Position difference is 1 - 2
+ # Input is [Voltage 1, Voltage 2]
+
+ C1 = self.spring / (self.J * 0.5)
+ C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
+ C3 = self.G * self.G * self.Kt / (self.R * self.J * 0.5 * self.Kv)
+
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, -C3, 0, 0],
+ [0, 0, 0, 1],
+ [0, 0, -C1 * 2.0, -C3]])
+
+ print 'Full speed is', C2 / C3 * 12.0
+
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [C2 / 2.0, C2 / 2.0],
+ [0, 0],
+ [C2 / 2.0, -C2 / 2.0]])
+
+ self.C = numpy.matrix([[1, 0, 1, 0],
+ [1, 0, -1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ controlability = controls.ctrb(self.A, self.B);
+ print 'Rank of augmented controlability matrix.', numpy.linalg.matrix_rank(
+ controlability)
+
+ q_pos = 0.02
+ q_vel = 0.300
+ q_pos_diff = 0.01
+ q_vel_diff = 0.15
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
+
+ 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 'Controller'
+ print self.K
+
+ print 'Controller Poles'
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = 0.20
+ self.ipl = 0.05
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+def CapU(U):
+ if U[0, 0] - U[1, 0] > 24:
+ return numpy.matrix([[12], [-12]])
+ elif U[0, 0] - U[1, 0] < -24:
+ return numpy.matrix([[-12], [12]])
+ else:
+ max_u = max(U[0, 0], U[1, 0])
+ min_u = min(U[0, 0], U[1, 0])
+ if max_u > 12:
+ return U - (max_u - 12)
+ if min_u < -12:
+ return U - (min_u + 12)
+ return U
+
+
+def run_test(arm, initial_X, goal, max_separation_error=0.01,
+ show_graph=True, iterations=200, controller_arm=None,
+ observer_arm=None):
+ """Runs the arm plant with an initial condition and goal.
+
+ The tests themselves are not terribly sophisticated; I just test for
+ whether the goal has been reached and whether the separation goes
+ outside of the initial and goal values by more than max_separation_error.
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ arm: arm object to use.
+ initial_X: starting state.
+ goal: goal state.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for.
+ controller_arm: arm object to get K from, or None if we should
+ use arm.
+ observer_arm: arm object to use for the observer, or None if we should
+ use the actual state.
+ """
+
+ arm.X = initial_X
+
+ if controller_arm is None:
+ controller_arm = arm
+
+ if observer_arm is not None:
+ observer_arm.X_hat = initial_X + 0.01
+ observer_arm.X_hat = initial_X
+
+ # Various lists for graphing things.
+ t = []
+ x_avg = []
+ x_sep = []
+ x_hat_avg = []
+ x_hat_sep = []
+ v_avg = []
+ v_sep = []
+ u_left = []
+ u_right = []
+
+ sep_plot_gain = 100.0
+
+ for i in xrange(iterations):
+ X_hat = arm.X
+ if observer_arm is not None:
+ X_hat = observer_arm.X_hat
+ x_hat_avg.append(observer_arm.X_hat[0, 0])
+ x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
+ U = controller_arm.K * (goal - X_hat)
+ U = CapU(U)
+ x_avg.append(arm.X[0, 0])
+ v_avg.append(arm.X[1, 0])
+ x_sep.append(arm.X[2, 0] * sep_plot_gain)
+ v_sep.append(arm.X[3, 0])
+ if observer_arm is not None:
+ observer_arm.PredictObserver(U)
+ arm.Update(U)
+ if observer_arm is not None:
+ observer_arm.Y = arm.Y
+ observer_arm.CorrectObserver(U)
+
+ t.append(i * arm.dt)
+ u_left.append(U[0, 0])
+ u_right.append(U[1, 0])
+
+ print numpy.linalg.inv(arm.A)
+ print "delta time is ", arm.dt
+ print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
+ print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+
+ if show_graph:
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x_avg, label='x avg')
+ pylab.plot(t, x_sep, label='x sep')
+ if observer_arm is not None:
+ pylab.plot(t, x_hat_avg, label='x_hat avg')
+ pylab.plot(t, x_hat_sep, label='x_hat sep')
+ pylab.legend()
+
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u_left, label='u left')
+ pylab.plot(t, u_right, label='u right')
+ pylab.legend()
+ pylab.show()
+
+
+def main(argv):
+ loaded_mass = 25
+ #loaded_mass = 0
+ arm = Arm(mass=13 + loaded_mass)
+ arm_controller = Arm(mass=13 + 15)
+ observer_arm = Arm(mass=13 + 15)
+ #observer_arm = None
+
+ # Test moving the arm with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
+ #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(arm, initial_X, R, controller_arm=arm_controller,
+ observer_arm=observer_arm)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name for the arm."
+ else:
+ arm = Arm("Arm")
+ loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
+ 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/claw.py b/frc971/control_loops/python/claw.py
index b5ea6a1..1a25f95 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -6,357 +6,99 @@
import polydrivetrain
import numpy
import sys
+import matplotlib
from matplotlib import pylab
class Claw(control_loop.ControlLoop):
- def __init__(self, name="RawClaw"):
+ def __init__(self, name="Claw", mass=None):
super(Claw, self).__init__(name)
# Stall Torque in N m
- self.stall_torque = 2.42
+ self.stall_torque = 0.476
# Stall Current in Amps
- self.stall_current = 133
+ self.stall_current = 80.730
# Free Speed in RPM
- self.free_speed = 5500.0
+ self.free_speed = 13906.0
# Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the claw in kg m^2
- self.J_top = 2.8
- self.J_bottom = 3.0
+ self.free_current = 5.820
+ # Mass of the claw
+ if mass is None:
+ self.mass = 5.0
+ else:
+ self.mass = mass
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (13.5 - self.R * self.free_current))
+ (12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
- self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
- # Control loop time step
- self.dt = 0.01
+ self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
+ # Claw length
+ self.r = 18 * 0.0254
- # State is [bottom position, bottom velocity, top - bottom position,
- # top - bottom velocity]
- # Input is [bottom power, top power - bottom power * J_top / J_bottom]
- # Motor time constants. difference_bottom refers to the constant for how the
- # bottom velocity affects the difference of the top and bottom velocities.
- self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
- self.bottom_bottom = self.common_motor_constant / self.J_bottom
- self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom
- - 1 / self.J_top)
- self.difference_difference = self.common_motor_constant / self.J_top
- # State feedback matrices
+ self.J = self.r * self.mass
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # State is [position, velocity]
+ # Input is [Voltage]
+
+ C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
+ C2 = self.Kt * self.G / (self.J * self.R)
self.A_continuous = numpy.matrix(
- [[0, 0, 1, 0],
- [0, 0, 0, 1],
- [0, 0, self.bottom_bottom, 0],
- [0, 0, self.difference_bottom, self.difference_difference]])
-
- self.A_bottom_cont = numpy.matrix(
[[0, 1],
- [0, self.bottom_bottom]])
+ [0, -C1]])
- self.A_diff_cont = numpy.matrix(
- [[0, 1],
- [0, self.difference_difference]])
-
- self.motor_feedforward = self.Kt / (self.G * self.R)
- self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
- self.motor_feedforward_difference = self.motor_feedforward / self.J_top
- self.motor_feedforward_difference_bottom = (
- self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
+ # Start with the unmodified input
self.B_continuous = numpy.matrix(
- [[0, 0],
- [0, 0],
- [self.motor_feedforward_bottom, 0],
- [-self.motor_feedforward_bottom, self.motor_feedforward_difference]])
-
- print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant
-
- self.B_bottom_cont = numpy.matrix(
[[0],
- [self.motor_feedforward_bottom]])
+ [C2]])
- self.B_diff_cont = numpy.matrix(
- [[0],
- [self.motor_feedforward_difference]])
-
- self.C = numpy.matrix([[1, 0, 0, 0],
- [1, 1, 0, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.A_bottom, self.B_bottom = controls.c2d(
- self.A_bottom_cont, self.B_bottom_cont, self.dt)
- self.A_diff, self.B_diff = controls.c2d(
- self.A_diff_cont, self.B_diff_cont, self.dt)
-
- self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.75 + 0.1j, .75 - 0.1j])
- self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.75 + 0.1j, .75 - 0.1j])
-
- print "K_diff", self.K_diff
- print "K_bottom", self.K_bottom
-
- print "A"
print self.A
- print "B"
- print self.B
-
-
- self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, 0.10, 0.0],
- [0.0, 0.0, 0.0, 0.1]])
-
- self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (5.0 ** 2.0))]])
- #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
-
- self.K = numpy.matrix([[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
- [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
-
- # Compute the feed forwards aceleration term.
- self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
-
- lstsq_A = numpy.identity(2)
- lstsq_A[0, :] = self.B[1, :]
- lstsq_A[1, :] = self.B[3, :]
- print "System of Equations coefficients:"
- print lstsq_A
- print "det", numpy.linalg.det(lstsq_A)
-
- out_x = numpy.linalg.lstsq(
- lstsq_A,
- numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
- self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
-
- print "K unaugmented"
- print self.K
- print "B * K unaugmented"
- print self.B * self.K
- F = self.A - self.B * self.K
- print "A - B * K unaugmented"
- print F
- print "eigenvalues"
- print numpy.linalg.eig(F)[0]
-
- self.rpl = .05
- self.ipl = 0.010
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
- self.rpl - 1j * self.ipl])
-
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
- # For the tests that check the limits, these are (upper, lower) for both
- # claws.
- self.hard_pos_limits = None
- self.pos_limits = None
-
- # Compute the steady state velocities for a given applied voltage.
- # The top and bottom of the claw should spin at the same rate if the
- # physics is right.
- X_ss = numpy.matrix([[0], [0], [0.0], [0]])
-
- U = numpy.matrix([[1.0],[1.0]])
- A = self.A
- B = self.B
- #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
- X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
- #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
- #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
- X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
-
- print "X_ss", X_ss
-
- self.InitializeState()
-
-
-class ClawDeltaU(Claw):
- def __init__(self, name="Claw"):
- super(ClawDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
- C_unaugmented = self.C
-
- self.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 1.0]])
- self.A[0:4, 0:4] = A_unaugmented
- self.A[0:4, 4] = B_unaugmented[0:4, 0]
-
- self.B = numpy.matrix([[0.0, 0.0],
- [0.0, 0.0],
- [0.0, 0.0],
- [0.0, 0.0],
- [1.0, 0.0]])
- self.B[0:4, 1] = B_unaugmented[0:4, 1]
-
- self.C = numpy.concatenate((C_unaugmented, numpy.matrix([[0.0], [0.0]])),
- axis=1)
- self.D = numpy.matrix([[0.0, 0.0],
- [0.0, 0.0]])
-
- #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
- self.Q = numpy.matrix([[(1.0 / (0.04 ** 2.0)), 0.0, 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.01 ** 2)), 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.01, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.08, 0.0],
- [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0 ** 2))]])
-
- self.R = numpy.matrix([[0.000001, 0.0],
- [0.0, 1.0 / (10.0 ** 2.0)]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
-
- self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
- [50.0, 0.0, 10.0, 0.0, 1.0]])
- #self.K = numpy.matrix([[50.0, -100.0, 0, -10, 0],
- # [50.0, 100.0, 0, 10, 0]])
controlability = controls.ctrb(self.A, self.B);
print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(controlability)
- print "K"
+ q_pos = 0.03
+ q_vel = 0.5
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
print self.K
- print "Placed controller poles are"
+
print numpy.linalg.eig(self.A - self.B * self.K)[0]
- print [numpy.abs(x) for x in numpy.linalg.eig(self.A - self.B * self.K)[0]]
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl, 0.10, 0.09,
- self.rpl - 1j * self.ipl, 0.90])
- #print "A is"
- #print self.A
- #print "L is"
- #print self.L
- #print "C is"
- #print self.C
- #print "A - LC is"
- #print self.A - self.L * self.C
+ self.rpl = 0.20
+ self.ipl = 0.05
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
- #print "Placed observer poles are"
- #print numpy.linalg.eig(self.A - self.L * self.C)[0]
-
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
self.InitializeState()
-def ScaleU(claw, U, K, error):
- """Clips U as necessary.
- Args:
- claw: claw object containing moments of inertia and U limits.
- U: Input matrix to clip as necessary.
- """
+def run_test(claw, initial_X, goal, max_separation_error=0.01,
+ show_graph=True, iterations=200, controller_claw=None,
+ observer_claw=None):
+ """Runs the claw plant with an initial condition and goal.
- bottom_u = U[0, 0]
- top_u = U[1, 0]
- position_error = error[0:2, 0]
- velocity_error = error[2:, 0]
-
- U_poly = polytope.HPolytope(
- numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]]),
- numpy.matrix([[12],
- [12],
- [12],
- [12]]))
-
- if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
- top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
-
- position_K = K[:, 0:2]
- velocity_K = K[:, 2:]
-
- # H * U <= k
- # U = UPos + UVel
- # H * (UPos + UVel) <= k
- # H * UPos <= k - H * UVel
- #
- # Now, we can do a coordinate transformation and say the following.
- #
- # UPos = position_K * position_error
- # (H * position_K) * position_error <= k - H * UVel
- #
- # Add in the constraint that 0 <= t <= 1
- # Now, there are 2 ways this can go. Either we have a region, or we don't
- # have a region. If we have a region, then pick the largest t and go for it.
- # If we don't have a region, we need to pick a good comprimise.
-
- pos_poly = polytope.HPolytope(
- U_poly.H * position_K,
- U_poly.k - U_poly.H * velocity_K * velocity_error)
-
- # The actual angle for the line we call 45.
- angle_45 = numpy.matrix([[numpy.sqrt(3), 1]])
- if claw.pos_limits and claw.hard_pos_limits and claw.X[0, 0] + claw.X[1, 0] > claw.pos_limits[1]:
- angle_45 = numpy.matrix([[1, 1]])
-
- P = position_error
- L45 = numpy.multiply(numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]]), angle_45)
- if L45[0, 1] == 0:
- L45[0, 1] = 1
- if L45[0, 0] == 0:
- L45[0, 0] = 1
- w45 = numpy.matrix([[0]])
-
- if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
- LH = numpy.matrix([[0, 1]])
- else:
- LH = numpy.matrix([[1, 0]])
- wh = LH * P
- standard = numpy.concatenate((L45, LH))
- W = numpy.concatenate((w45, wh))
- intersection = numpy.linalg.inv(standard) * W
- adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(pos_poly,
- LH, wh, position_error)
- adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(pos_poly,
- L45, w45, intersection)
- if pos_poly.IsInside(intersection):
- adjusted_pos_error = adjusted_pos_error_h
- else:
- if is_inside_h:
- if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(adjusted_pos_error_45):
- adjusted_pos_error = adjusted_pos_error_h
- else:
- adjusted_pos_error = adjusted_pos_error_45
- else:
- adjusted_pos_error = adjusted_pos_error_45
- #print adjusted_pos_error
-
- #print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
- return velocity_K * velocity_error + position_K * adjusted_pos_error
-
- #U = Kpos * poserror + Kvel * velerror
-
- #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
-
- #top_u *= scalar
- #bottom_u *= scalar
-
- return numpy.matrix([[bottom_u], [top_u]])
-
-def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=False, iterations=200):
- """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
-
- The tests themselves are not terribly sophisticated; I just test for
+ The tests themselves are not terribly sophisticated; I just test for
whether the goal has been reached and whether the separation goes
outside of the initial and goal values by more than max_separation_error.
Prints out something for a failure of either condition and returns
@@ -367,112 +109,76 @@
goal: goal state.
show_graph: Whether or not to display a graph showing the changing
states and voltages.
- iterations: Number of timesteps to run the model for."""
+ iterations: Number of timesteps to run the model for.
+ controller_claw: claw object to get K from, or None if we should
+ use claw.
+ observer_claw: claw object to use for the observer, or None if we should
+ use the actual state.
+ """
claw.X = initial_X
+ if controller_claw is None:
+ controller_claw = claw
+
+ if observer_claw is not None:
+ observer_claw.X_hat = initial_X + 0.01
+ observer_claw.X_hat = initial_X
+
# Various lists for graphing things.
t = []
- x_bottom = []
- x_top = []
- u_bottom = []
- u_top = []
- x_separation = []
+ x = []
+ v = []
+ x_hat = []
+ u = []
- tests_passed = True
-
- # Bounds which separation should not exceed.
- lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0]
- else goal[1, 0]) - max_separation_error
- upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0]
- else goal[1, 0]) + max_separation_error
+ sep_plot_gain = 100.0
for i in xrange(iterations):
- U = claw.K * (goal - claw.X)
- U = ScaleU(claw, U, claw.K, goal - claw.X)
+ X_hat = claw.X
+ if observer_claw is not None:
+ X_hat = observer_claw.X_hat
+ x_hat.append(observer_claw.X_hat[0, 0])
+ U = controller_claw.K * (goal - X_hat)
+ U[0, 0] = numpy.clip(U[0, 0], -12, 12)
+ x.append(claw.X[0, 0])
+ v.append(claw.X[1, 0])
+ if observer_claw is not None:
+ observer_claw.PredictObserver(U)
claw.Update(U)
-
- if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
- tests_passed = False
- print "Claw separation was", claw.X[1, 0]
- print "Should have been between", lower_bound, "and", upper_bound
-
- if claw.hard_pos_limits and \
- (claw.X[0, 0] > claw.hard_pos_limits[1] or
- claw.X[0, 0] < claw.hard_pos_limits[0] or
- claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
- claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
- tests_passed = False
- print "Claws at %f and %f" % (claw.X[0, 0], claw.X[0, 0] + claw.X[1, 0])
- print "Both should be in %s, definitely %s" % \
- (claw.pos_limits, claw.hard_pos_limits)
+ if observer_claw is not None:
+ observer_claw.Y = claw.Y
+ observer_claw.CorrectObserver(U)
t.append(i * claw.dt)
- x_bottom.append(claw.X[0, 0] * 10.0)
- x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
- u_bottom.append(U[0, 0])
- u_top.append(U[1, 0])
- x_separation.append(claw.X[1, 0] * 10.0)
+ u.append(U[0, 0])
if show_graph:
- pylab.plot(t, x_bottom, label='x bottom * 10')
- pylab.plot(t, x_top, label='x top * 10')
- pylab.plot(t, u_bottom, label='u bottom')
- pylab.plot(t, u_top, label='u top')
- pylab.plot(t, x_separation, label='separation * 10')
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x, label='x')
+ if observer_claw is not None:
+ pylab.plot(t, x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u, label='u')
pylab.legend()
pylab.show()
- # Test to make sure that we are near the goal.
- if numpy.max(abs(claw.X - goal)) > 1e-4:
- tests_passed = False
- print "X was", claw.X, "Expected", goal
-
- return tests_passed
def main(argv):
- claw = Claw()
+ loaded_mass = 5
+ #loaded_mass = 0
+ claw = Claw(mass=5 + loaded_mass)
+ claw_controller = Claw(mass=5 + 5)
+ observer_claw = Claw(mass=5 + 5)
+ #observer_claw = None
# Test moving the claw with constant separation.
- initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test just changing separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test changing both separation and position at once.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test opening with the top claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-1.6, 0.1)
- claw.pos_limits = (-1.5, 0.0)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
-
- # Test opening with the bottom claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-0.1, 1.6)
- claw.pos_limits = (0.0, 1.6)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0]])
+ run_test(claw, initial_X, R, controller_claw=claw_controller,
+ observer_claw=observer_claw)
# Write the generated constants out to a file.
if len(argv) != 3:
@@ -480,8 +186,6 @@
else:
claw = Claw("Claw")
loop_writer = control_loop.ControlLoopWriter("Claw", [claw])
- loop_writer.AddConstant(control_loop.Constant("kClawMomentOfInertiaRatio",
- "%f", claw.J_top / claw.J_bottom))
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index c3dd23e..ba4a43c 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -2,16 +2,16 @@
import numpy
class Constant(object):
- def __init__ (self, name, formatt, value):
- self.name = name
- self.formatt = formatt
- self.value = value
- self.formatToType = {}
- self.formatToType['%f'] = "double";
- self.formatToType['%d'] = "int";
- def __str__ (self):
- return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
- (self.formatToType[self.formatt], self.name, self.value)
+ def __init__ (self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double";
+ self.formatToType['%d'] = "int";
+ def __str__ (self):
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (self.formatToType[self.formatt], self.name, self.value)
class ControlLoopWriter(object):
@@ -140,24 +140,24 @@
fd.write('%s Make%sPlant() {\n' %
(self._PlantType(), self._gain_schedule_name))
- fd.write(' ::std::vector<%s *> plants(%d);\n' % (
+ fd.write(' ::std::vector< ::std::unique_ptr<%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(),
+ fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._CoeffType(), self._CoeffType(),
loop.PlantFunction()))
- fd.write(' return %s(plants);\n' % self._PlantType())
+ 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' % (
+ fd.write(' ::std::vector< ::std::unique_ptr<%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(),
+ fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._ControllerType(), self._ControllerType(),
loop.ControllerFunction()))
- fd.write(' return %s(controllers);\n' % self._LoopType())
+ fd.write(' return %s(&controllers);\n' % self._LoopType())
fd.write('}\n\n')
fd.write(self._namespace_end)
@@ -216,6 +216,15 @@
self.X = self.A * self.X + self.B * U
self.Y = self.C * self.X + self.D * U
+ def PredictObserver(self, U):
+ """Runs the predict step of the observer update."""
+ self.X_hat = (self.A * self.X_hat + self.B * U)
+
+ def CorrectObserver(self, U):
+ """Runs the correct step of the observer update."""
+ self.X_hat += numpy.linalg.inv(self.A) * self.L * (
+ self.Y - self.C * self.X_hat - self.D * U)
+
def UpdateObserver(self, U):
"""Updates the observer given the provided U."""
self.X_hat = (self.A * self.X_hat + self.B * U +
@@ -320,9 +329,10 @@
ans.append(self._DumpMatrix('L', self.L))
ans.append(self._DumpMatrix('K', self.K))
+ ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
ans.append(' return StateFeedbackController<%d, %d, %d>'
- '(L, K, Make%sPlantCoefficients());\n' % (num_states, num_inputs,
- num_outputs, self._name))
+ '(L, K, A_inv, Make%sPlantCoefficients());\n' % (
+ num_states, num_inputs, num_outputs, self._name))
ans.append('}\n')
return ''.join(ans)
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 64bd52d..57268a9 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -89,8 +89,8 @@
integral(e^(A t) * B, 0, dt).
Returns (A, B). C and D are unchanged."""
e, P = numpy.linalg.eig(A)
- diag = numpy.matrix(numpy.eye(A.shape[0]))
- diage = numpy.matrix(numpy.eye(A.shape[0]))
+ diag = numpy.matrix(numpy.eye(A.shape[0]), dtype=numpy.complex128)
+ diage = numpy.matrix(numpy.eye(A.shape[0]), dtype=numpy.complex128)
for eig, count in zip(e, range(0, A.shape[0])):
diag[count, count] = numpy.exp(eig * dt)
if abs(eig) < 1.0e-16:
@@ -98,7 +98,13 @@
else:
diage[count, count] = (numpy.exp(eig * dt) - 1.0) / eig
- return (P * diag * numpy.linalg.inv(P), P * diage * numpy.linalg.inv(P) * B)
+ ans_a = P * diag * numpy.linalg.inv(P)
+ ans_b = P * diage * numpy.linalg.inv(P) * B
+ if numpy.abs(ans_a.imag).sum() / numpy.abs(ans_a).sum() < 1e-6:
+ ans_a = ans_a.real
+ if numpy.abs(ans_b.imag).sum() / numpy.abs(ans_b).sum() < 1e-6:
+ ans_b = ans_b.real
+ return (ans_a, ans_b)
def ctrb(A, B):
"""Returns the controlability matrix.
@@ -123,7 +129,8 @@
# P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
- P, rcond, w, S, T = slycot.sb02od(A.shape[0], B.shape[1], A, B, Q, R, 'D')
+ P, rcond, w, S, T = slycot.sb02od(
+ n=A.shape[0], m=B.shape[1], A=A, B=B, Q=Q, R=R, dico='D')
F = numpy.linalg.inv(R + B.T * P *B) * B.T * P * A
return F
diff --git a/frc971/control_loops/python/elevator.py b/frc971/control_loops/python/elevator.py
new file mode 100755
index 0000000..2b42945
--- /dev/null
+++ b/frc971/control_loops/python/elevator.py
@@ -0,0 +1,246 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import polytope
+import polydrivetrain
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+
+class Elevator(control_loop.ControlLoop):
+ def __init__(self, name="Elevator", mass=None):
+ super(Elevator, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.476
+ # Stall Current in Amps
+ self.stall_current = 80.730
+ # Free Speed in RPM
+ self.free_speed = 13906.0
+ # Free Current in Amps
+ self.free_current = 5.820
+ # Mass of the elevator
+ if mass is None:
+ self.mass = 13.0
+ else:
+ self.mass = mass
+
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = (56.0 / 12.0) * (84.0 / 14.0)
+ # Pulley diameter
+ self.r = 32 * 0.005 / numpy.pi / 2.0
+ # Control loop time step
+ self.dt = 0.005
+
+ # Elevator left/right spring constant (N/m)
+ self.spring = 3000.0
+
+ # State is [average position, average velocity,
+ # position difference/2, velocity difference/2]
+ # Input is [V1, V2]
+
+ C1 = self.spring / (self.mass * 0.5)
+ C2 = self.Kt * self.G / (self.mass * 0.5 * self.r * self.R)
+ C3 = self.G * self.G * self.Kt / (
+ self.R * self.r * self.r * self.mass * 0.5 * self.Kv)
+
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, -C3, 0, 0],
+ [0, 0, 0, 1],
+ [0, 0, -C1 * 2.0, -C3]])
+
+ print "Full speed is", C2 / C3 * 12.0
+
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [C2 / 2.0, C2 / 2.0],
+ [0, 0],
+ [C2 / 2.0, -C2 / 2.0]])
+
+ self.C = numpy.matrix([[1, 0, 1, 0],
+ [1, 0, -1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ print self.A
+
+ controlability = controls.ctrb(self.A, self.B);
+ print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(
+ controlability)
+
+ q_pos = 0.02
+ q_vel = 0.400
+ q_pos_diff = 0.01
+ q_vel_diff = 0.45
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
+
+ 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.K
+
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = 0.20
+ self.ipl = 0.05
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+def CapU(U):
+ if U[0, 0] - U[1, 0] > 24:
+ return numpy.matrix([[12], [-12]])
+ elif U[0, 0] - U[1, 0] < -24:
+ return numpy.matrix([[-12], [12]])
+ else:
+ max_u = max(U[0, 0], U[1, 0])
+ min_u = min(U[0, 0], U[1, 0])
+ if max_u > 12:
+ return U - (max_u - 12)
+ if min_u < -12:
+ return U - (min_u + 12)
+ return U
+
+
+def run_test(elevator, initial_X, goal, max_separation_error=0.01,
+ show_graph=True, iterations=200, controller_elevator=None,
+ observer_elevator=None):
+ """Runs the elevator plant with an initial condition and goal.
+
+ The tests themselves are not terribly sophisticated; I just test for
+ whether the goal has been reached and whether the separation goes
+ outside of the initial and goal values by more than max_separation_error.
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ elevator: elevator object to use.
+ initial_X: starting state.
+ goal: goal state.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for.
+ controller_elevator: elevator object to get K from, or None if we should
+ use elevator.
+ observer_elevator: elevator object to use for the observer, or None if we
+ should use the actual state.
+ """
+
+ elevator.X = initial_X
+
+ if controller_elevator is None:
+ controller_elevator = elevator
+
+ if observer_elevator is not None:
+ observer_elevator.X_hat = initial_X + 0.01
+ observer_elevator.X_hat = initial_X
+
+ # Various lists for graphing things.
+ t = []
+ x_avg = []
+ x_sep = []
+ x_hat_avg = []
+ x_hat_sep = []
+ v_avg = []
+ v_sep = []
+ u_left = []
+ u_right = []
+
+ sep_plot_gain = 100.0
+
+ for i in xrange(iterations):
+ X_hat = elevator.X
+ if observer_elevator is not None:
+ X_hat = observer_elevator.X_hat
+ x_hat_avg.append(observer_elevator.X_hat[0, 0])
+ x_hat_sep.append(observer_elevator.X_hat[2, 0] * sep_plot_gain)
+ U = controller_elevator.K * (goal - X_hat)
+ U = CapU(U)
+ x_avg.append(elevator.X[0, 0])
+ v_avg.append(elevator.X[1, 0])
+ x_sep.append(elevator.X[2, 0] * sep_plot_gain)
+ v_sep.append(elevator.X[3, 0])
+ if observer_elevator is not None:
+ observer_elevator.PredictObserver(U)
+ elevator.Update(U)
+ if observer_elevator is not None:
+ observer_elevator.Y = elevator.Y
+ observer_elevator.CorrectObserver(U)
+
+ t.append(i * elevator.dt)
+ u_left.append(U[0, 0])
+ u_right.append(U[1, 0])
+
+ print numpy.linalg.inv(elevator.A)
+ print "delta time is ", elevator.dt
+ print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
+ print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+
+ if show_graph:
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x_avg, label='x avg')
+ pylab.plot(t, x_sep, label='x sep')
+ if observer_elevator is not None:
+ pylab.plot(t, x_hat_avg, label='x_hat avg')
+ pylab.plot(t, x_hat_sep, label='x_hat sep')
+ pylab.legend()
+
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u_left, label='u left')
+ pylab.plot(t, u_right, label='u right')
+ pylab.legend()
+ pylab.show()
+
+
+def main(argv):
+ loaded_mass = 25
+ #loaded_mass = 0
+ elevator = Elevator(mass=13 + loaded_mass)
+ elevator_controller = Elevator(mass=13 + 15)
+ observer_elevator = Elevator(mass=13 + 15)
+ #observer_elevator = None
+
+ # Test moving the elevator with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
+ #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(elevator, initial_X, R, controller_elevator=elevator_controller,
+ observer_elevator=observer_elevator)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name for the elevator."
+ else:
+ elevator = Elevator("Elevator")
+ loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
+ 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/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 8dcce5a..465da33 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -4,6 +4,7 @@
#include <assert.h>
#include <vector>
+#include <memory>
#include <iostream>
#include "Eigen/Dense"
@@ -90,11 +91,10 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
StateFeedbackPlant(
- const ::std::vector<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs,
- number_of_outputs> *> &coefficients)
- : coefficients_(coefficients),
- plant_index_(0) {
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>> *
+ coefficients)
+ : coefficients_(::std::move(*coefficients)), plant_index_(0) {
Reset();
}
@@ -106,11 +106,7 @@
U_.swap(other.U_);
}
- virtual ~StateFeedbackPlant() {
- for (auto c : coefficients_) {
- delete c;
- }
- }
+ virtual ~StateFeedbackPlant() {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
return coefficients().A();
@@ -202,8 +198,8 @@
Eigen::Matrix<double, number_of_outputs, 1> Y_;
Eigen::Matrix<double, number_of_inputs, 1> U_;
- ::std::vector<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs> *> coefficients_;
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>> coefficients_;
int plant_index_;
@@ -219,16 +215,19 @@
const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
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_inputs, number_of_states> &K,
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &plant)
: L(L),
K(K),
+ A_inv(A_inv),
plant(plant) {
}
};
@@ -241,7 +240,7 @@
StateFeedbackLoop(const StateFeedbackController<
number_of_states, number_of_inputs, number_of_outputs> &controller)
: controller_index_(0) {
- controllers_.push_back(new StateFeedbackController<
+ controllers_.emplace_back(new StateFeedbackController<
number_of_states, number_of_inputs, number_of_outputs>(controller));
Reset();
}
@@ -249,19 +248,20 @@
StateFeedbackLoop(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &plant)
: controller_index_(0) {
- controllers_.push_back(
+ controllers_.emplace_back(
new StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs>(L, K, plant));
+ number_of_outputs>(L, K, A_inv, plant));
Reset();
}
- StateFeedbackLoop(const ::std::vector<StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs> *> &controllers)
- : controllers_(controllers), controller_index_(0) {
+ StateFeedbackLoop(::std::vector< ::std::unique_ptr<StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+ : controllers_(::std::move(*controllers)), controller_index_(0) {
Reset();
}
@@ -276,11 +276,7 @@
controller_index_ = other.controller_index_;
}
- virtual ~StateFeedbackLoop() {
- for (auto c : controllers_) {
- delete c;
- }
- }
+ virtual ~StateFeedbackLoop() {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
return controller().plant.A();
@@ -435,8 +431,8 @@
int controller_index() const { return controller_index_; }
protected:
- ::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> *> controllers_;
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs>>> controllers_;
// These are accessible from non-templated subclasses.
static const int kNumStates = number_of_states;
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index 33f17b2..f355b6d 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -19,8 +19,9 @@
Eigen::Matrix<double, 4, 1>::Constant(-1));
{
- StateFeedbackPlant<2, 4, 7> plant(
- {new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients)});
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>> v;
+ v.emplace_back(new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
+ StateFeedbackPlant<2, 4, 7> plant(&v);
plant.Update();
plant.Reset();
plant.CheckU();
@@ -28,7 +29,8 @@
{
StateFeedbackLoop<2, 4, 7> test_loop(StateFeedbackController<2, 4, 7>(
Eigen::Matrix<double, 2, 7>::Identity(),
- Eigen::Matrix<double, 4, 2>::Identity(), coefficients));
+ Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 2, 2>::Identity(), coefficients));
test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
test_loop.Update(false);
test_loop.CapU();
@@ -36,7 +38,8 @@
{
StateFeedbackLoop<2, 4, 7> test_loop(
Eigen::Matrix<double, 2, 7>::Identity(),
- Eigen::Matrix<double, 4, 2>::Identity(), coefficients);
+ Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 2, 2>::Identity(), coefficients);
test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
test_loop.Update(false);
test_loop.CapU();
diff --git a/frc971/control_loops/update_arm.sh b/frc971/control_loops/update_arm.sh
new file mode 100755
index 0000000..40ef68a
--- /dev/null
+++ b/frc971/control_loops/update_arm.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+#
+# Updates the arm controllers (for both robots).
+
+cd $(dirname $0)
+
+./python/arm.py fridge/arm_motor_plant.cc \
+ fridge/arm_motor_plant.h
diff --git a/frc971/control_loops/update_claw.sh b/frc971/control_loops/update_claw.sh
new file mode 100755
index 0000000..3ef6921
--- /dev/null
+++ b/frc971/control_loops/update_claw.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+#
+# Updates the claw controllers (for both robots).
+
+cd $(dirname $0)
+
+./python/claw.py claw/claw_motor_plant.cc \
+ claw/claw_motor_plant.h
diff --git a/frc971/control_loops/update_elevator.sh b/frc971/control_loops/update_elevator.sh
new file mode 100755
index 0000000..25ea27e
--- /dev/null
+++ b/frc971/control_loops/update_elevator.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+#
+# Updates the elevator controllers (for both robots).
+
+cd $(dirname $0)
+
+./python/elevator.py fridge/elevator_motor_plant.cc \
+ fridge/elevator_motor_plant.h