From c08506dd5e424be48621089fe5cd716ffbc27d36 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 25 Jul 2024 11:41:41 -0600 Subject: [PATCH] Make swerve drive work. Some stuff on drift correction --- src/main/java/frc4388/robot/Constants.java | 32 ++++++------- .../java/frc4388/robot/RobotContainer.java | 3 +- src/main/java/frc4388/robot/RobotMap.java | 48 +++++++++---------- .../frc4388/robot/subsystems/SwerveDrive.java | 32 +++++++++---- .../robot/subsystems/SwerveModule.java | 40 ++++++++++++++-- src/main/java/frc4388/utility/RobotGyro.java | 4 +- 6 files changed, 103 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 296c8d4..aee6cae 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -40,28 +40,28 @@ public final class Constants { public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 220; - public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; - public static final double BACK_LEFT_ROT_OFFSET = -279.08349884; - public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656; + public static final double FRONT_LEFT_ROT_OFFSET = -0.130126953125 + 0.25 + 0.25; + public static final double FRONT_RIGHT_ROT_OFFSET = 0.364990234375 + 0.25;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -0.4775390625 + 0.25; + public static final double BACK_RIGHT_ROT_OFFSET = 0.356201171875 + 0.25; } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 3; - public static final int RIGHT_FRONT_STEER_ID = 2; - public static final int RIGHT_FRONT_ENCODER_ID = 12; + public static final int RIGHT_FRONT_WHEEL_ID = 2; + public static final int RIGHT_FRONT_STEER_ID = 3; + public static final int RIGHT_FRONT_ENCODER_ID = 10; - // public static final int LEFT_FRONT_WHEEL_ID = 4; - // public static final int LEFT_FRONT_STEER_ID = 5; - // public static final int LEFT_FRONT_ENCODER_ID = 11; + public static final int LEFT_FRONT_WHEEL_ID = 4; + public static final int LEFT_FRONT_STEER_ID = 5; + public static final int LEFT_FRONT_ENCODER_ID = 11; - // public static final int LEFT_BACK_WHEEL_ID = 6; - // public static final int LEFT_BACK_STEER_ID = 7; - // public static final int LEFT_BACK_ENCODER_ID = 12; + public static final int LEFT_BACK_WHEEL_ID = 6; + public static final int LEFT_BACK_STEER_ID = 7; + public static final int LEFT_BACK_ENCODER_ID = 12; - // public static final int RIGHT_BACK_WHEEL_ID = 8; - // public static final int RIGHT_BACK_STEER_ID = 9; - // public static final int RIGHT_BACK_ENCODER_ID = 13; + public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_STEER_ID = 9; + public static final int RIGHT_BACK_ENCODER_ID = 13; } public static final class PIDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c671420..8ccef78 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -179,7 +179,8 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index c6f0224..9595f48 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -48,27 +48,27 @@ public class RobotMap { // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + // public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + // public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + + /* Swreve Drive Subsystem */ + public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); + public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); + public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + + public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - - /* Swreve Drive Subsystem */ - // public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); - // public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); - // public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - - - // public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); - // public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); - // public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - - // public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); - // public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); - // public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - - // public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); - // public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); - // public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + + public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); + public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); + public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + + public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); + public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); + public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); /* Shooter Subsystem */ // public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); @@ -104,7 +104,7 @@ public class RobotMap { // rightBackSteer.configFactoryDefault(); // // config open loop ramp - // leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); @@ -117,7 +117,7 @@ public class RobotMap { // rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // // config closed loop ramp - // leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); @@ -139,7 +139,7 @@ public class RobotMap { // leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // // set neutral mode @@ -156,9 +156,9 @@ public class RobotMap { // // initialize SwerveModules this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); - this.leftFront = this.rightFront; - this.leftBack = this.rightFront; - this.rightBack = this.rightFront; + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); // this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 40b30e5..45f68f7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -10,10 +10,12 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; +import frc4388.utility.RobotUnits; public class SwerveDrive extends SubsystemBase { @@ -68,6 +70,9 @@ public class SwerveDrive extends SubsystemBase { boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 1.0; + SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); + if (fieldRelative) { double rot = 0; @@ -75,7 +80,8 @@ public class SwerveDrive extends SubsystemBase { // ! drift correction if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); - rot = rightStick.getX(); + rot_correction = 0; + // rot = rightStick.getX(); // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { @@ -86,7 +92,7 @@ public class SwerveDrive extends SubsystemBase { // SmartDashboard.putBoolean("drift correction", true); - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } @@ -95,7 +101,8 @@ public class SwerveDrive extends SubsystemBase { // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1)); + // chassisSpeeds = chassisSpeeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -120,8 +127,8 @@ public class SwerveDrive extends SubsystemBase { } // SmartDashboard.putBoolean("drift correction", true); + // double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } @@ -137,9 +144,15 @@ public class SwerveDrive extends SubsystemBase { // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } - public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - Translation2d rightStick = new Translation2d(-rightX, rightY); + // Translation2d rightStick = new Translation2d(-rightX, rightY); + double rightX = rightStick.getX(); + double rightY = rightStick.getY(); + + double rot_correction = 0; + + // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; if(fieldRelative) { double rot = 0; @@ -157,7 +170,7 @@ public class SwerveDrive extends SubsystemBase { Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -191,7 +204,7 @@ public class SwerveDrive extends SubsystemBase { } public double getGyroAngle() { - return gyro.getAngle(); + return -gyro.getAngle(); } public void add180() { @@ -238,7 +251,8 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run\ - // SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("RotTartget", rotTarget); } public void shiftDown() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 0bd2649..8042080 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -17,7 +17,10 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; @@ -26,6 +29,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.hardware.CANcoder; @@ -58,7 +62,34 @@ public class SwerveModule extends SubsystemBase { this.angleMotor = angleMotor; this.encoder = encoder; - TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + var motorCfg = new TalonFXConfiguration() + .withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ); + + driveMotor.getConfigurator().apply(motorCfg); + + TalonFXConfiguration angleConfig = new TalonFXConfiguration() + .withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ); + angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; angleConfig.Slot0.kP = swerveGains.kP; @@ -71,6 +102,7 @@ public class SwerveModule extends SubsystemBase { CANcoderConfiguration canconfig = new CANcoderConfiguration(); canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + canconfig.MagnetSensor.MagnetOffset = offset; encoder.getConfigurator().apply(canconfig); rotateToAngle(0); @@ -127,17 +159,17 @@ public class SwerveModule extends SubsystemBase { public double getAngularVel() { // return this.angleMotor.getSelectedSensorVelocity(); - return 0; + return angleMotor.getVelocity().getValueAsDouble(); } public double getDrivePos() { // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; - return 0; + return driveMotor.getPosition().getValueAsDouble(); } public double getDriveVel() { // return this.driveMotor.getSelectedSensorVelocity(0); - return 0.0; + return driveMotor.getVelocity().getValueAsDouble(); } public void stop() { diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 2f966d7..e1fed56 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -186,7 +186,7 @@ public class RobotGyro implements Gyro { m_pigeon.getAngle(); var rotation = m_pigeon.getRotation3d(); - return new double[] {rotation.getX(), (rotation.getY() - pitchZero), (rotation.getZ() - rollZero)}; + return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())}; } @Override @@ -197,7 +197,7 @@ public class RobotGyro implements Gyro { @Override public double getAngle() { if (m_isGyroAPigeon) { - return getPigeonAngles()[0]; + return getPigeonAngles()[2]; } else { return m_navX.getAngle(); }