From 695ddfca77184c39e5b4972c750720b17ce2cec0 Mon Sep 17 00:00:00 2001 From: aborunda <90010674+aborunda@users.noreply.github.com> Date: Mon, 25 Oct 2021 18:02:23 -0600 Subject: [PATCH] Variable Constants --- src/main/java/frc4388/robot/Constants.java | 23 +++++++++++++++++++ src/main/java/frc4388/robot/Robot.java | 14 ++++++----- .../java/frc4388/robot/RobotContainer.java | 4 +++- .../commands/drive/DriveWithJoystick.java | 15 ++++-------- 4 files changed, 39 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 050b295..930549c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,6 +23,26 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { + public enum Mode { + COMPETITIVE, CASUAL; + private static Mode mode; + public static Mode get() { + return mode; + } + public static void set(Mode mode) { + Mode.mode = mode; + int i = mode.ordinal(); + DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR = DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR_MODES[i]; + } + public static void toggle() { + int i = mode.ordinal() + 1; + Mode[] values = values(); + i = i >= values.length ? 0 : i; + mode = values[i]; + System.out.println(mode); + } + } + public static final int SELECTED_AUTO = 0; public static final class DriveConstants { @@ -63,6 +83,9 @@ public final class Constants { public static final int DRIVE_CRUISE_VELOCITY = 30000; public static final int DRIVE_ACCELERATION = 23000; + private static final double[] DRIVE_WITH_JOYSTICK_FACTOR_MODES = { 1.0, 0.5 }; + public static double DRIVE_WITH_JOYSTICK_FACTOR; + public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5); public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 30b626e..91628b1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.robot.Constants.Mode; /** * The VM is configured to automatically run this class, and to call the @@ -31,11 +32,6 @@ public class Robot extends TimedRobot { double m_initialTime; double m_currentTime; double m_deltaTime; - - protected static Supplier lastIsTestMethod; - public static Boolean lastIsTest() { - return lastIsTestMethod.get(); - } /** * This function is run when the robot is first started up and should be @@ -46,7 +42,6 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - Robot.lastIsTestMethod = this::isTest; SmartDashboard.putString("Is Auto Start?", "NAH"); } @@ -92,6 +87,7 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { + Mode.set(Mode.COMPETITIVE); //m_robotContainer.buildAutos(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -133,6 +129,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { + Mode.set(Mode.COMPETITIVE); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(true); @@ -157,6 +154,11 @@ public class Robot extends TimedRobot { public void teleopPeriodic() { } + @Override + public void testInit() { + Mode.set(Mode.CASUAL); + } + /** * This function is called periodically during test mode. */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2891b60..eccdd89 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.Mode; import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.InterruptSubystem; import frc4388.robot.commands.auto.Barrel; @@ -225,7 +226,8 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand()); - + new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON) + .whenPressed(new InstantCommand(Mode::toggle)); /* Driver Buttons */ // sets solenoids into high gear diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java index 3406397..e2949ff 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java @@ -8,6 +8,7 @@ package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants; import frc4388.robot.Robot; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; @@ -43,15 +44,8 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput; - double steerInput; - if (Robot.lastIsTest()) { - moveInput = m_controller.getLeftYAxis() / 2; - steerInput = m_controller.getRightXAxis() / 2; - } else { - moveInput = m_controller.getLeftYAxis(); - steerInput = m_controller.getRightXAxis(); - } + double moveInput = m_controller.getLeftYAxis(); + double steerInput = m_controller.getRightXAxis(); double moveOutput = 0; double steerOutput = 0; @@ -60,7 +54,8 @@ public class DriveWithJoystick extends CommandBase { } else { moveOutput = Math.cos(1.571*moveInput)-1; } - + moveOutput *= DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR; + System.out.println(moveOutput); double cosMultiplier; double deadzone = .1;