From 3efe228e00a119f82e5580bf59c996cb12ba9c14 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 13 Feb 2023 19:47:24 -0700 Subject: [PATCH] mode switching (might need more testing) --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 16 +++++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index efe227b..a7767f5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -65,7 +65,7 @@ public final class Constants { public static final int CANCODER_TICKS_PER_ROTATION = 4096; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.5; // TODO: find the actual value public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; public static final double MOTOR_REV_PER_STEER_REV = 12.8; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index edcd441..2f7a659 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,8 @@ package frc4388.robot; +import java.util.function.BooleanSupplier; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -41,6 +43,9 @@ public class RobotContainer { private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + + private boolean mode = true; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -77,7 +82,8 @@ public class RobotContainer { .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.highSpeed(false), m_robotSwerveDrive)); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.highSpeed(mode), m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> this.toggleMode())); // /* Operator Buttons */ // // interrupt button @@ -110,6 +116,14 @@ public class RobotContainer { return this.m_operatorXbox; } + public boolean getMode() { + return this.mode; + } + + public void toggleMode() { + mode = !mode; + } + /** * Add your docs here. */