Added distance interpolation table for vision
Change-Id: I20077eb38c0c3fa91c4e2b6dc32a1bd7233e7ebf
diff --git a/frc971/shooter_interpolation/BUILD b/frc971/shooter_interpolation/BUILD
new file mode 100644
index 0000000..bbef440
--- /dev/null
+++ b/frc971/shooter_interpolation/BUILD
@@ -0,0 +1,20 @@
+cc_library(
+ name = 'interpolation',
+ hdrs = [
+ 'interpolation.h',
+ ],
+ srcs = [
+ 'interpolation.cc',
+ ],
+)
+
+cc_test(
+ name = 'interpolation_test',
+ srcs = [
+ 'interpolation_test.cc',
+ ],
+ deps = [
+ ':interpolation',
+ '//aos/testing:googletest',
+ ]
+)
diff --git a/frc971/shooter_interpolation/interpolation.cc b/frc971/shooter_interpolation/interpolation.cc
new file mode 100644
index 0000000..71989ff
--- /dev/null
+++ b/frc971/shooter_interpolation/interpolation.cc
@@ -0,0 +1,55 @@
+#include "frc971/shooter_interpolation/interpolation.h"
+
+#include <algorithm>
+#include <utility>
+#include <vector>
+
+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(
+ ::std::vector<::std::pair<double, ShotParams>> interpolation_table) {
+ interpolation_table_ = ::std::move(interpolation_table);
+ ::std::sort(interpolation_table_.begin(), interpolation_table_.end(),
+ [](const ::std::pair<double, ShotParams> &a,
+ const ::std::pair<double, ShotParams> &b) {
+ return a.first < b.first;
+ });
+}
+
+ShotParams InterpolationTable::GetShooterData(double distance) {
+ // Points to to the smallest item such that it->first >= dist, or end() if no
+ // such item exists.
+ auto it =
+ std::lower_bound(interpolation_table_.begin(), interpolation_table_.end(),
+ distance, [](const ::std::pair<double, ShotParams> &a,
+ double dist) { return a.first < dist; });
+ if (it == interpolation_table_.begin()) {
+ return it->second;
+ } else if (it == interpolation_table_.end()) {
+ return interpolation_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
new file mode 100644
index 0000000..a40993a
--- /dev/null
+++ b/frc971/shooter_interpolation/interpolation.h
@@ -0,0 +1,33 @@
+#ifndef FRC971_SHOOTER_INTERPOLATION_INTERPOLATION_H_
+#define FRC971_SHOOTER_INTERPOLATION_INTERPOLATION_H_
+
+#include <utility>
+#include <vector>
+
+namespace frc971 {
+namespace shooter_interpolation {
+
+// Struct for shot angle and power
+struct ShotParams {
+ double angle;
+ double power;
+};
+
+class InterpolationTable {
+ public:
+ InterpolationTable(
+ ::std::vector<::std::pair<double, ShotParams>> interpolation_table);
+
+ // Uses the interpolation table to calculate the optimal shooter angle and
+ // power for a shot
+ ShotParams GetShooterData(double distance);
+
+ private:
+ // Contains the list of angle entries in the interpolation table
+ ::std::vector<::std::pair<double, ShotParams>> interpolation_table_;
+};
+
+} // namespace shooter_interpolation
+} // namespace frc971
+
+#endif // FRC971_SHOOTER_INTERPOLATION_INTERPOLATION_H_
diff --git a/frc971/shooter_interpolation/interpolation_test.cc b/frc971/shooter_interpolation/interpolation_test.cc
new file mode 100644
index 0000000..120975d
--- /dev/null
+++ b/frc971/shooter_interpolation/interpolation_test.cc
@@ -0,0 +1,55 @@
+#include <unistd.h>
+
+#include <memory>
+#include <random>
+#include <utility>
+
+#include "gtest/gtest.h"
+
+#include "frc971/shooter_interpolation/interpolation.h"
+
+namespace frc971 {
+namespace shooter_interpolation {
+
+bool operator==(ShotParams a1, ShotParams a2) {
+ return a1.angle == a2.angle && a1.power == a2.power;
+}
+
+// Tests to see if distances whose values are on the table are processed
+// correctly
+TEST(InterpolationTable, ExactNumbers) {
+ ::std::vector<::std::pair<double, ShotParams>> 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));
+}
+
+// Tests to see if distances whose values are off the table are processed
+// correctly
+TEST(InterpolationTable, InexactNumbers) {
+ ::std::vector<::std::pair<double, ShotParams>> 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));
+}
+
+// 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{
+ {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));
+}
+
+} // namespace shooter_interpolation
+} // namespace frc971