diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 67ffff1..0b47efe 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -34,6 +34,7 @@ import edu.wpi.first.units.Measure; import edu.wpi.first.units.Velocity; import edu.wpi.first.units.Voltage; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; @@ -78,12 +79,23 @@ public class Drive extends SubsystemBase { private boolean overrideDriveAutoAim = false; + private boolean manualOverideDriveAutoAim = false; + public boolean isOverrideDriveAutoAim() { - return overrideDriveAutoAim; + return overrideDriveAutoAim || manualOverideDriveAutoAim; } public void setOverrideDriveAutoAim(boolean overrideDriveAutoAim) { - this.overrideDriveAutoAim = overrideDriveAutoAim; + this.overrideDriveAutoAim = overrideDriveAutoAim || manualOverideDriveAutoAim; + } + + public boolean isManualOverideDriveAutoAim() { + return manualOverideDriveAutoAim; + } + + public Sendable setManualOverideDriveAutoAim(boolean manualOverideDriveAutoAim) { + this.manualOverideDriveAutoAim = manualOverideDriveAutoAim; + return null; } private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); diff --git a/src/main/java/frc/robot/util/Dashboard.java b/src/main/java/frc/robot/util/Dashboard.java index 63c4ee4..6ed0ec0 100644 --- a/src/main/java/frc/robot/util/Dashboard.java +++ b/src/main/java/frc/robot/util/Dashboard.java @@ -42,6 +42,7 @@ public Dashboard( builder.addDoubleProperty("Back Right Angle", () -> drive.shuffleboardMethod()[3].getAngle().getRadians(), null); builder.addDoubleProperty("Robot Angle", () -> drive.getRotation().getRadians(), null); }); + SmartDashboard.putData("Override Auto Aim", drive.setManualOverideDriveAutoAim(!(drive.isManualOverideDriveAutoAim()))); // main.add("Hood Angle Manager", hood.hoodPositionRad) // .withWidget(BuiltInWidgets.kEncoder); }