Skip to content

Fix getHeading to return Rotation2d with gyro inversion applied #39

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
IanOlds wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix getHeading to return Rotation2d with gyro inversion applied #39

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 7 additions & 8 deletions src/main/java/frc/robot/subsystems/DriveSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ public class DriveSubsystem extends SubsystemBase {
// Odometry class for tracking robot pose
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
DriveConstants.kDriveKinematics,
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
getHeading(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
Expand All @@ -65,7 +65,7 @@ public DriveSubsystem() {
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
getHeading(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
Expand All @@ -90,7 +90,7 @@ public Pose2d getPose() {
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
getHeading(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
Expand All @@ -117,8 +117,7 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ

var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, getHeading())
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
Expand Down Expand Up @@ -168,10 +167,10 @@ public void zeroHeading() {
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from -180 to 180
* @return the robot's heading as a Rotation2d
*/
public double getHeading() {
return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
public Rotation2d getHeading() {
return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0));
}

/**
Expand Down