commit 2527ed229972811967d3c097e259df11f38d2dc7
author Campbell Crowley <mail@campbellcrowley.com> Fri Feb 17 21:10:02 2017 -0800
committer Campbell Crowley <mail@campbellcrowley.com> Fri Feb 17 21:42:44 2017 -0800
tree bf44b0bd31b5085fff258cd64c6ca5a7c4272290
parent 645963c31449a4e9d5ba00c31f27b438bfe7c3a5

Add support in drivetrain for IMU gyro

Change-Id: Id1d63cb2f01ae34d53c620168f85ac5d5b53e665

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7bbba74..6a2822b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -20,6 +20,7 @@
 #include "frc971/wpilib/imu.q.h"
 
 using frc971::sensors::gyro_reading;
+using frc971::imu_values;
 
 namespace frc971 {
 namespace control_loops {
@@ -121,8 +122,8 @@
     gear_logging.controller_index = kf_.controller_index();
     LOG_STRUCT(DEBUG, "state", gear_logging);
   }
-
-  if (::frc971::imu_values.FetchLatest()) {
+  const bool is_latest_imu_values = ::frc971::imu_values.FetchLatest();
+  if (is_latest_imu_values) {
     const double rate = -::frc971::imu_values->gyro_y;
     const double accel_squared = ::frc971::imu_values->accelerometer_x *
                                      ::frc971::imu_values->accelerometer_x +
@@ -154,10 +155,40 @@
 
   // TODO(austin): Signal the current gear to both loops.
 
-  if (gyro_reading.FetchLatest()) {
-    LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
-    last_gyro_heading_ = gyro_reading->angle;
-    last_gyro_rate_ = gyro_reading->velocity;
+  switch (dt_config_.gyro_type) {
+    case GyroType::IMU_X_GYRO:
+      if (is_latest_imu_values) {
+        LOG_STRUCT(DEBUG, "using", *imu_values.get());
+        last_gyro_rate_ = imu_values->gyro_x;
+      }
+      break;
+    case GyroType::IMU_Y_GYRO:
+      if (is_latest_imu_values) {
+        LOG_STRUCT(DEBUG, "using", *imu_values.get());
+        last_gyro_rate_ = imu_values->gyro_y;
+      }
+      break;
+    case GyroType::IMU_Z_GYRO:
+      if (is_latest_imu_values) {
+        LOG_STRUCT(DEBUG, "using", *imu_values.get());
+        last_gyro_rate_ = imu_values->gyro_z;
+      }
+      break;
+    case GyroType::SPARTAN_GYRO:
+      if (gyro_reading.FetchLatest()) {
+        LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+        last_gyro_rate_ = gyro_reading->velocity;
+      }
+      break;
+    case GyroType::FLIPPED_SPARTAN_GYRO:
+      if (gyro_reading.FetchLatest()) {
+        LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+        last_gyro_rate_ = -gyro_reading->velocity;
+      }
+      break;
+    default:
+      LOG(FATAL, "invalid gyro configured");
+      break;
   }
 
   {