Add interpolation table boilerplate
Change-Id: I599821c53366e85a9b11a48f7a47888372553773
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index e7b6559..4739cd1 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -82,6 +82,7 @@
":superstructure_output_fbs",
":superstructure_position_fbs",
":superstructure_status_fbs",
+ "//aos:flatbuffer_merge",
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index 4a644e2..8b5c7eb 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -312,25 +312,28 @@
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
+Catapult::Iterate(const CatapultGoal *catapult_goal, const Position *position,
double battery_voltage, double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb) {
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
- *catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
- ? (unsafe_goal->catapult()->return_position())
- : nullptr;
+ *return_goal =
+ catapult_goal != nullptr && catapult_goal->has_return_position()
+ ? catapult_goal->return_position()
+ : nullptr;
const bool catapult_disabled = catapult_.Correct(
- catapult_goal, position->catapult(), catapult_voltage == nullptr);
+ return_goal, position->catapult(), catapult_voltage == nullptr);
if (catapult_disabled) {
catapult_state_ = CatapultState::PROFILE;
- } else if (catapult_.running() && unsafe_goal &&
- unsafe_goal->has_catapult() && fire && !last_firing_) {
+ } else if (catapult_.running() && catapult_goal != nullptr && fire &&
+ !last_firing_) {
catapult_state_ = CatapultState::FIRING;
+ latched_shot_position = catapult_goal->shot_position();
+ latched_shot_velocity = catapult_goal->shot_velocity();
}
- if (catapult_.running() && unsafe_goal && unsafe_goal->has_catapult()) {
+ if (catapult_.running()) {
last_firing_ = fire;
}
@@ -354,8 +357,7 @@
catapult_mpc_.SetState(
next_X.block<2, 1>(0, 0),
- Eigen::Vector2d(unsafe_goal->catapult()->shot_position(),
- unsafe_goal->catapult()->shot_velocity()));
+ Eigen::Vector2d(latched_shot_position, latched_shot_velocity));
const bool solved = catapult_mpc_.Solve();
@@ -379,7 +381,7 @@
use_profile_ = false;
}
} else {
- if (unsafe_goal && unsafe_goal->has_catapult() && !fire) {
+ if (!fire) {
// Eh, didn't manage to solve before it was time to fire. Give up.
catapult_state_ = CatapultState::PROFILE;
}
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index 0606c0d..6a2c834 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -210,14 +210,11 @@
// shooting or not. Returns the status.
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- Iterate(const Goal *unsafe_goal, const Position *position,
+ Iterate(const CatapultGoal *unsafe_goal, const Position *position,
double battery_voltage, double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb);
private:
- // TODO(austin): Prototype is just an encoder. Catapult has both an encoder
- // and pot. Switch back once we have a catapult.
- // PotAndAbsoluteEncoderSubsystem catapult_;
PotAndAbsoluteEncoderSubsystem catapult_;
catapult::CatapultController catapult_mpc_;
@@ -226,6 +223,9 @@
CatapultState catapult_state_ = CatapultState::PROFILE;
+ double latched_shot_position = 0.0;
+ double latched_shot_velocity = 0.0;
+
bool last_firing_ = false;
bool use_profile_ = true;
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 0b693ef..56e58cc 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -1,6 +1,7 @@
#include "y2022/control_loops/superstructure/superstructure.h"
#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
namespace y2022 {
@@ -48,6 +49,7 @@
aos::FlatbufferFixedAllocatorArray<
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
turret_goal_buffer;
+ aos::FlatbufferFixedAllocatorArray<CatapultGoal, 64> catapult_goal_buffer;
const aos::monotonic_clock::time_point timestamp =
event_loop()->context().monotonic_event_time;
@@ -64,6 +66,7 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*turret_goal = nullptr;
+ const CatapultGoal *catapult_goal = nullptr;
double roller_speed_compensated_front = 0.0;
double roller_speed_compensated_back = 0.0;
double transfer_roller_speed_front = 0.0;
@@ -84,6 +87,31 @@
turret_goal =
unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
+
+ catapult_goal = unsafe_goal->catapult();
+
+ constants::Values::ShotParams shot_params;
+ const double distance_to_goal = aimer_.DistanceToGoal();
+ if (unsafe_goal->auto_aim() && values_->shot_interpolation_table.GetInRange(
+ distance_to_goal, &shot_params)) {
+ std::optional<flatbuffers::Offset<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal>>
+ return_position_offset;
+ if (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
+ unsafe_goal->catapult()->has_return_position()) {
+ return_position_offset = {
+ aos::CopyFlatBuffer(unsafe_goal->catapult()->return_position(),
+ catapult_goal_buffer.fbb())};
+ }
+ CatapultGoal::Builder builder(*catapult_goal_buffer.fbb());
+ builder.add_shot_position(shot_params.shot_angle);
+ builder.add_shot_velocity(shot_params.shot_velocity);
+ if (return_position_offset.has_value()) {
+ builder.add_return_position(return_position_offset.value());
+ }
+ catapult_goal_buffer.Finish(builder.Finish());
+ catapult_goal = &catapult_goal_buffer.message();
+ }
}
// Superstructure state machine:
@@ -395,7 +423,7 @@
// flippers
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
catapult_status_offset = catapult_.Iterate(
- unsafe_goal, position, robot_state().voltage_battery(),
+ catapult_goal, position, robot_state().voltage_battery(),
output != nullptr && !catapult_.estopped()
? &(output_struct.catapult_voltage)
: nullptr,
@@ -459,6 +487,10 @@
status_builder.add_solve_time(catapult_.solve_time());
status_builder.add_shot_count(catapult_.shot_count());
status_builder.add_mpc_active(catapult_.mpc_active());
+ if (catapult_goal != nullptr) {
+ status_builder.add_shot_position(catapult_goal->shot_position());
+ status_builder.add_shot_velocity(catapult_goal->shot_velocity());
+ }
status_builder.add_flippers_open(flippers_open_);
status_builder.add_reseating_in_catapult(reseating_in_catapult_);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 73ceee7..e6ff227 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -1254,6 +1254,42 @@
superstructure_status_fetcher_->aimer()->turret_velocity());
}
+TEST_F(SuperstructureTest, InterpolationTableTest) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ constexpr double kDistance = 3.0;
+
+ SendDrivetrainStatus(0.0, {0.0, kDistance}, 0.0);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_auto_aim(true);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(2));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(superstructure_status_fetcher_->aimer()->target_distance(),
+ kDistance, 0.01);
+
+ constants::Values::ShotParams shot_params;
+ EXPECT_TRUE(
+ values_->shot_interpolation_table.GetInRange(kDistance, &shot_params));
+
+ EXPECT_EQ(superstructure_status_fetcher_->shot_velocity(),
+ shot_params.shot_velocity);
+ EXPECT_EQ(superstructure_status_fetcher_->shot_position(),
+ shot_params.shot_angle);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 4b215ac..d802f08 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -70,6 +70,8 @@
solve_time:double (id: 7);
mpc_active:bool (id: 8);
+ shot_position:double (id: 16);
+ shot_velocity:double (id: 17);
// The number of shots we have taken.
shot_count:int32 (id: 9);