diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a336a4f..1aaef2f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -33,6 +33,7 @@ import edu.wpi.first.units.measure.Distance; import frc4388.utility.CanDevice; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; +import frc4388.utility.Trim; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -82,6 +83,8 @@ public final class Constants { public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 + public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); + private static final class ModuleSpecificConstants { //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e64ca36..18e54f5 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -25,6 +25,7 @@ import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; import frc4388.utility.Status; import frc4388.utility.Subsystem; +import frc4388.utility.Trim; import frc4388.utility.Status.Report; import frc4388.utility.Status.ReportLevel; //import frc4388.robot.subsystems.LED; @@ -160,6 +161,14 @@ public class Robot extends TimedRobot { // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); } + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopExit() { // the only OTHER mode that teleop can enter into is disabled. + Trim.dumpAll(); + } + @Override public void testInit() { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0a28e41..6f8887c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.GenericHID; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -145,7 +146,18 @@ public class RobotContainer { DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - + + // @ /* Trim Test Buttons */ + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load())); + // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); diff --git a/src/main/java/frc4388/utility/Trim.java b/src/main/java/frc4388/utility/Trim.java index 3d985ea..7a9d017 100644 --- a/src/main/java/frc4388/utility/Trim.java +++ b/src/main/java/frc4388/utility/Trim.java @@ -56,7 +56,7 @@ public class Trim { trimElement.setValue(currentValue); modified = true; } - + public void stepUp() { this.currentValue += step; clampModify(); @@ -109,7 +109,7 @@ public class Trim { } public static void dumpAll() { - for (int i = 0; i > trims.size(); i++) { + for (int i = 0; i < trims.size(); i++) { Trim trim = trims.get(i); if (trim.isModified()) trim.dump();