Skip to content

Command Logging Addition #9

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jheidegger wants to merge 1 commit into
base: IRI_dev
Choose a base branch
from
Open

Command Logging Addition #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/team3176/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
32 changes: 32 additions & 0 deletions src/main/java/team3176/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Expand Down Expand Up @@ -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;
}
Expand All @@ -104,6 +110,32 @@ public void robotInit() {
fisheyeThread.start();
}

Map<String, Integer> commandCounts = new HashMap<>();
BiConsumer<Command, Boolean> 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);
});

}

Expand Down
14 changes: 6 additions & 8 deletions src/main/java/team3176/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand All @@ -87,7 +87,6 @@ public RobotContainer() {
autonChooser.addOption(s, s);
}
}


SmartDashboard.putData("Auton Choice", autonChooser);

Expand Down Expand Up @@ -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());
Expand All @@ -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());
Expand Down Expand Up @@ -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();
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/team3176/robot/subsystems/superstructure/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(() -> {
Expand All @@ -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);
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/team3176/robot/subsystems/superstructure/Claw.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
/**
Expand All @@ -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( () -> {
Expand All @@ -142,7 +142,7 @@ public Command determineGamePiece() {
this.currentGamePiece = GamePiece.CUBE;
hold();
}
});
}).withName("determineGamePiece");
}


Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3176/robot/subsystems/superstructure/IntakeCone.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
9 changes: 8 additions & 1 deletion src/main/java/team3176/robot/subsystems/superstructure/IntakeCube.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}

}
20 changes: 10 additions & 10 deletions src/main/java/team3176/robot/subsystems/superstructure/Superstructure.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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");
}

/*
Expand All @@ -67,46 +67,46 @@ 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()
.andThen(intakeCone.extendAndFreeSpin().withTimeout(1.0)
.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()
{
return claw.determineGamePiece()
.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()
{
return claw.determineGamePiece()
.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() {
Expand Down