Adding the climber to the 2021 3rd robot messages
Signed-off-by: Sabina Leaver <100027607@mvla.net>
Change-Id: I05a32aff04cabc58a4c18fd4b9016172725c36a6
Signed-off-by: Sabina Leaver <100027607@mvla.net>
diff --git a/y2021_bot3/control_loops/superstructure/superstructure.cc b/y2021_bot3/control_loops/superstructure/superstructure.cc
index 22c7c44..fbedb22 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure.cc
@@ -31,6 +31,8 @@
std::clamp(unsafe_goal->intake_speed(), -12.0, 12.0);
output_struct.outtake_volts =
std::clamp(unsafe_goal->outtake_speed(), -12.0, 12.0);
+ output_struct.climber_volts =
+ std::clamp(unsafe_goal->climber_speed(), -12.0, 12.0);
output->Send(Output::Pack(*output->fbb(), &output_struct));
}
@@ -42,6 +44,7 @@
if (unsafe_goal != nullptr) {
status_builder.add_intake_speed(unsafe_goal->intake_speed());
status_builder.add_outtake_speed(unsafe_goal->outtake_speed());
+ status_builder.add_climber_speed(unsafe_goal->climber_speed());
}
status->Send(status_builder.Finish());
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
index 718edca..e1e3fce 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -9,7 +9,8 @@
// Outtake speed for the outtake roller, positive number means it is outtaking balls
outtake_speed:double (id:1);
- // TODO: Add climber
+ // Positive is deploying climber and to climb; cannot run in reverse
+ climber_speed:double (id: 2);
}
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
index cd031c3..a6b6353 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -43,15 +43,18 @@
}
void VerifyResults(double intake_voltage, double outtake_voltage,
- double intake_speed, double outtake_speed) {
+ double climber_voltage, double intake_speed,
+ double outtake_speed, double climber_speed) {
superstructure_output_fetcher_.Fetch();
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr);
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
EXPECT_EQ(superstructure_output_fetcher_->intake_volts(), intake_voltage);
EXPECT_EQ(superstructure_output_fetcher_->outtake_volts(), outtake_voltage);
+ EXPECT_EQ(superstructure_output_fetcher_->climber_volts(), climber_voltage);
EXPECT_EQ(superstructure_status_fetcher_->intake_speed(), intake_speed);
EXPECT_EQ(superstructure_status_fetcher_->outtake_speed(), outtake_speed);
+ EXPECT_EQ(superstructure_status_fetcher_->climber_speed(), climber_speed);
EXPECT_EQ(superstructure_status_fetcher_->estopped(), false);
EXPECT_EQ(superstructure_status_fetcher_->zeroed(), true);
}
@@ -83,7 +86,7 @@
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
SendPositionMessage();
RunFor(dt() * 2);
- VerifyResults(10.0, 0.0, 10.0, 0.0);
+ VerifyResults(10.0, 0.0, 0.0, 10.0, 0.0, 0.0);
}
{
@@ -92,10 +95,19 @@
goal_builder.add_outtake_speed(10.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(0.0, 10.0, 0.0, 10.0);
+ VerifyResults(0.0, 10.0, 0.0, 0.0, 10.0, 0.0);
}
}
+TEST_F(SuperstructureTest, RunClimber) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_climber_speed(4.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 0.0, 4.0, 0.0, 0.0, 4.0);
+}
+
// Tests running both the intake and the outtake simultaneously
TEST_F(SuperstructureTest, RunIntakeAndOuttake) {
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -104,7 +116,7 @@
goal_builder.add_outtake_speed(5.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(10.0, 5.0, 10.0, 5.0);
+ VerifyResults(10.0, 5.0, 0.0, 10.0, 5.0, 0.0);
}
// Tests for an invalid voltage (over 12 or under -12) to check that it defaults
@@ -115,9 +127,10 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_speed(20.0);
goal_builder.add_outtake_speed(15.0);
+ goal_builder.add_climber_speed(18.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(12.0, 12.0, 20.0, 15.0);
+ VerifyResults(12.0, 12.0, 12.0, 20.0, 15.0, 18.0);
}
{
@@ -125,9 +138,10 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_speed(-20.0);
goal_builder.add_outtake_speed(-15.0);
+ goal_builder.add_climber_speed(-18.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(-12.0, -12.0, -20.0, -15.0);
+ VerifyResults(-12.0, -12.0, -12.0, -20.0, -15.0, -18.0);
}
}
@@ -137,7 +151,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
- VerifyResults(0.0, 0.0, 0.0, 0.0);
+ VerifyResults(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
// Tests that the robot behaves properly when disabled
@@ -146,10 +160,11 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_speed(6.0);
goal_builder.add_outtake_speed(5.0);
+ goal_builder.add_climber_speed(4.0);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
SetEnabled(false);
RunFor(dt() * 2);
- VerifyResults(0.0, 0.0, 6.0, 5.0);
+ VerifyResults(0.0, 0.0, 0.0, 6.0, 5.0, 4.0);
}
} // namespace testing
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_output.fbs b/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
index 47b9476..69c7090 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -7,8 +7,8 @@
// Voltage for outtake motor, positive number is outtaking balls
outtake_volts:double (id:1);
- // TODO: Add climber
-
+ // Positive is deploying climber and to climb; cannot run in reverse
+ climber_volts:double (id:2);
}
root_type Output;
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_status.fbs b/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
index 4f94e76..a53d0a6 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -16,7 +16,8 @@
// Outtake speed for the outtake roller, positive number outtakes balls
outtake_speed:double (id:3);
- // TODO: Add climber
+ // Positive is deploying climber and to climb; cannot run in reverse
+ climber_speed:double (id: 4);
}
-root_type Status;
+root_type Status;
\ No newline at end of file