commit 7f1a4080808cb3efce1005b16ca3ae549c65af9f
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Apr 14 10:50:44 2019 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Apr 14 13:45:23 2019 -0700
tree 77a96431e8206516fe9d845a49245bac4063a8b5
parent 137388681ceba2c0203dd6ddb502f5e43c1f4fd0

Tuning constants from drive practice

Change-Id: I8a820317b693358326bb91a9df3051f481bc68b4

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index dd6b05c..4e13a40 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -331,7 +331,7 @@
 
     status->x = localizer_->x();
     status->y = localizer_->y();
-    status->theta = localizer_->theta();
+    status->theta = ::aos::math::NormalizeAngle(localizer_->theta());
 
     status->ground_angle = down_estimator_.X_hat(0) + dt_config_.down_offset;
 

frc971/control_loops/drivetrain/hybrid_ekf.h

diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index f006bb4..ae92069 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -459,7 +459,7 @@
   // since the wheels aren't likely to slip much stopped.
   Q_continuous_(kX, kX) = 0.002;
   Q_continuous_(kY, kY) = 0.002;
-  Q_continuous_(kTheta, kTheta) = 0.0002;
+  Q_continuous_(kTheta, kTheta) = 0.0001;
   Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.15, 2.0);
   Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.15, 2.0);
   Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.5, 2.0);