Post GRC cope

.github/scripts/constants.py

@@ -13,7 +13,7 @@
 ]
 
 # Weird stuff that shouldn't go in constants, dont put function/var names in here theyre already checked
-excused_cases = ["ModuleIOSparkMax", "case", "new Module(", "new BaseStatusSignal[", "BaseStatusSignal.waitForAll(", "new ModuleIOHybrid(", "Math.pow(", "+=", "drive.getRotation()", "autoChooser.addOption("]
+excused_cases = ["ModuleIOSparkMax", "case", "new Module(", "new BaseStatusSignal[", "BaseStatusSignal.waitForAll(", "new ModuleIOHybrid(", "Math.pow(", "+=", "drive.getRotation()", "autoChooser.addOption(", "? -1 : 1"]
 
 def check_for_magic_numbers(file_path):
     magic_numbers = []

.vscode/settings.json

@@ -41,5 +41,6 @@
   "java.debug.settings.onBuildFailureProceed": true,
   "[yaml]": {
     "editor.defaultFormatter": "esbenp.prettier-vscode"
-  }
+  },
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

src/main/java/frc/robot/Constants.java

@@ -223,7 +223,7 @@ public static final class Drive {
 		public static final double FAST_TM = 2.0;
 
 		// Auto Config
-		public static final PIDConstants TRANSLATION_PID = new PIDConstants(7.0, 0.0, 0.25);
+		public static final PIDConstants TRANSLATION_PID = new PIDConstants(1.0, 0.0, 0.25);
 		public static final PIDConstants ROTATION_PID = new PIDConstants(4.0, 0.0, 0.4);
 		public static final double MAX_MODULE_SPEED = 6.0;
 

src/main/java/frc/robot/commands/AutoCommands.java

@@ -18,10 +18,6 @@ public Command intaking() {
 		return new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.INTAKING));
 	}
 
-	// public Command shooting() {
-	// 	return new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SHOOTING));
-	// }
-
 	public Command returnToIdle() {
 		return new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.IDLE));
 	}

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -53,6 +53,7 @@ public class Drive extends Subsystem<DriveStates> {
 	private double lastHeadingRadians;
 	private PIDController headingCorrectionController;
 	private boolean headingCorrectionEnabled;
+	private boolean fieldRelativeEnabled = true;
 
 	private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
 	private Rotation2d rawGyroRotation = new Rotation2d();
@@ -82,7 +83,7 @@ public Drive(
 
 		lastHeadingRadians = poseEstimator.getEstimatedPosition().getRotation().getRadians();
 		headingCorrectionEnabled = true;
-		// TODO: Tune
+
 		headingCorrectionController = new PIDController(
 			Constants.Drive.HEADING_CORRECTION_PID.kP,
 			Constants.Drive.HEADING_CORRECTION_PID.kI,
@@ -118,23 +119,27 @@ public void runState() {
 		// Can't run in auto otherwise it will constantly tell drive not to drive in
 		// auto (and thats not
 		// good)
-		// Logger.recordOutput("driveState", getState());
-		// TODO: TURN ON HEADING CORRECTION LATER
 		if (DriverStation.isTeleop() && getState() != DriveStates.AUTO_ALIGN) {
 			drive(
 				this,
-				() -> -Constants.controller.getLeftY(),
-				() -> -Constants.controller.getLeftX(),
+				() -> Constants.controller.getLeftY(),
+				() -> Constants.controller.getLeftX(),
 				() -> Constants.controller.getRightX(),
 				getState().getRotationModifier(),
 				getState().getTranslationModifier(),
-				headingCorrectionEnabled
+				headingCorrectionEnabled,
+				fieldRelativeEnabled
 			);
 		}
 
 		if (Constants.controller.getStartButtonPressed()) {
 			zeroGryo();
 		}
+
+		if (Constants.controller.getBackButtonPressed()) {
+			fieldRelativeEnabled = !fieldRelativeEnabled;
+			Logger.recordOutput("Drive", fieldRelativeEnabled);
+		}
 	}
 
 	// L code??? (taken from AA) good enough
@@ -164,7 +169,8 @@ public void drive(
 		DoubleSupplier omegaSupplier,
 		double rotationMultiplier,
 		double translationMultiplier,
-		boolean headingCorrection
+		boolean headingCorrection,
+		boolean fieldRelative
 	) {
 		// Apply deadband
 		double linearMagnitude = MathUtil.applyDeadband(
@@ -181,14 +187,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 ||
@@ -199,7 +197,6 @@ public void drive(
 					lastHeadingRadians
 				) *
 				Constants.Drive.MAX_ANGULAR_SPEED;
-				// System.out.println("something is happening");
 			} else {
 				lastHeadingRadians = poseEstimator
 					.getEstimatedPosition()
@@ -222,18 +219,28 @@ public void drive(
 			DriverStation.getAlliance().isPresent() &&
 			DriverStation.getAlliance().get() == Alliance.Red;
 		drive.runVelocity(
-			ChassisSpeeds.fromFieldRelativeSpeeds(
-				linearVelocity.getX() *
-				drive.getMaxLinearSpeedMetersPerSec() *
-				translationMultiplier,
-				linearVelocity.getY() *
-				drive.getMaxLinearSpeedMetersPerSec() *
-				translationMultiplier,
-				omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier,
-				(isFlipped
-						? drive.getRotation().plus(new Rotation2d(Math.PI))
-						: drive.getRotation()).times(-1)
-			)
+			fieldRelative
+				? ChassisSpeeds.fromFieldRelativeSpeeds(
+					linearVelocity.getX() *
+					drive.getMaxLinearSpeedMetersPerSec() *
+					translationMultiplier,
+					linearVelocity.getY() *
+					drive.getMaxLinearSpeedMetersPerSec() *
+					translationMultiplier,
+					omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier,
+					(isFlipped
+							? drive.getRotation().plus(new Rotation2d(Math.PI))
+							: drive.getRotation()).times(-1)
+				)
+				: new ChassisSpeeds(
+					linearVelocity.getX() *
+					drive.getMaxLinearSpeedMetersPerSec() *
+					translationMultiplier,
+					linearVelocity.getY() *
+					drive.getMaxLinearSpeedMetersPerSec() *
+					translationMultiplier,
+					omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier
+				)
 		);
 	}
 
@@ -263,6 +270,7 @@ public void periodic() {
 				module.stop();
 			}
 		}
+
 		// Log empty setpoint states when disabled
 		if (DriverStation.isDisabled()) {
 			Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});

src/main/java/frc/robot/subsystems/drive/GyroIONavx2.java

@@ -2,6 +2,7 @@
 
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.SerialPort;
 import java.util.OptionalDouble;
@@ -14,15 +15,18 @@ public class GyroIONavx2 implements GyroIO {
 	private final Queue<Double> yawPositionQueue;
 	private final Queue<Double> yawTimestampQueue;
 
+	private Rotation3d offset;
+
 	public GyroIONavx2() {
 		navx = new AHRS(SerialPort.Port.kUSB1);
-		navx.enableBoardlevelYawReset(true);
 		navx.reset();
+		offset = new Rotation3d();
+
 		yawTimestampQueue = HybridOdometryThread.getInstance().makeTimestampQueue();
 		yawPositionQueue = HybridOdometryThread.getInstance()
 			.registerSignal(() -> {
 				if (navx.isConnected()) {
-					return OptionalDouble.of(navx.getYaw());
+					return OptionalDouble.of(navx.getRotation3d().minus(offset).getAngle());
 				} else {
 					return OptionalDouble.empty();
 				}
@@ -31,18 +35,15 @@ public GyroIONavx2() {
 
 	@Override
 	public void zero() {
-		System.out.println("ahhaha");
-		navx.reset();
-		navx.zeroYaw();
-		System.out.println(navx.getYaw());
+		this.offset = navx.getRotation3d();
 	}
 
 	@Override
 	public void updateInputs(GyroIOInputs inputs) {
 		inputs.connected = navx.isConnected();
-		inputs.yawPosition = Rotation2d.fromDegrees(navx.getYaw());
+		inputs.yawPosition = Rotation2d.fromRadians(navx.getRotation3d().minus(offset).getAngle());
 		inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRate());
-		inputs.yawPosDeg = navx.getYaw();
+		inputs.yawPosDeg = Units.radiansToDegrees(navx.getRotation3d().minus(offset).getAngle());
 
 		if (yawTimestampQueue != null && yawPositionQueue != null) {
 			inputs.odometryYawTimestamps = yawTimestampQueue

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -247,18 +247,6 @@ public SwerveModuleState runSetpoint(SwerveModuleState state) {
 		return optimizedState;
 	}
 
-	/**
-	 * Runs the module with the specified voltage while controlling to zero degrees.
-	 */
-	public void runCharacterization(double volts) {
-		// Closed loop turn control
-		angleSetpoint = new Rotation2d();
-
-		// Open loop drive control
-		io.setDriveVoltage(volts);
-		speedSetpoint = null;
-	}
-
 	/** Disables all outputs to motors. */
 	public void stop() {
 		io.setTurnVoltage(0.0);
@@ -313,9 +301,4 @@ public SwerveModulePosition[] getOdometryPositions() {
 	public double[] getOdometryTimestamps() {
 		return inputs.odometryTimestamps;
 	}
-
-	/** Returns the drive velocity in radians/sec. */
-	public double getCharacterizationVelocity() {
-		return inputs.driveVelocityRadPerSec;
-	}
 }