Switch IMU orientation for different robots
Change-Id: I290961925edba2b6578738d94cb001f2bdba60b8
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 17a2959..f582696 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -101,6 +101,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
+ "//y2020:constants",
":polydrivetrain_plants",
"//frc971:shifter_hall_effect",
"//frc971/control_loops/drivetrain:drivetrain_config",
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;
};
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index ab600fa..a01f47d 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -1,5 +1,6 @@
#include "y2020/control_loops/superstructure/turret/aiming.h"
+#include "aos/network/team_number.h"
#include "frc971/control_loops/pose.h"
#include "gtest/gtest.h"
#include "y2020/constants.h"
@@ -15,6 +16,8 @@
class AimerTest : public ::testing::Test {
public:
+ AimerTest() { aos::network::OverrideTeamNumber(971); }
+
typedef Aimer::Goal Goal;
typedef Aimer::Status Status;
struct StatusData {