commit 09f564a999d4bf2dff930838156513ccffef40d2
author James Kuszmaul <jabukuszmaul@gmail.com> Mon Feb 18 17:37:09 2019 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Thu Feb 21 18:06:08 2019 -0800
tree 41327e7a831ea83be1fca8c3d459dc2b52438117
parent 06c36922b400fb6fe795fc490c4b55940f90422f

Fold y2019 localizer into drivetrain

Change-Id: Icc192ae9f910741b54f114ec9a27559cc289b29b

y2019/constants.cc

diff --git a/y2019/constants.cc b/y2019/constants.cc
index d77d2c7..7baac55 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -23,6 +23,7 @@
 using ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator;
 
 const int Values::kZeroingSampleSize;
+constexpr size_t Values::kNumCameras;
 
 namespace {
 
@@ -122,6 +123,12 @@
   stilts_params->zeroing_constants.moving_buffer_size = 20;
   stilts_params->zeroing_constants.allowable_encoder_error = 0.9;
 
+  r->camera_noise_parameters = {.max_viewable_distance = 10.0,
+                                .heading_noise = 0.02,
+                                .nominal_distance_noise = 0.06,
+                                .nominal_skew_noise = 0.1,
+                                .nominal_height_noise = 0.01};
+
   switch (team) {
     // A set of constants for tests.
     case 1:
@@ -135,6 +142,17 @@
 
       stilts_params->zeroing_constants.measured_absolute_position = 0.0;
       stilts->potentiometer_offset = 0.0;
+
+      // Deliberately make FOV a bit large so that we are overly conservative in
+      // our EKF.
+      for (auto &camera : r->cameras) {
+        camera.fov = M_PI_2 * 1.1;
+      }
+      r->cameras[0].pose.set_theta(M_PI);
+      r->cameras[1].pose.set_theta(0.26);
+      r->cameras[2].pose.set_theta(-0.26);
+      r->cameras[3].pose.set_theta(M_PI_2);
+      r->cameras[4].pose.set_theta(-M_PI_2);
       break;
 
     case kCompTeamNumber: