Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add Gyro::GetRotation2d() (#2555)

Browse Source
This commit is contained in:
Tyler Veness
2020-06-29 19:10:07 -07:00
committed by GitHub
parent d9c7bbd046
commit e50dbe0c43
25 changed files with 130 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
wpilibc/src/main/native/include/frc/interfaces/Gyro.h
View File

@@ -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 <units/units.h>
#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

3
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
View File

@@ -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()));
}

10
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
View File

@@ -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!

4
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -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());
}

10
wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
View File

@@ -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!

20
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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 <frc/geometry/Rotation2d.h>
#include <units/units.h>
#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());
}

4
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
View File

@@ -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<double>(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

6
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -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.

15
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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());
}

4
wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
View File

@@ -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<double>(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

6
wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
View File

@@ -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.

9
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View File

@@ -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());
}

12
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
View File

@@ -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()};
};

23
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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(); }

4
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
View File

@@ -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

4
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -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.

32
wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
View File

@@ -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.
*
* <p>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.
*
* <p>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.
*
* <p>This heading is based on integration of the returned rate from the gyro.
*
@@ -50,10 +51,29 @@ public interface Gyro extends AutoCloseable {
* <p>The rate is based on the most recent reading of the gyro analog value
*
* <p>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}.
*
* <p>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.
*
* <p>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.
*
* <p>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());
}
}

16
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -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());
}
}

17
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -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());
}
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
View File

@@ -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.

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -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();
}
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
View File

@@ -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.

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
View File

@@ -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();
}
}

20
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View File

@@ -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(),

22
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -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();
}
/**