From e50dbe0c435c955e4e8ebeb25977e07896dab649 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 29 Jun 2020 19:10:07 -0700 Subject: [PATCH] [wpilib] Add Gyro::GetRotation2d() (#2555) --- .../main/native/include/frc/interfaces/Gyro.h | 33 +++++++++++++++---- .../DifferentialDriveBot/cpp/Drivetrain.cpp | 3 +- .../DifferentialDriveBot/include/Drivetrain.h | 10 +----- .../examples/MecanumBot/cpp/Drivetrain.cpp | 4 +-- .../examples/MecanumBot/include/Drivetrain.h | 10 +----- .../cpp/subsystems/DriveSubsystem.cpp | 20 ++++------- .../include/Constants.h | 4 +-- .../include/subsystems/DriveSubsystem.h | 6 ++-- .../cpp/subsystems/DriveSubsystem.cpp | 15 ++++----- .../RamseteCommand/include/Constants.h | 4 +-- .../include/subsystems/DriveSubsystem.h | 6 ++-- .../cpp/examples/SwerveBot/cpp/Drivetrain.cpp | 9 ++--- .../examples/SwerveBot/include/Drivetrain.h | 12 ++----- .../cpp/subsystems/DriveSubsystem.cpp | 23 +++++-------- .../include/Constants.h | 4 +-- .../include/subsystems/DriveSubsystem.h | 4 +-- .../wpi/first/wpilibj/interfaces/Gyro.java | 32 ++++++++++++++---- .../differentialdrivebot/Drivetrain.java | 16 ++------- .../examples/mecanumbot/Drivetrain.java | 17 ++-------- .../mecanumcontrollercommand/Constants.java | 4 +-- .../subsystems/DriveSubsystem.java | 25 ++++---------- .../examples/ramsetecommand/Constants.java | 4 +-- .../subsystems/DriveSubsystem.java | 11 +++---- .../examples/swervebot/Drivetrain.java | 20 +++-------- .../subsystems/DriveSubsystem.java | 22 ++++--------- 25 files changed, 130 insertions(+), 188 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h index b5aa332f59..3fe9b83d54 100644 --- a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h +++ b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2014-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,6 +7,10 @@ #pragma once +#include + +#include "frc/geometry/Rotation2d.h" + namespace frc { /** @@ -38,7 +42,7 @@ class Gyro { virtual void Reset() = 0; /** - * Return the actual angle in degrees that the robot is currently facing. + * Return the heading of the robot in degrees. * * The angle is based on the current accumulator value corrected by the * oversampling rate, the gyro type and the A/D calibration values. The angle @@ -47,8 +51,7 @@ class Gyro { * output as it sweeps past from 360 to 0 on the second time around. * * The angle is expected to increase as the gyro turns clockwise when looked - * at from the top. It needs to follow NED axis conventions in order to work - * properly with dependent control loops. + * at from the top. It needs to follow the NED axis convention. * * @return the current heading of the robot in degrees. This heading is based * on integration of the returned rate from the gyro. @@ -61,12 +64,30 @@ class Gyro { * The rate is based on the most recent reading of the gyro analog value. * * The rate is expected to be positive as the gyro turns clockwise when looked - * at from the top. It needs to follow NED axis conventions in order to work - * properly with dependent control loops. + * at from the top. It needs to follow the NED axis convention. * * @return the current rate in degrees per second */ virtual double GetRate() const = 0; + + /** + * Return the heading of the robot as a Rotation2d. + * + * The angle is based on the current accumulator value corrected by the + * oversampling rate, the gyro type and the A/D calibration values. The angle + * is continuous, that is it will continue from 360 to 361 degrees. This + * allows algorithms that wouldn't want to see a discontinuity in the gyro + * output as it sweeps past from 360 to 0 on the second time around. + * + * The angle is expected to increase as the gyro turns counterclockwise when + * looked at from the top. It needs to follow the NWU axis convention. + * + * @return the current heading of the robot as a Rotation2d. This heading is + * based on integration of the returned rate from the gyro. + */ + virtual Rotation2d GetRotation2d() const { + return Rotation2d{units::degree_t{-GetAngle()}}; + } }; } // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp index c62f741e3c..c639539f6c 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp @@ -25,6 +25,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, } void Drivetrain::UpdateOdometry() { - m_odometry.Update(GetAngle(), units::meter_t(m_leftEncoder.GetDistance()), + m_odometry.Update(m_gyro.GetRotation2d(), + units::meter_t(m_leftEncoder.GetDistance()), units::meter_t(m_rightEncoder.GetDistance())); } diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h index f601689e05..f2106b00f3 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h @@ -37,14 +37,6 @@ class Drivetrain { m_rightEncoder.Reset(); } - /** - * Get the robot angle as a Rotation2d. - */ - frc::Rotation2d GetAngle() const { - // Negating the angle because WPILib Gyros are CW positive. - return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle())); - } - static constexpr units::meters_per_second_t kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ @@ -77,7 +69,7 @@ class Drivetrain { frc::AnalogGyro m_gyro{0}; frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; - frc::DifferentialDriveOdometry m_odometry{GetAngle()}; + frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()}; // Gains are for example purposes only - must be determined for your own // robot! diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp index 79011111a4..b65b1d782e 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -48,12 +48,12 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, units::radians_per_second_t rot, bool fieldRelative) { auto wheelSpeeds = m_kinematics.ToWheelSpeeds( fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, GetAngle()) + xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); wheelSpeeds.Normalize(kMaxSpeed); SetSpeeds(wheelSpeeds); } void Drivetrain::UpdateOdometry() { - m_odometry.Update(GetAngle(), GetCurrentState()); + m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentState()); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h index 41bfb80f5f..9c5d9e4938 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -25,14 +25,6 @@ class Drivetrain { public: Drivetrain() { m_gyro.Reset(); } - /** - * Get the robot angle as a Rotation2d - */ - frc::Rotation2d GetAngle() const { - // Negating the angle because WPILib Gyros are CW positive. - return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle())); - } - frc::MecanumDriveWheelSpeeds GetCurrentState() const; void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds); void Drive(units::meters_per_second_t xSpeed, @@ -72,7 +64,7 @@ class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation}; - frc::MecanumDriveOdometry m_odometry{m_kinematics, GetAngle()}; + frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d()}; // Gains are for example purposes only - must be determined for your own // robot! diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp index b758b0755e..3651cae36a 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,7 +7,6 @@ #include "subsystems/DriveSubsystem.h" -#include #include #include "Constants.h" @@ -30,9 +29,7 @@ DriveSubsystem::DriveSubsystem() m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1], kRearRightEncoderReversed}, - m_odometry{kDriveKinematics, - frc::Rotation2d(units::degree_t(GetHeading())), - frc::Pose2d()} { + m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d()} { // Set the distance per pulse for the encoders m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); @@ -43,7 +40,7 @@ DriveSubsystem::DriveSubsystem() void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. m_odometry.Update( - frc::Rotation2d(units::degree_t(GetHeading())), + m_gyro.GetRotation2d(), frc::MecanumDriveWheelSpeeds{ units::meters_per_second_t(m_frontLeftEncoder.GetRate()), units::meters_per_second_t(m_rearLeftEncoder.GetRate()), @@ -103,19 +100,16 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) { m_drive.SetMaxOutput(maxOutput); } -double DriveSubsystem::GetHeading() { - return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.); +units::degree_t DriveSubsystem::GetHeading() const { + return m_gyro.GetRotation2d().Degrees(); } void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); } -double DriveSubsystem::GetTurnRate() { - return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.); -} +double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); } frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { - m_odometry.ResetPosition(pose, - frc::Rotation2d(units::degree_t(GetHeading()))); + m_odometry.ResetPosition(pose, m_gyro.GetRotation2d()); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h index 1baa3de0ef..8ca9aade7d 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -50,8 +50,6 @@ constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts (kWheelDiameterMeters * wpi::math::pi) / static_cast(kEncoderCPR); -constexpr bool kGyroReversed = false; - // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or // theoretically for *your* robot's drive. The RobotPy Characterization diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h index fb3c19dd7d..1cd72d229b 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -103,9 +103,9 @@ class DriveSubsystem : public frc2::SubsystemBase { /** * Returns the heading of the robot. * - * @return the robot's heading in degrees, from 180 to 180 + * @return the robot's heading in degrees, from -180 to 180 */ - double GetHeading(); + units::degree_t GetHeading() const; /** * Zeroes the heading of the robot. diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp index 4a23397e4a..a82d22d9a7 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp @@ -19,7 +19,7 @@ DriveSubsystem::DriveSubsystem() m_right2{kRightMotor2Port}, m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]}, m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]}, - m_odometry{frc::Rotation2d(units::degree_t(GetHeading()))} { + m_odometry{m_gyro.GetRotation2d()} { // Set the distance per pulse for the encoders m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); @@ -29,7 +29,7 @@ DriveSubsystem::DriveSubsystem() void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. - m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())), + m_odometry.Update(m_gyro.GetRotation2d(), units::meter_t(m_leftEncoder.GetDistance()), units::meter_t(m_rightEncoder.GetDistance())); } @@ -61,13 +61,11 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) { m_drive.SetMaxOutput(maxOutput); } -double DriveSubsystem::GetHeading() { - return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1.0 : 1.0); +units::degree_t DriveSubsystem::GetHeading() const { + return m_gyro.GetRotation2d().Degrees(); } -double DriveSubsystem::GetTurnRate() { - return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0); -} +double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); } frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } @@ -78,6 +76,5 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() { void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { ResetEncoders(); - m_odometry.ResetPosition(pose, - frc::Rotation2d(units::degree_t(GetHeading()))); + m_odometry.ResetPosition(pose, m_gyro.GetRotation2d()); } diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h index 649082b852..ad9930d19c 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -41,8 +41,6 @@ constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); -constexpr bool kGyroReversed = true; - // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or // theoretically for *your* robot's drive. The Robot Characterization diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h index d8d1abd8af..2fb211e546 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -83,9 +83,9 @@ class DriveSubsystem : public frc2::SubsystemBase { /** * Returns the heading of the robot. * - * @return the robot's heading in degrees, from 180 to 180 + * @return the robot's heading in degrees, from -180 to 180 */ - double GetHeading(); + units::degree_t GetHeading() const; /** * Returns the turn rate of the robot. diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp index 416d3032a3..fcc0bcd67d 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -12,7 +12,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, units::radians_per_second_t rot, bool fieldRelative) { auto states = m_kinematics.ToSwerveModuleStates( fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, GetAngle()) + xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed); @@ -26,6 +26,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, } void Drivetrain::UpdateOdometry() { - m_odometry.Update(GetAngle(), m_frontLeft.GetState(), m_frontRight.GetState(), - m_backLeft.GetState(), m_backRight.GetState()); + m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(), + m_frontRight.GetState(), m_backLeft.GetState(), + m_backRight.GetState()); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h index 2de6e956d7..fed7adab36 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -22,14 +22,6 @@ class Drivetrain { public: Drivetrain() { m_gyro.Reset(); } - /** - * Get the robot angle as a Rotation2d. - */ - frc::Rotation2d GetAngle() const { - // Negating the angle because WPILib Gyros are CW positive. - return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle())); - } - void Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative); @@ -57,5 +49,5 @@ class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation}; - frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, GetAngle()}; + frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, m_gyro.GetRotation2d()}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp index f5d05ceb1d..06a5b58523 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -37,15 +37,13 @@ DriveSubsystem::DriveSubsystem() kRearRightDriveEncoderPorts, kRearRightTurningEncoderPorts, kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed}, - m_odometry{kDriveKinematics, - frc::Rotation2d(units::degree_t(GetHeading())), - frc::Pose2d()} {} + m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d()} {} void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. - m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())), - m_frontLeft.GetState(), m_rearLeft.GetState(), - m_frontRight.GetState(), m_rearRight.GetState()); + m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(), + m_rearLeft.GetState(), m_frontRight.GetState(), + m_rearRight.GetState()); } void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, @@ -54,8 +52,7 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, bool fieldRelative) { auto states = kDriveKinematics.ToSwerveModuleStates( fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, - frc::Rotation2d(units::degree_t(GetHeading()))) + xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); kDriveKinematics.NormalizeWheelSpeeds(&states, AutoConstants::kMaxSpeed); @@ -85,15 +82,13 @@ void DriveSubsystem::ResetEncoders() { m_rearRight.ResetEncoders(); } -double DriveSubsystem::GetHeading() { - return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.); +units::degree_t DriveSubsystem::GetHeading() const { + return m_gyro.GetRotation2d().Degrees(); } void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); } -double DriveSubsystem::GetTurnRate() { - return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.); -} +double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); } frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h index 51b7030156..237ad5145c 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -53,8 +53,6 @@ constexpr bool kRearLeftDriveEncoderReversed = true; constexpr bool kFrontRightDriveEncoderReversed = false; constexpr bool kRearRightDriveEncoderReversed = true; -constexpr bool kGyroReversed = false; - // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or // theoretically for *your* robot's drive. The RobotPy Characterization diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h index db4623f8aa..562af50649 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -63,7 +63,7 @@ class DriveSubsystem : public frc2::SubsystemBase { * * @return the robot's heading in degrees, from 180 to 180 */ - double GetHeading(); + units::degree_t GetHeading() const; /** * Zeroes the heading of the robot. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java index a21e6d57ce..c62c29523b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2014-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.interfaces; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + /** * Interface for yaw rate gyros. */ @@ -27,7 +29,7 @@ public interface Gyro extends AutoCloseable { void reset(); /** - * Return the actual angle in degrees that the robot is currently facing. + * Return the heading of the robot in degrees. * *

The angle is based on the current accumulator value corrected by the oversampling rate, the * gyro type and the A/D calibration values. The angle is continuous, that is it will continue @@ -35,8 +37,7 @@ public interface Gyro extends AutoCloseable { * the gyro output as it sweeps past from 360 to 0 on the second time around. * *

The angle is expected to increase as the gyro turns clockwise when looked - * at from the top. It needs to follow NED axis conventions in order to work - * properly with dependent control loops. + * at from the top. It needs to follow the NED axis convention. * *

This heading is based on integration of the returned rate from the gyro. * @@ -50,10 +51,29 @@ public interface Gyro extends AutoCloseable { *

The rate is based on the most recent reading of the gyro analog value * *

The rate is expected to be positive as the gyro turns clockwise when looked - * at from the top. It needs to follow NED axis conventions in order to work - * properly with dependent control loops. + * at from the top. It needs to follow the NED axis convention. * * @return the current rate in degrees per second */ double getRate(); + + /** + * Return the heading of the robot as a {@link edu.wpi.first.wpilibj.geometry.Rotation2d}. + * + *

The angle is based on the current accumulator value corrected by the oversampling rate, the + * gyro type and the A/D calibration values. The angle is continuous, that is it will continue + * from 360 to 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in + * the gyro output as it sweeps past from 360 to 0 on the second time around. + * + *

The angle is expected to increase as the gyro turns counterclockwise + * when looked at from the top. It needs to follow the NWU axis convention. + * + *

This heading is based on integration of the returned rate from the gyro. + * + * @return the current heading of the robot as a + * {@link edu.wpi.first.wpilibj.geometry.Rotation2d}. + */ + default Rotation2d getRotation2d() { + return new Rotation2d(-getAngle()); + } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java index 1be450cdc5..b9c69a83d6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; @@ -73,17 +72,7 @@ public class Drivetrain { m_leftEncoder.reset(); m_rightEncoder.reset(); - m_odometry = new DifferentialDriveOdometry(getAngle()); - } - - /** - * Returns the angle of the robot as a Rotation2d. - * - * @return The angle of the robot. - */ - public Rotation2d getAngle() { - // Negating the angle because WPILib gyros are CW positive. - return Rotation2d.fromDegrees(-m_gyro.getAngle()); + m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d()); } /** @@ -119,6 +108,7 @@ public class Drivetrain { * Updates the field-relative position. */ public void updateOdometry() { - m_odometry.update(getAngle(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); + m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(), + m_rightEncoder.getDistance()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index 220fb87219..2a081ff19a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; @@ -55,7 +54,7 @@ public class Drivetrain { ); private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics, - getAngle()); + m_gyro.getRotation2d()); // Gains are for example purposes only - must be determined for your own robot! private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); @@ -67,16 +66,6 @@ public class Drivetrain { m_gyro.reset(); } - /** - * Returns the angle of the robot as a Rotation2d. - * - * @return The angle of the robot. - */ - public Rotation2d getAngle() { - // Negating the angle because WPILib gyros are CW positive. - return Rotation2d.fromDegrees(-m_gyro.getAngle()); - } - /** * Returns the current state of the drivetrain. * @@ -133,7 +122,7 @@ public class Drivetrain { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, getAngle() + xSpeed, ySpeed, rot, m_gyro.getRotation2d() ) : new ChassisSpeeds(xSpeed, ySpeed, rot) ); mecanumDriveWheelSpeeds.normalize(kMaxSpeed); @@ -144,6 +133,6 @@ public class Drivetrain { * Updates the field relative position of the robot. */ public void updateOdometry() { - m_odometry.update(getAngle(), getCurrentState()); + m_odometry.update(m_gyro.getRotation2d(), getCurrentState()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java index 88f904b62e..b402109095 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -55,8 +55,6 @@ public final class Constants { // Assumes the encoders are directly mounted on the wheel shafts (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; - public static final boolean kGyroReversed = false; - // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or theoretically // for *your* robot's drive. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java index 7aaf114ab3..1fac67ef9f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.drive.MecanumDrive; import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages; import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry; @@ -62,7 +61,7 @@ public class DriveSubsystem extends SubsystemBase { // Odometry class for tracking robot pose MecanumDriveOdometry m_odometry = - new MecanumDriveOdometry(DriveConstants.kDriveKinematics, getAngle()); + new MecanumDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d()); /** * Creates a new DriveSubsystem. @@ -75,20 +74,10 @@ public class DriveSubsystem extends SubsystemBase { m_rearRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); } - /** - * Returns the angle of the robot as a Rotation2d. - * - * @return The angle of the robot. - */ - public Rotation2d getAngle() { - // Negating the angle because WPILib gyros are CW positive. - return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? 1.0 : -1.0)); - } - @Override public void periodic() { // Update the odometry in the periodic block - m_odometry.update(getAngle(), + m_odometry.update(m_gyro.getRotation2d(), new MecanumDriveWheelSpeeds( m_frontLeftEncoder.getRate(), m_rearLeftEncoder.getRate(), @@ -111,7 +100,7 @@ public class DriveSubsystem extends SubsystemBase { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition(pose, getAngle()); + m_odometry.resetPosition(pose, m_gyro.getRotation2d()); } /** @@ -227,10 +216,10 @@ public class DriveSubsystem extends SubsystemBase { /** * Returns the heading of the robot. * - * @return the robot's heading in degrees, from 180 to 180 + * @return the robot's heading in degrees, from -180 to 180 */ public double getHeading() { - return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); + return m_gyro.getRotation2d().getDegrees(); } /** @@ -239,6 +228,6 @@ public class DriveSubsystem extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0); + return -m_gyro.getRate(); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java index f70a00301c..49044a2367 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -39,8 +39,6 @@ public final class Constants { // Assumes the encoders are directly mounted on the wheel shafts (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; - public static final boolean kGyroReversed = true; - // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or theoretically // for *your* robot's drive. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java index 03ffbbe56c..396d5df79c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; @@ -60,13 +59,13 @@ public class DriveSubsystem extends SubsystemBase { m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); resetEncoders(); - m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading())); + m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d()); } @Override public void periodic() { // Update the odometry in the periodic block - m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(), + m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); } @@ -95,7 +94,7 @@ public class DriveSubsystem extends SubsystemBase { */ public void resetOdometry(Pose2d pose) { resetEncoders(); - m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); + m_odometry.resetPosition(pose, m_gyro.getRotation2d()); } /** @@ -177,7 +176,7 @@ public class DriveSubsystem extends SubsystemBase { * @return the robot's heading in degrees, from -180 to 180 */ public double getHeading() { - return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); + return m_gyro.getRotation2d().getDegrees(); } /** @@ -186,6 +185,6 @@ public class DriveSubsystem extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0); + return -m_gyro.getRate(); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 828a94b927..5f1227af9e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -8,7 +8,6 @@ package edu.wpi.first.wpilibj.examples.swervebot; import edu.wpi.first.wpilibj.AnalogGyro; -import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; @@ -37,22 +36,13 @@ public class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation ); - private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, getAngle()); + private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, + m_gyro.getRotation2d()); public Drivetrain() { m_gyro.reset(); } - /** - * Returns the angle of the robot as a Rotation2d. - * - * @return The angle of the robot. - */ - public Rotation2d getAngle() { - // Negating the angle because WPILib gyros are CW positive. - return Rotation2d.fromDegrees(-m_gyro.getAngle()); - } - /** * Method to drive the robot using joystick info. * @@ -65,7 +55,7 @@ public class Drivetrain { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { var swerveModuleStates = m_kinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, getAngle()) + xSpeed, ySpeed, rot, m_gyro.getRotation2d()) : new ChassisSpeeds(xSpeed, ySpeed, rot) ); SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed); @@ -80,7 +70,7 @@ public class Drivetrain { */ public void updateOdometry() { m_odometry.update( - getAngle(), + m_gyro.getRotation2d(), m_frontLeft.getState(), m_frontRight.getState(), m_backLeft.getState(), diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 5d9cf56357..2baeec41f0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -60,7 +60,7 @@ public class DriveSubsystem extends SubsystemBase { // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = - new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getAngle()); + new SwerveDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d()); /** * Creates a new DriveSubsystem. @@ -68,16 +68,6 @@ public class DriveSubsystem extends SubsystemBase { public DriveSubsystem() { } - /** - * Returns the angle of the robot as a Rotation2d. - * - * @return The angle of the robot. - */ - public Rotation2d getAngle() { - // Negating the angle because WPILib gyros are CW positive. - return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? 1.0 : -1.0)); - } - @Override public void periodic() { // Update the odometry in the periodic block @@ -104,7 +94,7 @@ public class DriveSubsystem extends SubsystemBase { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition(pose, getAngle()); + m_odometry.resetPosition(pose, m_gyro.getRotation2d()); } /** @@ -119,7 +109,7 @@ public class DriveSubsystem extends SubsystemBase { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, getAngle()) + xSpeed, ySpeed, rot, m_gyro.getRotation2d()) : new ChassisSpeeds(xSpeed, ySpeed, rot) ); SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, @@ -164,10 +154,10 @@ public class DriveSubsystem extends SubsystemBase { /** * Returns the heading of the robot. * - * @return the robot's heading in degrees, from 180 to 180 + * @return the robot's heading in degrees, from -180 to 180 */ public double getHeading() { - return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); + return m_gyro.getRotation2d().getDegrees(); } /**