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();