diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e1800d7..180770f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -140,6 +140,7 @@ public void runState() { if (Constants.controller.getBackButtonPressed()) { fieldRelative = !fieldRelative; + Logger.recordOutput("Drive/Field Rel", fieldRelative); } } @@ -188,14 +189,6 @@ public void drive( ); if (headingCorrection) { - // System.out.println(headingCorrection); - // System.out.println("Omgea = 0?" + (omega == 0)); - // System.out.println( - // Math.abs(xSupplier.getAsDouble()) > Constants.Drive.CONTROLLER_DEADBAND - // ); - // System.out.println( - // Math.abs(ySupplier.getAsDouble()) > Constants.Drive.CONTROLLER_DEADBAND - // ); if ( Math.abs(omega) == 0.0 && (Math.abs(xSupplier.getAsDouble()) > Constants.Drive.CONTROLLER_DEADBAND || @@ -206,7 +199,6 @@ public void drive( lastHeadingRadians ) * Constants.Drive.MAX_ANGULAR_SPEED; - // System.out.println("something is happening"); } else { lastHeadingRadians = poseEstimator .getEstimatedPosition() @@ -230,7 +222,6 @@ public void drive( DriverStation.getAlliance().get() == Alliance.Red; if (fieldRelative) { - // TODO: INVERT THE GYRO, MULTIPLYING IT BY 1 IS SO STUPID!!!!!!!!!!!!!!!!!!!!!!!!! drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( linearVelocity.getX() *