finished up implementing shooter output stuff
diff --git a/vision/SensorProcessor.h b/vision/SensorProcessor.h
index b5ad65c..fe9f0ff 100644
--- a/vision/SensorProcessor.h
+++ b/vision/SensorProcessor.h
@@ -1,6 +1,7 @@
+#ifndef VISION_SENSOR_PROCESSOR_H_
+#define VISION_SENSOR_PROCESSOR_H_
-#ifndef _SENSOR_PROCESSOR_H_
-#define _SENSOR_PROCESSOR_H_
+namespace frc971 {
// struct maps a single point x to to a value f of x
typedef struct {
@@ -8,21 +9,26 @@
double fx;
} Interpolation;
-// a set of mapping to use to determine distance
-// in inches given a pixel offset
-const Interpolation pixel_to_dist[4] = {
- {43.0, 495.0},
- {98.0, 260.0},
- {145.75, 174.0},
- {216.75, 110.0}};
-const Interpolation pixel_to_dist640x480[4] = {
- {86.0, 495.0},
- {196.0, 260.0},
- {291.5, 176.0},
- {433.5, 110.0}};
+static const Interpolation kPixelsToMeters[] = {
+ {43.0 / 320.0, 12.573},
+ {98.0 / 320.0, 6.604},
+ {145.75 / 320.0, 4.420},
+ {216.75 / 320.0, 2.794},
+};
+
+static const Interpolation kMetersToShooterSpeeds[] = {
+ {10.0, 200.0},
+ {5.0, 175.0},
+};
+
+static const Interpolation kMetersToShooterAngles[] = {
+ {10.0, 0.7},
+ {5.0, 0.9},
+};
double interpolate(int num_interp_vals,
const Interpolation *interp, double value);
-#endif //_SENSOR_PROCESSOR_H_
+} // namespace frc971
+#endif // VISION_SENSOR_PROCESSOR_H_