From 4e44b33bcdedba965b060ec8ebdd131db3e2b4ca Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 10 Jul 2023 11:04:35 -0600 Subject: [PATCH] logging commands and using withName decoration for clarity of inline commands --- src/main/java/team3176/robot/Constants.java | 2 +- src/main/java/team3176/robot/Robot.java | 32 +++++++++++++++++++ .../java/team3176/robot/RobotContainer.java | 14 ++++---- .../robot/subsystems/superstructure/Arm.java | 8 ++--- .../robot/subsystems/superstructure/Claw.java | 10 +++--- .../subsystems/superstructure/IntakeCone.java | 2 +- .../subsystems/superstructure/IntakeCube.java | 9 +++++- .../superstructure/Superstructure.java | 20 ++++++------ 8 files changed, 67 insertions(+), 30 deletions(-) diff --git a/src/main/java/team3176/robot/Constants.java b/src/main/java/team3176/robot/Constants.java index ffc104e..67e298f 100644 --- a/src/main/java/team3176/robot/Constants.java +++ b/src/main/java/team3176/robot/Constants.java @@ -11,7 +11,7 @@ import java.util.Map; public final class Constants { - private static final RobotType robot = RobotType.ROBOT_SIMBOT; + private static final RobotType robot = RobotType.ROBOT_2023C; public static final double LOOP_PERIODIC_SECS = 0.02; public static final boolean TUNING_MODE = true; diff --git a/src/main/java/team3176/robot/Robot.java b/src/main/java/team3176/robot/Robot.java index 9ecc806..75ae9c3 100644 --- a/src/main/java/team3176/robot/Robot.java +++ b/src/main/java/team3176/robot/Robot.java @@ -4,6 +4,10 @@ package team3176.robot; +import java.util.HashMap; +import java.util.Map; +import java.util.function.BiConsumer; + import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -17,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import team3176.robot.Constants.RobotType; +import team3176.robot.subsystems.drivetrain.Drivetrain; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; @@ -82,6 +87,7 @@ public void robotInit() { case REPLAY: String path = LogFileUtil.findReplayLog(); logger.setReplaySource(new WPILOGReader(path)); + logger.addDataReceiver(new NT4Publisher()); logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(path, "_sim"))); break; } @@ -104,6 +110,32 @@ public void robotInit() { fisheyeThread.start(); } + Map commandCounts = new HashMap<>(); + BiConsumer logCommandFunction = + (Command command, Boolean active) -> { + String name = command.getName(); + int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); + commandCounts.put(name, count); + Logger.getInstance() + .recordOutput( + "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + Logger.getInstance().recordOutput("CommandsAll/" + name, count > 0); + }; + CommandScheduler.getInstance() + .onCommandInitialize( + (Command command) -> { + logCommandFunction.accept(command, true); + }); + CommandScheduler.getInstance() + .onCommandFinish( + (Command command) -> { + logCommandFunction.accept(command, false); + }); + CommandScheduler.getInstance() + .onCommandInterrupt( + (Command command) -> { + logCommandFunction.accept(command, false); + }); } diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 069839f..fc44ee5 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -78,7 +78,7 @@ public RobotContainer() { controller::getForward, controller::getStrafe, controller::getSpin)); - arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); + //arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); autonChooser = new SendableChooser<>(); File paths = new File(Filesystem.getDeployDirectory(), "pathplanner"); for (File f : paths.listFiles()) { @@ -87,7 +87,6 @@ public RobotContainer() { autonChooser.addOption(s, s); } } - SmartDashboard.putData("Auton Choice", autonChooser); @@ -133,7 +132,7 @@ private void configureBindings() { ); - controller.rotStick.button(3).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense())); + controller.rotStick.button(3).whileTrue(new InstantCommand(drivetrain::setBrakeMode).andThen(new SwerveDefense()).withName("setBrakeMode")); controller.rotStick.button(4).whileTrue(superstructure.intakeCubeHumanPlayer()); controller.rotStick.button(4).onFalse(superstructure.prepareCarry()); @@ -142,13 +141,13 @@ private void configureBindings() { double conveyorBumpTime = .1; //In units of seconds controller.operator.povUp().whileTrue(superstructure.prepareScoreHigh()); - controller.operator.povUp().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povUp().onTrue(intakeCube.bumpConveyorTimeout(conveyorBumpTime)); controller.operator.povRight().whileTrue(superstructure.prepareCarry()); - controller.operator.povRight().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povRight().onTrue(intakeCube.bumpConveyorTimeout(conveyorBumpTime)); controller.operator.povDown().whileTrue(superstructure.prepareCatch()); - controller.operator.povDown().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povDown().onTrue(intakeCube.bumpConveyorTimeout(conveyorBumpTime)); controller.operator.povLeft().whileTrue(superstructure.prepareScoreMid()); - controller.operator.povLeft().onTrue(intakeCube.bumpConveyor().withTimeout(conveyorBumpTime)); + controller.operator.povLeft().onTrue(intakeCube.bumpConveyorTimeout(conveyorBumpTime)); // m_Controller.operator.start().onTrue(new ToggleVisionLEDs()); // m_Controller.operator.back().onTrue(new SwitchToNextVisionPipeline()); @@ -228,7 +227,6 @@ public void printCanFaults(){ public Command getAutonomousCommand() { // An example command will be run in autonomous String chosen = autonChooser.getSelected(); - //String chosen = "wall_cone_exit_balance"; PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto(); diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java index 5346969..d642bcb 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Arm.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Arm.java @@ -136,7 +136,7 @@ public void setAngleSetpoint(double setpointAngle) { */ public Command armSetPosition(double angleInDegrees) { SmartDashboard.putNumber("armSetPosition",angleInDegrees); - return this.run(() -> setPIDPosition(angleInDegrees)); + return this.run(() -> setPIDPosition(angleInDegrees)).withName("armSetPosition"+angleInDegrees); } public Command armSetPositionBlocking(double angleInDegrees) { return new FunctionalCommand(() -> { @@ -145,15 +145,15 @@ public Command armSetPositionBlocking(double angleInDegrees) { ()-> {}, b -> {}, this::isArmAtPosition, - this); + this).withName("armsetPositionBlocking"); } public Command armSetPositionOnce(double angleInDegrees) { return this.runOnce(() -> { this.currentState = States.CLOSED_LOOP; - this.armSetpointAngleRaw = angleInDegrees;}); + this.armSetpointAngleRaw = angleInDegrees;}).withName("armSetPosition"+angleInDegrees); } public Command armFineTune(DoubleSupplier angleDeltaCommand) { - return this.run(() -> fineTune(angleDeltaCommand.getAsDouble())); + return this.run(() -> fineTune(angleDeltaCommand.getAsDouble())).withName("armFineTune"); } public Command armAnalogUpCommand() { return this.runEnd(this::armAnalogUp, this::idle); diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java index 3ab63e4..1f55413 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Claw.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Claw.java @@ -110,7 +110,7 @@ public static Claw getInstance() /** * to be called with a whileTrue trigger binding */ - public CommandBase intakeGamePiece(GamePiece piece) { + public Command intakeGamePiece(GamePiece piece) { return this.startEnd(() -> {this.currentGamePiece = piece; intake();},() -> hold()); } /** @@ -121,17 +121,17 @@ public Command scoreGamePiece() { return this.run(() -> {score(); this.currentGamePiece = GamePiece.NONE;}) .until(() -> this.isEmpty()) .andThen(new WaitCommand(0.7)) - .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()); + .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()).withName("scoreGamepiece"); } public Command scoreGamePieceTeleop() { return this.runEnd(() -> {score(); this.currentGamePiece = GamePiece.NONE;},() -> idle()); } //more examples of command composition and why its awesome!! public Command intakeCone() { - return this.intakeGamePiece(GamePiece.CONE).until(this::getLinebreakTwo); + return this.intakeGamePiece(GamePiece.CONE).until(this::getLinebreakTwo).withName("intakeCone"); } public Command intakeCube() { - return this.intakeGamePiece(GamePiece.CUBE).until(this::getLinebreakOne); + return this.intakeGamePiece(GamePiece.CUBE).until(this::getLinebreakOne).withName("intakeCube"); } public Command determineGamePiece() { return this.runOnce( () -> { @@ -142,7 +142,7 @@ public Command determineGamePiece() { this.currentGamePiece = GamePiece.CUBE; hold(); } - }); + }).withName("determineGamePiece"); } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java index 121b3de..b4e6d1e 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java @@ -105,7 +105,7 @@ public Command coneToClaw() { return this.run(() -> {spit();}) .until(() -> this.claw.getLinebreakTwo() == false) .andThen(new WaitCommand(0.5)) - .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()); + .andThen(this.runOnce(()->idle())).withTimeout(2.0).finallyDo((b)->idle()).withName("coneToClaw"); } public double getVelocity() diff --git a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java index f4ae54c..9c4d611 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java @@ -148,7 +148,14 @@ public Command bumpConveyor() { this.spinConveyor(-.8); }, () -> { this.spinConveyor(0); - }); + }).withName("bumpConveyor"); + } + public Command bumpConveyorTimeout(double timeout) { + return this.startEnd(() ->{ + this.spinConveyor(-.8); + }, () -> { + this.spinConveyor(0); + }).withTimeout(timeout).withName("bumpConveyor"); } } diff --git a/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java b/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java index 064b3bd..f2469fe 100644 --- a/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java @@ -34,7 +34,7 @@ public enum GamePiece {CUBE, CONE, NONE} public Command groundCube() { - return new IntakeGroundCube().andThen(this.prepareCarry()); + return new IntakeGroundCube().andThen(this.prepareCarry()).withName("groundCube"); } public Command groundCone() @@ -45,12 +45,12 @@ public Command groundCone() .until(() -> this.claw.getLinebreakThree() == false) .andThen(intakeCone.coneToClaw()) .andThen(new IntakeConeRetractSpinot()) - .andThen(this.prepareCarry()); + .andThen(this.prepareCarry()).withName("groundCone"); } public Command clawIntakeCube() { - return new InstantCommand(() -> claw.intakeGamePiece(GamePiece.CUBE)); + return new InstantCommand(() -> claw.intakeGamePiece(GamePiece.CUBE)).withName("clawIntakeCube"); } /* @@ -67,7 +67,7 @@ public Command scoreGamePieceAuto() { return claw.determineGamePiece() .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(1.5) .andThen(claw.scoreGamePiece()) - .andThen(this.prepareCarry())); + .andThen(this.prepareCarry())).withName("scoreGamePieceAuto"); } public Command scoreFirstGamePieceAuto() { return claw.determineGamePiece() @@ -75,7 +75,7 @@ public Command scoreFirstGamePieceAuto() { .alongWith(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(3.0) .andThen(new WaitCommand(0.5)) .andThen(claw.scoreGamePiece().withTimeout(1.0)) - .andThen(this.prepareCarry()))); + .andThen(this.prepareCarry()))).withName("scoreFirstGamePieceAuto"); } public Command scoreGamePieceHigh() { @@ -83,13 +83,13 @@ public Command scoreGamePieceHigh() .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_HIGH_POS).withTimeout(3.0)) .andThen(new WaitCommand(0.5)) .andThen(claw.scoreGamePiece().withTimeout(1.0)) - .andThen(this.prepareCarry()); + .andThen(this.prepareCarry()).withName("scoreGamePieceHigh"); } public Command scoreCubeLow() { return arm.armSetPosition(SuperStructureConstants.ARM_ZERO_POS) .andThen(new WaitCommand(0.5)) .andThen(claw.scoreGamePiece()) - .andThen(this.prepareCarry()); + .andThen(this.prepareCarry()).withName("scoreCubeLow"); } public Command scoreGamePieceLowAuto() { @@ -97,16 +97,16 @@ public Command scoreGamePieceLowAuto() .andThen(arm.armSetPositionBlocking(SuperStructureConstants.ARM_CATCH_POS).withTimeout(3.0)) .andThen(new WaitCommand(0.5)) .andThen(claw.scoreGamePiece().withTimeout(1.0)) - .andThen(this.prepareCarry()); + .andThen(this.prepareCarry()).withName("scoreGamePieceLowAuto"); } public Command intakeCubeHumanPlayer() { return new ParallelCommandGroup(new ClawInhaleCube(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) - .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); + .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)).withName("intakeCubeHumanPlayer"); } public Command intakeConeHumanPlayer() { return new ParallelCommandGroup(new ClawInhaleCone(), arm.armSetPositionOnce(SuperStructureConstants.ARM_HIGH_POS)) - .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)); + .andThen(arm.armSetPositionOnce(SuperStructureConstants.ARM_CARRY_POS)).withName("intakeConeHumanPlayer"); } public Command preparePoop() {