Add a stowed climber position
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Id0df3e4044b9aafcce95160c05a9a57bcdd8a03c
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 4c93ebd..38d0a76 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -85,7 +85,7 @@
},
"climber_set_points": {
"full_extend": 0.8,
- "half_extend": 0.6,
+ "stowed": 0.4,
"retract": 0.2
},
"climber": {
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 3c1f409..883a945 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -51,7 +51,7 @@
// and retracted, which represents meters for when ClimberGoal is RETRACT
table ClimberSetPoints {
full_extend:double (id: 0);
- half_extend:double (id: 1);
+ stowed:double (id: 1);
retract:double (id: 2);
}
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index fece6e3..ce5b6a9 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -87,8 +87,9 @@
climber_position =
robot_constants_->common()->climber_set_points()->retract();
break;
- default:
- break;
+ case ClimberGoal::STOWED:
+ climber_position =
+ robot_constants_->common()->climber_set_points()->stowed();
}
}
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index f9dfcaa..42caf9b 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -18,6 +18,7 @@
enum ClimberGoal : ubyte {
FULL_EXTEND = 0,
RETRACT = 1,
+ STOWED = 2,
}
table ShooterGoal {
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 0e6498e..eb58424 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -455,6 +455,12 @@
->climber_set_points()
->full_extend();
}
+
+ if (superstructure_goal_fetcher_->climber_goal() == ClimberGoal::STOWED) {
+ set_point = simulated_robot_constants_->common()
+ ->climber_set_points()
+ ->stowed();
+ }
EXPECT_NEAR(set_point,
superstructure_status_fetcher_->climber()->position(), 0.001);
}
@@ -1563,4 +1569,55 @@
ExtendRollerStatus::IDLE);
EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 0.0);
}
-} // namespace y2024::control_loops::superstructure::testing
\ No newline at end of file
+
+TEST_F(SuperstructureTest, Climbing) {
+ SetEnabled(true);
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_goal(ClimberGoal::STOWED);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_goal(ClimberGoal::RETRACT);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ // TODO(max): Fill this with the logic to move the altitude and turret on the
+ // chain.
+}
+} // namespace y2024::control_loops::superstructure::testing