Integration: Updating queues to output current position. Added structure for fridge and claw files.
Change-Id: I216fb5431aaadbf0be3838031d4858401330da94
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
new file mode 100644
index 0000000..6177687
--- /dev/null
+++ b/frc971/control_loops/claw/claw.cc
@@ -0,0 +1,25 @@
+#include "frc971/control_loops/claw/claw.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/claw/claw_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+Claw::Claw(control_loops::ClawQueue *claw)
+ : aos::controls::ControlLoop<control_loops::ClawQueue>(claw),
+ claw_loop_(new StateFeedbackLoop<2, 1, 1>(MakeClawLoop())) {}
+
+void Claw::RunIteration(
+ const control_loops::ClawQueue::Goal * /*goal*/,
+ const control_loops::ClawQueue::Position * /*position*/,
+ control_loops::ClawQueue::Output * /*output*/,
+ control_loops::ClawQueue::Status * /*status*/) {
+
+ LOG(DEBUG, "Hi Brian!");
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 4c65994..78f9c18 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -1,7 +1,7 @@
{
'targets': [
{
- 'target_name': 'claw_queues',
+ 'target_name': 'claw_queue',
'type': 'static_library',
'sources': ['claw.q'],
'variables': {
@@ -18,5 +18,48 @@
],
'includes': ['../../../aos/build/queues.gypi'],
},
+ {
+ 'target_name': 'claw_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'claw.cc',
+ 'claw_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ 'claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'claw_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'claw_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'claw',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'claw_lib',
+ ],
+ },
],
}
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
new file mode 100644
index 0000000..3d7e647
--- /dev/null
+++ b/frc971/control_loops/claw/claw.h
@@ -0,0 +1,45 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_H_
+#define FRC971_CONTROL_LOOPS_CLAW_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/claw/claw_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class Claw
+ : public aos::controls::ControlLoop<control_loops::ClawQueue> {
+ public:
+ explicit Claw(
+ control_loops::ClawQueue *claw_queue = &control_loops::claw_queue);
+
+ // Control loop time step.
+ // Please figure out how to set dt from a common location
+ // Please decide the correct value
+ // Please use dt in your implementation so we can change looptimnig
+ // and be consistent with legacy
+ // And Brian please approve my code review as people are wait on
+ // these files to exist and they will be rewritten anyway
+ //static constexpr double dt;
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::ClawQueue::Goal *goal,
+ const control_loops::ClawQueue::Position *position,
+ control_loops::ClawQueue::Output *output,
+ control_loops::ClawQueue::Status *status);
+
+ private:
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> claw_loop_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_H_
+
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 8f5f9c0..1ca5746 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -3,7 +3,7 @@
import "aos/common/controls/control_loops.q";
import "frc971/control_loops/control_loops.q";
-queue_group Claw {
+queue_group ClawQueue {
implements aos.control_loops.ControlLoop;
// All angles are in radians with 0 sticking straight out the front. Rotating
@@ -42,6 +42,12 @@
// Has claw zeroed and reached goal?
bool done;
+ // Angle of wrist joint.
+ double angle;
+ // Voltage of intake rollers. Positive means sucking in, negative means
+ // spitting out.
+ double intake;
+
// True iff there has been enough time since we actuated the rollers outward
// that they should be there.
bool rollers_open;
@@ -56,4 +62,5 @@
queue Status status;
};
-queue_group Claw claw;
+queue_group ClawQueue claw_queue;
+
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
new file mode 100644
index 0000000..83a4a50
--- /dev/null
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -0,0 +1,115 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/control_loops/claw/claw.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the claw and sends out queue messages with the
+// position.
+class ClawSimulation {
+ public:
+ // Constructs a claw simulation.
+ ClawSimulation()
+ : claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeClawPlant())),
+ claw_queue_(".frc971.control_loops.claw_queue",
+ 0x78d8e372, ".frc971.control_loops.claw_queue.goal",
+ ".frc971.control_loops.claw_queue.position",
+ ".frc971.control_loops.claw_queue.output",
+ ".frc971.control_loops.claw_queue.status") {
+ }
+
+ // Sends a queue message with the position.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ClawQueue::Position> position =
+ claw_queue_.position.MakeMessage();
+ position.Send();
+ }
+
+ // Simulates for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(claw_queue_.output.FetchLatest());
+ claw_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> claw_plant_;
+
+ private:
+ ClawQueue claw_queue_;
+};
+
+class ClawTest : public ::testing::Test {
+ protected:
+ ClawTest() : claw_queue_(".frc971.control_loops.claw_queue",
+ 0x78d8e372,
+ ".frc971.control_loops.claw_queue.goal",
+ ".frc971.control_loops.claw_queue.position",
+ ".frc971.control_loops.claw_queue.output",
+ ".frc971.control_loops.claw_queue.status"),
+ claw_(&claw_queue_),
+ claw_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ virtual ~ClawTest() {
+ ::aos::robot_state.Clear();
+ }
+
+ // Update the robot state. Without this, the Iteration of the control loop
+ // will stop all the motors and this won't go anywhere.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ claw_queue_.goal.FetchLatest();
+ claw_queue_.status.FetchLatest();
+ EXPECT_NEAR(claw_queue_.goal->angle,
+ claw_queue_.status->angle,
+ 10.0);
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to
+ // shared memory that is no longer valid.
+ ClawQueue claw_queue_;
+
+ // Create a control loop and simulation.
+ Claw claw_;
+ ClawSimulation claw_plant_;
+};
+
+// Tests that the loop does nothing when the goal is zero.
+TEST_F(ClawTest, DoesNothing) {
+ claw_queue_.goal.MakeWithBuilder().angle(0.0).Send();
+ SendDSPacket(true);
+ claw_plant_.SendPositionMessage();
+ claw_.Iterate();
+ claw_plant_.Simulate();
+ VerifyNearGoal();
+ claw_queue_.output.FetchLatest();
+ EXPECT_EQ(claw_queue_.output->voltage, 0.0);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/claw/claw_main.cc b/frc971/control_loops/claw/claw_main.cc
new file mode 100644
index 0000000..bf25a03
--- /dev/null
+++ b/frc971/control_loops/claw/claw_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/claw/claw.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::Claw claw;
+ claw.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
new file mode 100644
index 0000000..cf09b6c
--- /dev/null
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -0,0 +1,46 @@
+#include "frc971/control_loops/claw/claw_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeClawPlantCoefficients() {
+ 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);
+}
+
+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());
+}
+
+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);
+}
+
+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);
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/claw/claw_motor_plant.h b/frc971/control_loops/claw/claw_motor_plant.h
new file mode 100644
index 0000000..cef7903
--- /dev/null
+++ b/frc971/control_loops/claw/claw_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeClawPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeClawLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_CLAW_CLAW_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/fridge/arm_motor_plant.cc b/frc971/control_loops/fridge/arm_motor_plant.cc
new file mode 100644
index 0000000..242aa57
--- /dev/null
+++ b/frc971/control_loops/fridge/arm_motor_plant.cc
@@ -0,0 +1,46 @@
+#include "frc971/control_loops/fridge/arm_motor_plant.h"
+
+#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);
+}
+
+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());
+}
+
+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);
+}
+
+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);
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/fridge/arm_motor_plant.h b/frc971/control_loops/fridge/arm_motor_plant.h
new file mode 100644
index 0000000..7deb5e6
--- /dev/null
+++ b/frc971/control_loops/fridge/arm_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_FRIDGE_ARM_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeArmPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeArmLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#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
new file mode 100644
index 0000000..cabd106
--- /dev/null
+++ b/frc971/control_loops/fridge/elevator_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/fridge/elevator_motor_plant.h"
+
+#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);
+}
+
+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());
+}
+
+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);
+}
+
+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);
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/fridge/elevator_motor_plant.h b/frc971/control_loops/fridge/elevator_motor_plant.h
new file mode 100644
index 0000000..dd91fb1
--- /dev/null
+++ b/frc971/control_loops/fridge/elevator_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_FRIDGE_ELEVATOR_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeElevatorPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeElevatorLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#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
new file mode 100644
index 0000000..94e8271
--- /dev/null
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -0,0 +1,29 @@
+#include "frc971/control_loops/fridge/fridge.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/fridge/elevator_motor_plant.h"
+#include "frc971/control_loops/fridge/arm_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+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())) {}
+
+void Fridge::RunIteration(
+ const control_loops::FridgeQueue::Goal * /*goal*/,
+ const control_loops::FridgeQueue::Position * /*position*/,
+ control_loops::FridgeQueue::Output * /*output*/,
+ control_loops::FridgeQueue::Status * /*status*/) {
+
+ LOG(DEBUG, "Hi Brian!");
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/fridge/fridge.gyp b/frc971/control_loops/fridge/fridge.gyp
index 093be4d..6edee2b 100644
--- a/frc971/control_loops/fridge/fridge.gyp
+++ b/frc971/control_loops/fridge/fridge.gyp
@@ -1,7 +1,7 @@
{
'targets': [
{
- 'target_name': 'fridge_queues',
+ 'target_name': 'fridge_queue',
'type': 'static_library',
'sources': ['fridge.q'],
'variables': {
@@ -18,5 +18,49 @@
],
'includes': ['../../../aos/build/queues.gypi'],
},
+ {
+ 'target_name': 'fridge_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'fridge.cc',
+ 'arm_motor_plant.cc',
+ 'elevator_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'fridge_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ 'fridge_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'fridge_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'fridge_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'fridge_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'fridge',
+ 'type': 'executable',
+ 'sources': [
+ 'fridge_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'fridge_lib',
+ ],
+ },
],
}
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
new file mode 100644
index 0000000..c73a85c
--- /dev/null
+++ b/frc971/control_loops/fridge/fridge.h
@@ -0,0 +1,49 @@
+#ifndef FRC971_CONTROL_LOOPS_FRIDGE_H_
+#define FRC971_CONTROL_LOOPS_FRIDGE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/fridge/fridge.q.h"
+#include "frc971/control_loops/fridge/arm_motor_plant.h"
+#include "frc971/control_loops/fridge/elevator_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class Fridge
+ : public aos::controls::ControlLoop<control_loops::FridgeQueue> {
+ public:
+ explicit Fridge(
+ control_loops::FridgeQueue *fridge_queue = &control_loops::fridge_queue);
+
+ // Control loop time step.
+ // Please figure out how to set dt from a common location
+ // Please decide the correct value
+ // Please use dt in your implementation so we can change looptimnig
+ // and be consistent with legacy
+ // And Brian please approve my code review as people are wait on
+ // these files to exist and they will be rewritten anyway
+ //static constexpr double dt;
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::FridgeQueue::Goal *goal,
+ const control_loops::FridgeQueue::Position *position,
+ control_loops::FridgeQueue::Output *output,
+ control_loops::FridgeQueue::Status *status);
+
+ 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_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_FRIDGE_H_
+
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index 6e09c0b..c85efa5 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -12,7 +12,7 @@
bool bottom_back;
};
-queue_group Fridge {
+queue_group FridgeQueue {
implements aos.control_loops.ControlLoop;
// All angles are in radians with 0 sticking straight out horizontally over
@@ -43,6 +43,13 @@
// Are we zeroed and have reached our goal position on both the arm and
// elevator?
bool done;
+
+ // Angle of the arm.
+ double angle;
+ // Height of the elevator.
+ double height;
+ // state of the grabber pistons
+ GrabberPistons grabbers;
};
message Output {
@@ -60,4 +67,4 @@
queue Status status;
};
-queue_group Fridge fridge;
+queue_group FridgeQueue fridge_queue;
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
new file mode 100644
index 0000000..2c1151f
--- /dev/null
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -0,0 +1,130 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/fridge/fridge.q.h"
+#include "frc971/control_loops/fridge/fridge.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the fridge and sends out queue messages with the
+// position.
+class FridgeSimulation {
+ 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") {
+ }
+
+ // Sends a queue message with the position.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::FridgeQueue::Position> position =
+ 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();
+ }
+
+ ::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_;
+
+ private:
+ FridgeQueue fridge_queue_;
+};
+
+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_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ virtual ~FridgeTest() {
+ ::aos::robot_state.Clear();
+ }
+
+ // Update the robot state. Without this, the Iteration of the control loop
+ // will stop all the motors and the shooter won't go anywhere.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ fridge_queue_.goal.FetchLatest();
+ fridge_queue_.status.FetchLatest();
+ EXPECT_NEAR(fridge_queue_.goal->angle,
+ fridge_queue_.status->angle,
+ 10.0);
+ EXPECT_NEAR(fridge_queue_.goal->height,
+ fridge_queue_.status->height,
+ 10.0);
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to
+ // shared memory that is no longer valid.
+ FridgeQueue fridge_queue_;
+
+ // Create a control loop and simulation.
+ Fridge fridge_;
+ FridgeSimulation fridge_plant_;
+};
+
+// Tests that the loop does nothing when the goal is zero.
+TEST_F(FridgeTest, DoesNothing) {
+ fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.0).Send();
+ SendDSPacket(true);
+ fridge_plant_.SendPositionMessage();
+ fridge_.Iterate();
+ fridge_plant_.Simulate();
+ VerifyNearGoal();
+ fridge_queue_.output.FetchLatest();
+ EXPECT_EQ(fridge_queue_.output->left_arm, 0.0);
+ EXPECT_EQ(fridge_queue_.output->right_arm, 0.0);
+ EXPECT_EQ(fridge_queue_.output->left_elevator, 0.0);
+ EXPECT_EQ(fridge_queue_.output->right_elevator, 0.0);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/fridge/fridge_main.cc b/frc971/control_loops/fridge/fridge_main.cc
new file mode 100644
index 0000000..f92ced3
--- /dev/null
+++ b/frc971/control_loops/fridge/fridge_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/fridge/fridge.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::Fridge fridge;
+ fridge.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 121ce33..b6c5ceb 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -9,6 +9,10 @@
'../control_loops/control_loops.gyp:state_feedback_loop_test',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
+ '../control_loops/fridge/fridge.gyp:fridge',
+ '../control_loops/fridge/fridge.gyp:fridge_lib_test',
+ '../control_loops/claw/claw.gyp:claw',
+ '../control_loops/claw/claw.gyp:claw_lib_test',
'../autonomous/autonomous.gyp:auto',
'../actions/actions.gyp:drivetrain_action',
'../frc971.gyp:joystick_reader',