From 81ef2a858ae64086b7dfda0edd3b2eff1884f88c Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 22 Mar 2022 16:56:40 -0600 Subject: [PATCH] tested RunCommandForTime + DWIForTime in robotsim, both work --- simgui.json | 1 + .../java/frc4388/robot/RobotContainer.java | 32 ++++++++++--------- .../DriveCommands/DriveWithInputForTime.java | 6 +++- .../robot/commands/RunCommandForTime.java | 3 ++ .../frc4388/robot/subsystems/BoomBoom.java | 12 +++---- 5 files changed, 32 insertions(+), 22 deletions(-) diff --git a/simgui.json b/simgui.json index f3617f0..2a797b5 100644 --- a/simgui.json +++ b/simgui.json @@ -22,6 +22,7 @@ "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Claws": "Subsystem", "/LiveWindow/Climber": "Subsystem", "/LiveWindow/Extender": "Subsystem", "/LiveWindow/Hood": "Subsystem", diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9bd41dc..ee57648 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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), - new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), - new ParallelRaceGroup( - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage) - )); + // 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 ParallelRaceGroup( + // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + // 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), diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java index 6f436b9..a2ce001 100644 --- a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java +++ b/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); } } diff --git a/src/main/java/frc4388/robot/commands/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java index 5e35682..b66947d 100644 --- a/src/main/java/frc4388/robot/commands/RunCommandForTime.java +++ b/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)); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 8e8c334..82eb661 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/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 Voltage", m_shooterFalconLeft.getMotorOutputVoltage()); - SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); + // SmartDashboard.putNumber("Shooter Current", getCurrent()); + // SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage()); + // 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 current stator", m_shooterFalconLeft.getStatorCurrent()); - SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); + // SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); + // SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent()); + // SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); }