Templatize interpolation table to make it more geneirc.
Change-Id: I7da0698486775b3e2482fa99fa172d6e1b6028ac
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 2e97b9e..802c57d 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -88,12 +88,12 @@
// TODO(phil): Should these be different per robot?
*shot_interpolation_table =
- ::frc971::shooter_interpolation::InterpolationTable(
+ ::frc971::shooter_interpolation::InterpolationTable<Values::ShotParams>(
{// { distance_to_target, { shot_angle, shot_power }},
- {1.67, {0.31, 320.0}},
- {1.90, {0.33, 330.0}},
- {2.15, {0.33, 347.0}},
- {2.45, {0.33, 361.0}},
+ {1.67, {0.31, 320.0, -2.25 * M_PI}},
+ {1.90, {0.33, 330.0, -2.25 * M_PI}},
+ {2.15, {0.33, 347.0, -2.25 * M_PI}},
+ {2.45, {0.33, 361.0, -2.25 * M_PI}},
});
switch (team) {
@@ -190,5 +190,12 @@
return *values[team_number];
}
+Values::ShotParams Values::ShotParams::BlendY(double coefficient, Values::ShotParams a1, Values::ShotParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return Values::ShotParams{Blend(coefficient, a1.angle, a2.angle),
+ Blend(coefficient, a1.power, a2.power),
+ Blend(coefficient, a1.indexer_velocity, a2.indexer_velocity)};
+}
+
} // namespace constants
} // namespace y2017
diff --git a/y2017/constants.h b/y2017/constants.h
index c6aa70c..bf25990 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -127,7 +127,15 @@
double vision_error;
- ::frc971::shooter_interpolation::InterpolationTable shot_interpolation_table;
+ struct ShotParams {
+ double angle;
+ double power;
+ double indexer_velocity;
+
+ static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2);
+ };
+
+ ::frc971::shooter_interpolation::InterpolationTable<ShotParams> shot_interpolation_table;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 2de0ab8..1faed36 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -45,20 +45,26 @@
// Create a copy of the goals so that we can modify them.
HoodGoal hood_goal;
ShooterGoal shooter_goal;
+ IndexerGoal indexer_goal;
if (unsafe_goal != nullptr) {
hood_goal = unsafe_goal->hood;
shooter_goal = unsafe_goal->shooter;
+ indexer_goal = unsafe_goal->indexer;
distance_average_.Tick(::aos::monotonic_clock::now(), vision_status);
status->vision_distance = distance_average_.Get();
if (distance_average_.Valid()) {
LOG(DEBUG, "VisionDistance %f\n", status->vision_distance);
if (unsafe_goal->use_vision_for_shots) {
- auto shot_params =
- constants::GetValues().shot_interpolation_table.GetShooterData(
- distance_average_.Get());
- hood_goal.angle = shot_params.angle;
- shooter_goal.angular_velocity = shot_params.power;
+ y2017::constants::Values::ShotParams shot_params;
+ if (constants::GetValues().shot_interpolation_table.GetInRange(
+ distance_average_.Get(), &shot_params)) {
+ hood_goal.angle = shot_params.angle;
+ shooter_goal.angular_velocity = shot_params.power;
+ if (indexer_goal.angular_velocity != 0.0) {
+ indexer_goal.angular_velocity = shot_params.indexer_velocity;
+ }
+ }
}
} else {
LOG(DEBUG, "VisionNotValid %f\n", status->vision_distance);
@@ -117,7 +123,7 @@
output != nullptr ? &(output->voltage_intake) : nullptr,
&(status->intake));
- column_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
+ column_.Iterate(unsafe_goal != nullptr ? &indexer_goal : nullptr,
unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
&(position->column), vision_status,
output != nullptr ? &(output->voltage_indexer) : nullptr,