Templatize interpolation table to make it more geneirc.
Change-Id: I7da0698486775b3e2482fa99fa172d6e1b6028ac
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 41812f2..f0cdb58 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -351,7 +351,7 @@
DelayedU_ * (::aos::robot_state.get()
? ::aos::robot_state->voltage_battery / 12.0
: 1.0);
- X_ = Update(X(), current_U);
+ X_ = Update(X(), current_U, dt);
Y_ = C() * X() + D() * current_U;
DelayedU_ = U;
}
diff --git a/frc971/shooter_interpolation/interpolation.cc b/frc971/shooter_interpolation/interpolation.cc
index 4eba448..66b2cf9 100644
--- a/frc971/shooter_interpolation/interpolation.cc
+++ b/frc971/shooter_interpolation/interpolation.cc
@@ -7,48 +7,9 @@
namespace frc971 {
namespace shooter_interpolation {
-namespace {
-
double Blend(double coefficient, double a1, double a2) {
return (1 - coefficient) * a1 + coefficient * a2;
}
-ShotParams Blend(double coefficient, ShotParams a1, ShotParams a2) {
- return ShotParams{Blend(coefficient, a1.angle, a2.angle),
- Blend(coefficient, a1.power, a2.power)};
-}
-
-} // namespace
-
-InterpolationTable::InterpolationTable(
- const ::std::vector<::std::pair<double, ShotParams>> &table)
- : table_(table) {
- ::std::sort(table_.begin(), table_.end(),
- [](const ::std::pair<double, ShotParams> &a,
- const ::std::pair<double, ShotParams> &b) {
- return a.first < b.first;
- });
-}
-
-ShotParams InterpolationTable::GetShooterData(double distance) const {
- // Points to to the smallest item such that it->first >= dist, or end() if no
- // such item exists.
- auto it = ::std::lower_bound(table_.begin(), table_.end(), distance,
- [](const ::std::pair<double, ShotParams> &a,
- double dist) { return a.first < dist; });
- if (it == table_.begin()) {
- return it->second;
- } else if (it == table_.end()) {
- return table_.back().second;
- } else {
- auto x_a2 = it;
- auto x_a1 = it - 1;
- double x1 = x_a1->first;
- double x2 = x_a2->first;
- double coefficient = (distance - x1) / (x2 - x1);
- return Blend(coefficient, x_a1->second, x_a2->second);
- }
-}
-
} // namespace shooter_interpolation
} // namespace frc971
diff --git a/frc971/shooter_interpolation/interpolation.h b/frc971/shooter_interpolation/interpolation.h
index 933c95e..dd0e316 100644
--- a/frc971/shooter_interpolation/interpolation.h
+++ b/frc971/shooter_interpolation/interpolation.h
@@ -1,33 +1,85 @@
#ifndef FRC971_SHOOTER_INTERPOLATION_INTERPOLATION_H_
#define FRC971_SHOOTER_INTERPOLATION_INTERPOLATION_H_
+#include <algorithm>
#include <utility>
#include <vector>
namespace frc971 {
namespace shooter_interpolation {
-// Struct for shot angle and power
-struct ShotParams {
- double angle;
- double power;
-};
+double Blend(double coefficient, double a1, double a2);
+template <typename YValue>
class InterpolationTable {
public:
+ using Point = ::std::pair<double, YValue>;
InterpolationTable() = default;
- InterpolationTable(
- const ::std::vector<::std::pair<double, ShotParams>> &table);
+ InterpolationTable(const ::std::vector<Point> &table);
// Uses the interpolation table to calculate the optimal shooter angle and
// power for a shot
- ShotParams GetShooterData(double distance) const;
+ YValue Get(double x) const;
+
+ bool GetInRange(double x, YValue* type) const;
private:
// Contains the list of angle entries in the interpolation table
- ::std::vector<::std::pair<double, ShotParams>> table_;
+ ::std::vector<Point> table_;
};
+template <typename YValue>
+InterpolationTable<YValue>::InterpolationTable(const ::std::vector<Point> &table)
+ : table_(table) {
+ ::std::sort(table_.begin(), table_.end(),
+ [](const Point &a, const Point &b) {
+ return a.first < b.first;
+ });
+ }
+
+template <typename YValue>
+YValue InterpolationTable<YValue>::Get(double x) const {
+ // Points to to the smallest item such that it->first >= dist, or end() if no
+ // such item exists.
+ auto it = ::std::lower_bound(table_.begin(), table_.end(), x,
+ [](const Point &a,
+ double dist) { return a.first < dist; });
+ if (it == table_.begin()) {
+ return it->second;
+ } else if (it == table_.end()) {
+ return table_.back().second;
+ } else {
+ auto x_a2 = it;
+ auto x_a1 = it - 1;
+ double x1 = x_a1->first;
+ double x2 = x_a2->first;
+ double coefficient = (x - x1) / (x2 - x1);
+ return YValue::BlendY(coefficient, x_a1->second, x_a2->second);
+ }
+}
+
+template <typename YValue>
+bool InterpolationTable<YValue>::GetInRange(double x, YValue* result) const {
+ // Points to to the smallest item such that it->first >= dist, or end() if no
+ // such item exists.
+ auto it = ::std::lower_bound(table_.begin(), table_.end(), x,
+ [](const Point &a,
+ double dist) { return a.first < dist; });
+ if (it == table_.begin()) {
+ return false;
+ } else if (it == table_.end()) {
+ return false;
+ } else {
+ auto x_a2 = it;
+ auto x_a1 = it - 1;
+ double x1 = x_a1->first;
+ double x2 = x_a2->first;
+ double coefficient = (x - x1) / (x2 - x1);
+ *result = YValue::BlendY(coefficient, x_a1->second, x_a2->second);
+ return true;
+ }
+}
+
} // namespace shooter_interpolation
} // namespace frc971
diff --git a/frc971/shooter_interpolation/interpolation_test.cc b/frc971/shooter_interpolation/interpolation_test.cc
index 120975d..fd99124 100644
--- a/frc971/shooter_interpolation/interpolation_test.cc
+++ b/frc971/shooter_interpolation/interpolation_test.cc
@@ -11,44 +11,57 @@
namespace frc971 {
namespace shooter_interpolation {
-bool operator==(ShotParams a1, ShotParams a2) {
+struct TestShotParams {
+ double angle;
+ double power;
+ static TestShotParams BlendY(double x, const TestShotParams& a, const TestShotParams& b) {
+ return TestShotParams{
+ Blend(x, a.angle, b.angle),
+ Blend(x, a.power, b.power)
+ };
+ }
+};
+
+bool operator==(TestShotParams a1, TestShotParams a2) {
return a1.angle == a2.angle && a1.power == a2.power;
}
+using TestInterpolationTable = InterpolationTable<TestShotParams>;
+
// Tests to see if distances whose values are on the table are processed
// correctly
TEST(InterpolationTable, ExactNumbers) {
- ::std::vector<::std::pair<double, ShotParams>> data{
+ ::std::vector<::std::pair<double, TestShotParams>> data = {
{1, {10, 10}}, {3, {20, 20}}, {2, {15, 12345678}}, {4, {10, 567.323}},
};
- InterpolationTable interpolation(data);
- ASSERT_EQ(data[1].second, interpolation.GetShooterData(3));
- ASSERT_EQ(data[3].second, interpolation.GetShooterData(4));
+ TestInterpolationTable interpolation(data);
+ ASSERT_EQ(data[1].second, interpolation.Get(3));
+ ASSERT_EQ(data[3].second, interpolation.Get(4));
}
// Tests to see if distances whose values are off the table are processed
// correctly
TEST(InterpolationTable, InexactNumbers) {
- ::std::vector<::std::pair<double, ShotParams>> data{
+ ::std::vector<::std::pair<double, TestShotParams>> data = {
{1, {10, 10}}, {3, {20, 20}}, {2, {15, 15}}, {4, {10, 567.323}},
};
- InterpolationTable interpolation(data);
- ASSERT_EQ(ShotParams({12.5, 12.5}), interpolation.GetShooterData(1.5));
- ASSERT_EQ(ShotParams({10, 10}), interpolation.GetShooterData(0));
+ TestInterpolationTable interpolation(data);
+ ASSERT_EQ(TestShotParams({12.5, 12.5}), interpolation.Get(1.5));
+ ASSERT_EQ(TestShotParams({10, 10}), interpolation.Get(0));
}
// Tests to see if distances whose values are beyond the range of the table are
// processed correctly
TEST(InterpolationTable, OutOfScopeNumbers) {
- ::std::vector<::std::pair<double, ShotParams>> data{
+ ::std::vector<::std::pair<double, TestShotParams>> data = {
{1, {10, 10}}, {3, {20, 20}}, {2, {15, 12345678}}, {4, {10, 567.323}},
};
- InterpolationTable interpolation(data);
- ASSERT_EQ(ShotParams({10, 10}), interpolation.GetShooterData(0));
- ASSERT_EQ(ShotParams({10, 567.323}), interpolation.GetShooterData(5));
+ TestInterpolationTable interpolation(data);
+ ASSERT_EQ(TestShotParams({10, 10}), interpolation.Get(0));
+ ASSERT_EQ(TestShotParams({10, 567.323}), interpolation.Get(5));
}
} // namespace shooter_interpolation
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,