tested RunCommandForTime + DWIForTime in robotsim, both work

simgui.json

@@ -22,6 +22,7 @@
     "types": {
       "/FMSInfo": "FMSInfo",
       "/LiveWindow/BoomBoom": "Subsystem",
+      "/LiveWindow/Claws": "Subsystem",
       "/LiveWindow/Climber": "Subsystem",
       "/LiveWindow/Extender": "Subsystem",
       "/LiveWindow/Hood": "Subsystem",

src/main/java/frc4388/robot/RobotContainer.java

@@ -72,6 +72,7 @@ import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.commands.RunCommandForTime;
 import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
 import frc4388.robot.commands.ClimberCommands.RunClaw;
 import frc4388.robot.commands.ClimberCommands.RunClimberPath;
@@ -226,7 +227,7 @@ public class RobotContainer {
        }, m_robotClimber));
     
     m_robotBoomBoom.setDefaultCommand(
-      new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))
+      new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
       );
 
     //  autoInit();
@@ -431,13 +432,6 @@ public class RobotContainer {
     //   }).withName("No Autonomous Path");
     // }
 
-    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
-    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
-    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
-    // thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-    loadPath("Move Forward");
-
     // ! this will run each of the specified PathPlanner paths in sequence.
     // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
 
@@ -453,14 +447,22 @@ public class RobotContainer {
     //                                                       "Diamond"));
 
     // * assume turret is already pointed towards target.
-    return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+    // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
-                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
+    //                                    new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
-                                       new ParallelRaceGroup(
+    //                                    new ParallelRaceGroup(
-                                         new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+    //                                      new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
-                                         new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
+    //                                      new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
-                                       ));
+    //                                    ));
 
-    // TODO: we should test TrackTarget timing with my RunCommandForTime thing at some point, same with DriveWithInput timing
+    // return new RunCommandForTime(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true), m_robotSwerveDrive), 1.0, true);
+
+    return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.3, 0.0, 0.0}, 1.0));//,
+                                      //  new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+                                      //  new ParallelCommandGroup(
+                                      //    new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+                                      //    new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)
+                                       //));
 
     // * aim with RotateUntilTarget
     // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),

src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java

@@ -43,6 +43,7 @@ public class DriveWithInputForTime extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.out.println("RUNNING");
     elapsed = System.currentTimeMillis() - start;
     this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
   }
@@ -54,6 +55,9 @@ public class DriveWithInputForTime extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (elapsed >= duration);
+    System.out.println("Duration: " + duration);
+    System.out.println("Elapsed: " + elapsed);
+
+    return ((double) elapsed >= duration);
   }
 }

src/main/java/frc4388/robot/commands/RunCommandForTime.java

@@ -56,6 +56,7 @@ public class RunCommandForTime extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.out.println("RUNNING");
     this.elapsed = System.currentTimeMillis() - this.start;
     this.command.execute();
   }
@@ -70,6 +71,8 @@ public class RunCommandForTime extends CommandBase {
   @Override
   public boolean isFinished() {
     if (this.override) {
+      System.out.println("Duration: " + duration);
+      System.out.println("Elapsed: " + elapsed);
       return (this.elapsed >= this.duration);
     } else {
       return (this.command.isFinished() && (this.elapsed >= this.duration));

src/main/java/frc4388/robot/subsystems/BoomBoom.java

@@ -174,9 +174,9 @@ public class BoomBoom extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
-    SmartDashboard.putNumber("Shooter Current", getCurrent());
+    // SmartDashboard.putNumber("Shooter Current", getCurrent());
-    SmartDashboard.putNumber("Shooter Voltage",  m_shooterFalconLeft.getMotorOutputVoltage());
+    // SmartDashboard.putNumber("Shooter Voltage",  m_shooterFalconLeft.getMotorOutputVoltage());
-    SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
+    // SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
   }
 
   public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
@@ -191,9 +191,9 @@ public class BoomBoom extends SubsystemBase {
   public void runDrumShooter(double speed) {
     // m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2);
     m_shooterFalconLeft.set(speed);
-    SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
+    // SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
-    SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
+    // SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
-    SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
+    // SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
 
   }