commit 07508d0b034d3d2fa988d5e88ee59f103acbd74d
author Ravago Jones <ravagojones@gmail.com> Sat Nov 06 13:39:29 2021 -0700
committer Ravago Jones <ravagojones@gmail.com> Sat Nov 06 13:48:49 2021 -0700
tree 8a4ae9387497c5a8dc4512c9c390712735578f0e
parent 3de0f950297d5211f7e0265e67710a4a0328b8bc

Switch IMU orientation for different robots

Change-Id: I290961925edba2b6578738d94cb001f2bdba60b8
Signed-off-by: Ravago Jones <ravagojones@gmail.com>

y2020/control_loops/drivetrain/drivetrain_base.cc

diff --git a/y2020/control_loops/drivetrain/drivetrain_base.cc b/y2020/control_loops/drivetrain/drivetrain_base.cc
index ff47197..9cd7494 100644
--- a/y2020/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_base.cc
@@ -2,8 +2,10 @@
 
 #include <chrono>
 
+#include "aos/network/team_number.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/state_feedback_loop.h"
+#include "y2020/constants.h"
 #include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 #include "y2020/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
 #include "y2020/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
@@ -25,9 +27,8 @@
   static DrivetrainConfig<double> kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
-      ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
-      // TODO(james): CHECK IF THIS IS IMU_X or IMU_FLIPPED_X.
-      ::frc971::control_loops::drivetrain::IMUType::IMU_X,
+      ::frc971::control_loops::drivetrain::GyroType::IMU_X_GYRO,
+      ::frc971::control_loops::drivetrain::IMUType::IMU_Z,
 
       drivetrain::MakeDrivetrainLoop,
       drivetrain::MakeVelocityDrivetrainLoop,
@@ -54,12 +55,24 @@
       1.2 /* quickturn_wheel_multiplier */,
       1.2 /* wheel_multiplier */,
       true /*pistol_grip_shift_enables_line_follow*/,
-      // TODO(james): Check X/Y axis transformations.
-      (Eigen::Matrix<double, 3, 3>() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
-       1.0)
+      (Eigen::Matrix<double, 3, 3>() << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0,
+       0.0)
           .finished() /*imu_transform*/,
   };
 
+  if (::aos::network::GetTeamNumber() == constants::Values::kCompTeamNumber) {
+    // TODO(james): Check X/Y axis
+    // transformations.
+    kDrivetrainConfig.imu_transform = (Eigen::Matrix<double, 3, 3>() << 1.0,
+                                       0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)
+                                          .finished();
+    kDrivetrainConfig.gyro_type =
+        ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
+    // TODO(james): CHECK IF THIS IS IMU_X or IMU_FLIPPED_X.
+    kDrivetrainConfig.imu_type =
+        ::frc971::control_loops::drivetrain::IMUType::IMU_X;
+  }
+
   return kDrivetrainConfig;
 };