Add Climber class and tests
For now, it is a simple pass through.
Change-Id: I5274b03f6ade23fbc98713cf8ed849866a85b429
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index b9bbcf0..87ee689 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -55,6 +55,7 @@
"superstructure.h",
],
deps = [
+ ":climber",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -103,3 +104,20 @@
"//y2020/control_loops/superstructure/intake:intake_plants",
],
)
+
+cc_library(
+ name = "climber",
+ srcs = [
+ "climber.cc",
+ ],
+ hdrs = [
+ "climber.h",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/climber.cc b/y2020/control_loops/superstructure/climber.cc
new file mode 100644
index 0000000..bb449da
--- /dev/null
+++ b/y2020/control_loops/superstructure/climber.cc
@@ -0,0 +1,23 @@
+#include "y2020/control_loops/superstructure/climber.h"
+
+#include <algorithm>
+
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+
+void Climber::Iterate(const Goal *unsafe_goal, OutputT *output) {
+ if (unsafe_goal && output) {
+ // Pass through the voltage request from the user. Backwards isn't
+ // supported, so prevent that.
+ output->climber_voltage =
+ std::clamp(unsafe_goal->climber_voltage(), 0.0f, 12.0f);
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/superstructure/climber.h b/y2020/control_loops/superstructure/climber.h
new file mode 100644
index 0000000..29b7e51
--- /dev/null
+++ b/y2020/control_loops/superstructure/climber.h
@@ -0,0 +1,21 @@
+#ifndef Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
+#define Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
+
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+
+// Class to encapsulate the climber logic.
+class Climber {
+ public:
+ void Iterate(const Goal *unsafe_goal, OutputT *output);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+
+#endif // Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index eb88e93..c2512db 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -52,6 +52,8 @@
output != nullptr ? &(output_struct.turret_voltage) : nullptr,
status->fbb());
+ climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
+
bool zeroed;
bool estopped;
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 8fae9c5..2bc0cda 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -8,6 +8,7 @@
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/control_loops/superstructure/climber.h"
namespace y2020 {
namespace control_loops {
@@ -42,6 +43,7 @@
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
+ Climber climber_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index bade51e..7cdc974 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -24,7 +24,7 @@
// Positive = forward
intake:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
- //Positive is rollers intaking to Washing Machine.
+ // Positive is rollers intaking to Washing Machine.
roller_voltage:float;
// 0 = facing the front of the robot. Positive rotates counterclockwise.
@@ -49,7 +49,7 @@
shooter_tracking:bool;
// Positive is deploying climber and to climb; cannot run in reverse
- climber_winch_voltage:double;
+ climber_voltage:float;
}
root_type Goal;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index b4c6f9d..458a771 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -239,8 +239,12 @@
EXPECT_LE(-peak_turret_acceleration_, turret_acceleration);
EXPECT_GE(peak_turret_velocity_, turret_velocity());
EXPECT_LE(-peak_turret_velocity_, turret_velocity());
+
+ climber_voltage_ = superstructure_output_fetcher_->climber_voltage();
}
+ float climber_voltage() const { return climber_voltage_; }
+
void set_peak_hood_acceleration(double value) {
peak_hood_acceleration_ = value;
}
@@ -285,6 +289,8 @@
double peak_hood_velocity_ = 1e10;
double peak_intake_velocity_ = 1e10;
double peak_turret_velocity_ = 1e10;
+
+ float climber_voltage_ = 0.0f;
};
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
@@ -315,14 +321,21 @@
superstructure_goal_fetcher_.Fetch();
superstructure_status_fetcher_.Fetch();
- EXPECT_NEAR(superstructure_goal_fetcher_->hood()->unsafe_goal(),
- superstructure_status_fetcher_->hood()->position(), 0.001);
+ // Only check the goal if there is one.
+ if (superstructure_goal_fetcher_->has_hood()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->hood()->unsafe_goal(),
+ superstructure_status_fetcher_->hood()->position(), 0.001);
+ }
- EXPECT_NEAR(superstructure_goal_fetcher_->intake()->unsafe_goal(),
- superstructure_status_fetcher_->intake()->position(), 0.001);
+ if (superstructure_goal_fetcher_->has_intake()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->unsafe_goal(),
+ superstructure_status_fetcher_->intake()->position(), 0.001);
+ }
- EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
- superstructure_status_fetcher_->turret()->position(), 0.001);
+ if (superstructure_goal_fetcher_->has_turret()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
+ superstructure_status_fetcher_->turret()->position(), 0.001);
+ }
}
void CheckIfZeroed() {
@@ -534,6 +547,47 @@
CheckIfZeroed();
}
+// Tests that the climber passes through per the design.
+TEST_F(SuperstructureTest, Climber) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+
+ superstructure_plant_.InitializeHoodPosition(0.7);
+ superstructure_plant_.InitializeIntakePosition(0.7);
+
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_voltage(-10.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(1));
+
+ // Can't go backwards.
+ EXPECT_EQ(superstructure_plant_.climber_voltage(), 0.0);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_voltage(10.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+ RunFor(chrono::seconds(1));
+ // But forwards works.
+ EXPECT_EQ(superstructure_plant_.climber_voltage(), 10.0);
+
+ VerifyNearGoal();
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops