finished up implementing shooter output stuff
diff --git a/vision/GoalMaster.cpp b/vision/GoalMaster.cpp
index be42084..d5e8822 100644
--- a/vision/GoalMaster.cpp
+++ b/vision/GoalMaster.cpp
@@ -7,28 +7,47 @@
#include "frc971/queues/CameraTarget.q.h"
#include "vision/RingBuffer.h"
+#include "vision/SensorProcessor.h"
-using frc971::vision::RingBuffer;
-using frc971::sensors::gyro;
-using frc971::vision::targets;
-using frc971::vision::target_angle;
+using ::frc971::vision::RingBuffer;
+using ::frc971::sensors::gyro;
+using ::frc971::vision::targets;
+using ::frc971::vision::target_angle;
+using ::frc971::kPixelsToMeters;
+using ::frc971::kMetersToShooterSpeeds;
+using ::frc971::kMetersToShooterAngles;
+using ::frc971::interpolate;
int main() {
- RingBuffer< ::aos::time::Time, double> buff;
+ //RingBuffer< ::aos::time::Time, double> buff;
::aos::InitNRT();
while (true) {
- gyro.FetchNextBlocking();
- buff.Sample(gyro->sent_time, gyro->angle);
+ //gyro.FetchNextBlocking();
+ //buff.Sample(gyro->sent_time, gyro->angle);
if (targets.FetchNext()) {
- ::aos::time::Time stamp = ::aos::time::Time::InNS(targets->timestamp);
+ /*::aos::time::Time stamp = ::aos::time::Time::InNS(targets->timestamp);
double angle_goal =
buff.ValueAt(stamp) -
M_PI / 2.0 * targets->percent_azimuth_off_center / 2.0;
printf("%g ",angle_goal);
- printf("%g\n",gyro->angle);
+ printf("%g\n",gyro->angle);*/
+
+ double meters = interpolate(
+ sizeof(kPixelsToMeters) / sizeof(kPixelsToMeters[0]),
+ kPixelsToMeters,
+ targets->percent_elevation_off_center);
target_angle.MakeWithBuilder()
- .target_angle(angle_goal).Send();
+ .target_angle(/*angle_goal*/0)
+ .shooter_speed(interpolate(
+ sizeof(kMetersToShooterSpeeds) / sizeof(kMetersToShooterSpeeds[0]),
+ kMetersToShooterSpeeds,
+ meters))
+ .shooter_angle(interpolate(
+ sizeof(kMetersToShooterAngles) / sizeof(kMetersToShooterAngles[0]),
+ kMetersToShooterAngles,
+ meters))
+ .Send();
}
}
::aos::Cleanup();
diff --git a/vision/SensorProcessor.cpp b/vision/SensorProcessor.cpp
index 1264a20..2060956 100644
--- a/vision/SensorProcessor.cpp
+++ b/vision/SensorProcessor.cpp
@@ -1,7 +1,9 @@
+#include "vision/SensorProcessor.h"
-#include "SensorProcessor.h"
#include <stdio.h>
+namespace frc971 {
+
// 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
@@ -48,3 +50,4 @@
return ( (dy/dx)*a + intercept );
}
+} // namespace frc971
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_
diff --git a/vision/vision.gyp b/vision/vision.gyp
index c9ecc36..26ddb23 100644
--- a/vision/vision.gyp
+++ b/vision/vision.gyp
@@ -25,6 +25,7 @@
'type': 'executable',
'sources': [
'GoalMaster.cpp',
+ 'SensorProcessor.cpp',
],
'dependencies': [
'<(AOS)/atom_code/atom_code.gyp:init',