Variable Constants

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);

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<Boolean> 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.
    */

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

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;