Major code cleanup + Nuked autonomous commands(code is now ready for …

…deploying)
SciBorgs · Nov 17, 2024 · 2d7ade6 · 2d7ade6
1 parent 8a2bcea
commit 2d7ade6
Show file tree

Hide file tree

Showing 10 changed files with 354 additions and 661 deletions.
diff --git a/simgui.json b/simgui.json
@@ -12,7 +12,8 @@
       "/Robot/drive/diffDriveData/field": "Field2d",
       "/Robot/drive/motorData/field": "Field2d",
       "/Robot/drive/rotationAutos/pidController": "ProfiledPIDController",
-      "/Robot/drive/rotationPIDData/pidController": "ProfiledPIDController"
+      "/Robot/drive/rotationPIDData/pidController": "ProfiledPIDController",
+      "/Robot/drivetrain/motorData/field": "Field2d"
     }
   },
   "NetworkTables": {

diff --git a/src/main/java/org/sciborgs1155/robot/Constants.java b/src/main/java/org/sciborgs1155/robot/Constants.java
@@ -18,4 +18,7 @@
 public class Constants {
   /** Roborio Tick Rate */
   public static final Measure<Time> PERIOD = Seconds.of(0.02);
+
+  /** Returns whether the robot is real or not */
+  public static final boolean robotIsReal = Robot.isReal();
 }
diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java
@@ -1,8 +1,8 @@
 package org.sciborgs1155.robot;
 
-import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.autonomous;
+import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.disabled;
 import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.teleop;
 import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.test;
 import static org.sciborgs1155.robot.Constants.PERIOD;
@@ -15,12 +15,10 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import monologue.Logged;
 import monologue.Monologue;
-
 import org.littletonrobotics.urcl.URCL;
 import org.sciborgs1155.lib.CommandRobot;
 import org.sciborgs1155.lib.FaultLogger;
 import org.sciborgs1155.robot.Ports.OI;
-import org.sciborgs1155.robot.tankdrive.DriveConstants;
 import org.sciborgs1155.robot.tankdrive.DiffDrive;
 
 /**
@@ -34,15 +32,15 @@
  */
 public class Robot extends CommandRobot implements Logged {
 
-  /** Controls intake, shooter, and climber. */
+  /** Controls intake, shooter, and climber */
   @SuppressWarnings("unused")
   private final CommandXboxController operator = new CommandXboxController(OI.OPERATOR);
 
-  /** Controls drivetrain. */
+  /** Controls drivetrain */
+  @SuppressWarnings("unused")
   private final CommandXboxController driver = new CommandXboxController(OI.DRIVER);
 
-  /** Drivetrain Subsystem. */
-  private final DiffDrive drive = DiffDrive.create();
+  private final DiffDrive drivetrain = DiffDrive.create(isSimulation());
 
   /** The robot contains subsystems, OI devices, and commands. */
   public Robot() {
@@ -64,7 +62,7 @@ private void configureGameBehavior() {
     addPeriodic(FaultLogger::update, PERIOD.in(Seconds));
     System.out.println("Setup Fault Logger!");
 
-    addPeriodic(drive::updateVoltages, PERIOD.in(Seconds));
+    addPeriodic(drivetrain::updateVoltages, PERIOD.in(Seconds));
 
     if (!isReal()) {
       DriverStation.silenceJoystickConnectionWarning(true);
@@ -78,54 +76,49 @@ private void configureGameBehavior() {
 
   /** Configures command bindings */
   private void configureBindings() {
-    teleop().onTrue(teleOpCommand());
+    teleop().onTrue(teleopCommand());
     test().onTrue(testCommand());
     autonomous().onTrue(autonomousCommand());
+    disabled().onTrue(disabledCommand());
 
-    System.out.println("Configured Bindings!");
+    System.out.println("Configured Command Bindings!");
   }
 
-  /* Runs when teleop mode is enabled. */
-  private Command teleOpCommand() {
+  /** Runs once when teleop mode is enabled. Binded to 'teleop' trigger. */
+  private Command teleopCommand() {
     CommandScheduler.getInstance().cancelAll();
 
     return Commands.sequence(
-        Commands.print("Enabled Teleop!"),
-        Commands.runOnce(
-            () -> {
-              driver
-                  .leftBumper()
-                  .or(driver.rightBumper())
-                  .onTrue(Commands.runOnce(() -> drive.setSpeedMultiplier(DriveConstants.FULL_SPEED)))
-                  .onFalse(Commands.run(() -> drive.setSpeedMultiplier(DriveConstants.SLOW_SPEED)));
-
-              drive.setDefaultCommand(
-                  drive.inputArcade(
-                      () -> driver.getRawAxis(1),
-                      () -> driver.getRawAxis(0)));
-            }))
-        .withName("TeleOp Command");
+        Commands.print("Enabled Teleop Mode!"),
+        drivetrain.inputArcade(() -> -driver.getLeftY(), () -> -driver.getLeftX()).repeatedly())
+        .withName("Teleop Command")
+        .finallyDo(() -> System.out.println("Disabled Teleop Mode!"));
   }
 
-  /* Runs when test mode is enabled. */
+  /** Runs once when test mode is enabled. Binded to 'test' trigger. */
   private Command testCommand() {
     CommandScheduler.getInstance().cancelAll();
 
-    return Commands.sequence(
-        Commands.print("Enabled Test Mode!"),
-        drive.rotate(Degrees.of(90)), drive.rotate(Degrees.of(90))).withName("Test Command");
-
-    // return Commands.sequence(
-    // Commands.print("Enabled Test Mode!"),
-    // drive.rotate(Degrees.of(90)), drive.rotate(Degrees.of(90)),
-    // drive.rotate(Degrees.of(90)),
-    // drive.rotate(Degrees.of(90))).withName("Test Command");
+    return Commands.sequence(Commands.print("Enabled Test Mode!"))
+        .withName("Test Command")
+        .finallyDo(() -> System.out.println("Disabled Test Mode!"));
   }
 
-  /* Runs when autonomous mode is enabled. */
+  /**
+   * Runs once when autonomous mode is enabled. Binded to 'autonomous' trigger.
+   */
   private Command autonomousCommand() {
     CommandScheduler.getInstance().cancelAll();
 
-    return Commands.sequence(Commands.print("Enabled Autonomous Mode!")).withName("Autonomous Command");
+    return Commands.sequence(Commands.print("Enabled Autonomous Mode!"))
+        .withName("Autonomous Command")
+        .finallyDo(() -> System.out.println("Disabled Autonomous Mode!"));
+  }
+
+  /** Runs once when robot is disabled. Binded to 'disabled' trigger. */
+  private Command disabledCommand() {
+    CommandScheduler.getInstance().cancelAll();
+
+    return Commands.sequence(Commands.print("Disabled Robot!")).withName("Disabled Command");
   }
 }