Skip to content

Fix for Inverted Gyros mainly NavX #14

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
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Fix for Inverted Gyros mainly NavX #14

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
487a94a
Fix Gyro Inversion for other Gyros
Technologyman00 Nov 25, 2023
a9cda1b
Removed Unused Variables
Technologyman00 Nov 25, 2023
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
12 changes: 7 additions & 5 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 @@ -14,6 +14,7 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
import frc.robot.Constants;
import frc.robot.Constants.DriveConstants;
import frc.utils.SwerveUtils;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
Expand Down Expand Up @@ -52,10 +53,11 @@ public class DriveSubsystem extends SubsystemBase {
private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DriveConstants.kRotationalSlewRate);
private double m_prevTime = WPIUtilJNI.now() * 1e-6;


// Odometry class for tracking robot pose
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
DriveConstants.kDriveKinematics,
Rotation2d.fromDegrees(m_gyro.getAngle()),
Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? -1.0 : 1.0)),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
Expand All @@ -71,7 +73,7 @@ public DriveSubsystem() {
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
Rotation2d.fromDegrees(m_gyro.getAngle()),
Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? -1.0 : 1.0)),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
Expand All @@ -96,7 +98,7 @@ public Pose2d getPose() {
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(
Rotation2d.fromDegrees(m_gyro.getAngle()),
Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? -1.0 : 1.0)),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
Expand Down Expand Up @@ -176,7 +178,7 @@ else if (angleDif > 0.85*Math.PI) {

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

/**
Expand Down