Added control loops for all subsystems and made tests run.
Change-Id: I66542db4355a89f6d24c1ad4772004182197c863
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