Add support in drivetrain for IMU gyro
Change-Id: Id1d63cb2f01ae34d53c620168f85ac5d5b53e665
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;
}
{
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 1330708..3483b60 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -39,7 +39,6 @@
void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
- double last_gyro_heading_ = 0.0;
double last_gyro_rate_ = 0.0;
const DrivetrainConfig dt_config_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 1fd633a..c0c7482 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -21,6 +21,14 @@
CLOSED_LOOP = 1, // Add in closed loop calculation.
};
+enum class GyroType : int32_t {
+ SPARTAN_GYRO = 0, // Use the gyro on the spartan board.
+ IMU_X_GYRO = 1, // Use the x-axis of the gyro on the IMU.
+ IMU_Y_GYRO = 2, // Use the y-axis of the gyro on the IMU.
+ IMU_Z_GYRO = 3, // Use the z-axis of the gyro on the IMU.
+ FLIPPED_SPARTAN_GYRO = 4, // Use the gyro on the spartan board.
+};
+
struct DrivetrainConfig {
// Shifting method we are using.
ShifterType shifter_type;
@@ -28,6 +36,9 @@
// Type of loop to use.
LoopType loop_type;
+ // Type of gyro to use.
+ GyroType gyro_type;
+
// Polydrivetrain functions returning various controller loops with plants.
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 21a968a..403d32c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -38,6 +38,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2012/control_loops/drivetrain/drivetrain_base.cc b/y2012/control_loops/drivetrain/drivetrain_base.cc
index 321962e..8748d4d 100644
--- a/y2012/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2012::control_loops::drivetrain::MakeDrivetrainLoop,
::y2012::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index 4df1a0f..ffb0ee3 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -17,6 +17,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
index 1ebc4e9..afb5375 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2014_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
::y2014_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2015/control_loops/drivetrain/drivetrain_base.cc b/y2015/control_loops/drivetrain/drivetrain_base.cc
index aae8674..26a283b 100644
--- a/y2015/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2015/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2015::control_loops::drivetrain::MakeDrivetrainLoop,
::y2015::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
index 3fe392a..f7c3d2f 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
::y2015_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 1674540..f66d963 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -22,6 +22,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
index 5997342..26a551b 100644
--- a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2016_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
::y2016_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
index 967e8df..54a7321 100644
--- a/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::y2016_bot4::control_loops::drivetrain::MakeDrivetrainLoop,
::y2016_bot4::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.cc b/y2017/control_loops/drivetrain/drivetrain_base.cc
index 0eb506e..1e30e08 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -22,6 +22,7 @@
static DrivetrainConfig kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_Y_GYRO,
::y2017::control_loops::drivetrain::MakeDrivetrainLoop,
::y2017::control_loops::drivetrain::MakeVelocityDrivetrainLoop,