lots of vision stuff from Ben
diff --git a/vision/SensorProcessor.cpp b/vision/SensorProcessor.cpp
new file mode 100644
index 0000000..1264a20
--- /dev/null
+++ b/vision/SensorProcessor.cpp
@@ -0,0 +1,50 @@
+
+#include "SensorProcessor.h"
+#include <stdio.h>
+
+// give a set of x -> fx pairs find a range for our value
+// then interpolate between and return an interpolated fx.
+// If the value is off the end just extend the line to
+// meet our point. If something does go wrong (and it
+// never should) it will return -1.
+double interpolate(int num_interp_vals,
+ const Interpolation *interp, double value) {
+ double dy;
+ double dx;
+ double a;
+ double intercept;
+ //printf("for val %.1f\n", value);
+ if (value < interp[0].x) {
+ // if closer than nearest
+ dy = interp[1].fx - interp[0].fx;
+ dx = interp[1].x - interp[0].x;
+ a = value - interp[0].x;
+ intercept = interp[0].fx;
+ //printf("LESS THAN\n");
+ } else if (value > interp[num_interp_vals-1].x){
+ // if further than furthest
+ dy = interp[num_interp_vals-1].fx - interp[num_interp_vals-2].fx;
+ dx = interp[num_interp_vals-1].x - interp[num_interp_vals-2].x;
+ a = value - interp[num_interp_vals-2].x;
+ intercept = interp[num_interp_vals-2].fx;
+ //printf("GT THAN\n");
+ } else {
+ //printf("gh0\n");
+ // scan for range
+ for(int i=0; i<num_interp_vals-1; i++){
+ if(value >= interp[i].x && value <= interp[i+1].x){
+ // printf("(%.1f,%.1f)=(%.1f,%.1f)\n",
+ // interp[i].x, interp[i+1].x,
+ // interp[i].fx, interp[i+1].fx);
+ double lambda =
+ (value - interp[i].x)/(interp[i+1].x - interp[i].x);
+ return (1-lambda)*interp[i].fx + lambda*interp[i+1].fx;
+ }
+ }
+ // this should maybe be an assert
+ return -1;
+ }
+
+ return ( (dy/dx)*a + intercept );
+}
+