Skip to content

Commit

Permalink
Add tracer (#85)
Browse files Browse the repository at this point in the history 
* responded to feedback

* added credit
  • Loading branch information
@spellingcat
spellingcat authored Nov 2, 2024
1 parent ce429eb commit 3f42b2b
Show file tree
Hide file tree
Showing 4 changed files with 146 additions and 157 deletions.
68 changes: 29 additions & 39 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -473,39 +473,38 @@ public void robotInit() {

@Override
public void robotPeriodic() {
Tracer.startTrace("RobotPeriodic");
Tracer.traceFunc("CommandScheduler", CommandScheduler.getInstance()::run);
// Update ascope mechanism visualization
Logger.recordOutput(
"Mechanism Poses",
new Pose3d[] {
shooter.getMechanismPose(), elevator.getCarriagePose(), elevator.getFirstStagePose()
Tracer.trace(
"RobotPeriodic",
() -> {
Tracer.trace("CommandScheduler", CommandScheduler.getInstance()::run);
// Update ascope mechanism visualization
Logger.recordOutput(
"Mechanism Poses",
new Pose3d[] {
shooter.getMechanismPose(), elevator.getCarriagePose(), elevator.getFirstStagePose()
});
Logger.recordOutput("Target", currentTarget);
Logger.recordOutput("AutoAim/Speaker", FieldConstants.getSpeaker());
// Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization);
Logger.recordOutput(
"Angle to target",
Math.atan2(
FieldConstants.getSpeaker().getY() - swerve.getPose().getY(),
FieldConstants.getSpeaker().getX() - swerve.getPose().getX()));
Logger.recordOutput(
"AutoAim/Actual Distance",
swerve.getPose().minus(FieldConstants.getSpeaker()).getTranslation().getNorm());
});
Logger.recordOutput("Target", currentTarget);
Logger.recordOutput("AutoAim/Speaker", FieldConstants.getSpeaker());
// Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization);
Logger.recordOutput(
"Angle to target",
Math.atan2(
FieldConstants.getSpeaker().getY() - swerve.getPose().getY(),
FieldConstants.getSpeaker().getX() - swerve.getPose().getX()));
Logger.recordOutput(
"AutoAim/Actual Distance",
swerve.getPose().minus(FieldConstants.getSpeaker()).getTranslation().getNorm());

Tracer.endTrace();
}

@Override
public void loopFunc() {
Tracer.startTrace("Robot");
Tracer.traceFunc("LoopFunc", super::loopFunc);
Tracer.endTrace();
Tracer.trace("Robot/LoopFunc", super::loopFunc);
}

private void setUpLogging() {
HashMap<String, Integer> commandCounts = new HashMap<>();
BiConsumer<Command, Boolean> logCommandFunction =
final BiConsumer<Command, Boolean> logCommandFunction =
(Command command, Boolean active) -> {
String name = command.getName();
int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1);
Expand All @@ -515,21 +514,12 @@ private void setUpLogging() {
active.booleanValue());
Logger.recordOutput("Commands/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);
});

var scheduler = CommandScheduler.getInstance();

scheduler.onCommandInitialize(c -> logCommandFunction.accept(c, true));
scheduler.onCommandFinish(c -> logCommandFunction.accept(c, false));
scheduler.onCommandInterrupt(c -> logCommandFunction.accept(c, false));
}

private Command shootWithDashboard() {
Expand Down
78 changes: 41 additions & 37 deletions src/main/java/frc/robot/subsystems/swerve/PhoenixOdometryThread.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,50 +134,54 @@ public void registerSignals(Registration... registrations) {
}

public List<Samples> samplesSince(double timestamp) {
Tracer.startTrace("samples since");
var readLock = journalLock.readLock();
try {
readLock.lock();

return Tracer.traceFunc(
"stream timestamps",
() ->
journal.stream()
.filter(s -> s.timestamp > timestamp)
.collect(Collectors.toUnmodifiableList()));
} finally {
readLock.unlock();
Tracer.endTrace();
}
return Tracer.trace(
"samples since",
() -> {
var readLock = journalLock.readLock();
try {
readLock.lock();

return Tracer.trace(
"stream timestamps",
() ->
journal.stream()
.filter(s -> s.timestamp > timestamp)
.collect(Collectors.toUnmodifiableList()));
} finally {
readLock.unlock();
}
});
}

@Override
public void run() {
while (true) {
// Wait for updates from all signals
var writeLock = journalLock.writeLock();
Tracer.startTrace("Odometry Thread");
Tracer.traceFunc(
"wait for all",
() -> BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY_HZ, signalArr));
try {
writeLock.lock();
var filteredSignals =
registeredSignals.stream()
.filter(s -> s.signal().getStatus().equals(StatusCode.OK))
.collect(Collectors.toSet());
journal.add(
new Samples(
timestampFor(filteredSignals),
filteredSignals.stream()
.collect(
Collectors.toUnmodifiableMap(
s -> new SignalID(s.type(), s.modID()),
s -> s.signal().getValueAsDouble()))));
} finally {
writeLock.unlock();
}
Tracer.endTrace();
Tracer.trace(
"Odometry Thread",
() -> {
Tracer.trace(
"wait for all",
() -> BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY_HZ, signalArr));
try {
writeLock.lock();
var filteredSignals =
registeredSignals.stream()
.filter(s -> s.signal().getStatus().equals(StatusCode.OK))
.collect(Collectors.toSet());
journal.add(
new Samples(
timestampFor(filteredSignals),
filteredSignals.stream()
.collect(
Collectors.toUnmodifiableMap(
s -> new SignalID(s.type(), s.modID()),
s -> s.signal().getValueAsDouble()))));
} finally {
writeLock.unlock();
}
});
}
}

Expand Down
102 changes: 55 additions & 47 deletions src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -373,61 +373,66 @@ public static VisionIO[] createSimCameras() {
}

public void periodic() {
Tracer.startTrace("SwervePeriodic");
for (var camera : cameras) {
Tracer.traceFunc("Update cam inputs", camera::updateInputs);
Tracer.traceFunc("Process cam inputs", camera::processInputs);
}
Tracer.traceFunc(
"Update odo inputs",
() -> odoThread.updateInputs(odoThreadInputs, lastOdometryUpdateTimestamp));
Logger.processInputs("Async Odo", odoThreadInputs);
if (!odoThreadInputs.sampledStates.isEmpty()) {
lastOdometryUpdateTimestamp =
odoThreadInputs.sampledStates.get(odoThreadInputs.sampledStates.size() - 1).timestamp();
}
Tracer.traceFunc("update gyro inputs", () -> gyroIO.updateInputs(gyroInputs));
for (int i = 0; i < modules.length; i++) {
Tracer.traceFunc(
"SwerveModule update inputs from " + modules[i].getPrefix() + " Module",
modules[i]::updateInputs);
}
Logger.processInputs("Swerve/Gyro", gyroInputs);
for (int i = 0; i < modules.length; i++) {
Tracer.traceFunc(
"SwerveModule periodic from " + modules[i].getPrefix() + " Module", modules[i]::periodic);
}

// Stop moving when disabled
if (DriverStation.isDisabled()) {
for (var module : modules) {
module.stop();
}
}
// Log empty setpoint states when disabled
if (DriverStation.isDisabled()) {
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
}
Tracer.trace(
"SwervePeriodic",
() -> {
for (var camera : cameras) {
Tracer.trace("Update cam inputs", camera::updateInputs);
Tracer.trace("Process cam inputs", camera::processInputs);
}
Tracer.trace(
"Update odo inputs",
() -> odoThread.updateInputs(odoThreadInputs, lastOdometryUpdateTimestamp));
Logger.processInputs("Async Odo", odoThreadInputs);
if (!odoThreadInputs.sampledStates.isEmpty()) {
lastOdometryUpdateTimestamp =
odoThreadInputs
.sampledStates
.get(odoThreadInputs.sampledStates.size() - 1)
.timestamp();
}
Tracer.trace("update gyro inputs", () -> gyroIO.updateInputs(gyroInputs));
for (int i = 0; i < modules.length; i++) {
Tracer.trace(
"SwerveModule update inputs from " + modules[i].getPrefix() + " Module",
modules[i]::updateInputs);
}
Logger.processInputs("Swerve/Gyro", gyroInputs);
for (int i = 0; i < modules.length; i++) {
Tracer.trace(
"SwerveModule periodic from " + modules[i].getPrefix() + " Module",
modules[i]::periodic);
}

Logger.recordOutput("ShotData/Angle", AutoAimStates.curShotData.getRotation());
Logger.recordOutput("ShotData/Left RPM", AutoAimStates.curShotData.getLeftRPS());
Logger.recordOutput("ShotData/Right RPM", AutoAimStates.curShotData.getRightRPS());
Logger.recordOutput("ShotData/Flight Time", AutoAimStates.curShotData.getFlightTimeSeconds());
Logger.recordOutput("ShotData/Lookahead", AutoAimStates.lookaheadTime);
// Stop moving when disabled
if (DriverStation.isDisabled()) {
for (var module : modules) {
module.stop();
}
}
// Log empty setpoint states when disabled
if (DriverStation.isDisabled()) {
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
}

Tracer.traceFunc("Update odometry", () -> updateOdometry());
Tracer.traceFunc("update vision", () -> updateVision());
Logger.recordOutput("ShotData/Angle", AutoAimStates.curShotData.getRotation());
Logger.recordOutput("ShotData/Left RPM", AutoAimStates.curShotData.getLeftRPS());
Logger.recordOutput("ShotData/Right RPM", AutoAimStates.curShotData.getRightRPS());
Logger.recordOutput(
"ShotData/Flight Time", AutoAimStates.curShotData.getFlightTimeSeconds());
Logger.recordOutput("ShotData/Lookahead", AutoAimStates.lookaheadTime);

Tracer.endTrace();
Tracer.trace("Update odometry", this::updateOdometry);
Tracer.trace("update vision", this::updateVision);
});
}

private void updateOdometry() {
Logger.recordOutput("Swerve/Updates Since Last", odoThreadInputs.sampledStates.size());
var sampleStates = odoThreadInputs.sampledStates;
var i = 0;
for (var sample : sampleStates) {
Tracer.startTrace("Sample " + i);
i++;
// Read wheel deltas from each module
SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
Expand Down Expand Up @@ -476,7 +481,11 @@ private void updateOdometry() {
Rotation2d.fromDegrees(sample.values().get(new SignalID(SignalType.GYRO, -1)));
lastGyroRotation = rawGyroRotation;
Logger.recordOutput("Odometry/Gyro Rotation", lastGyroRotation);
estimator.updateWithTime(sample.timestamp(), rawGyroRotation, lastModulePositions);
Tracer.trace(
"update estimator",
() ->
estimator.updateWithTime(
sample.timestamp(), rawGyroRotation, lastModulePositions));
}
continue;
}
Expand Down Expand Up @@ -506,7 +515,6 @@ private void updateOdometry() {
Logger.recordOutput("Odometry/Gyro Rotation", lastGyroRotation);
// Apply update
estimator.updateWithTime(sample.timestamp(), rawGyroRotation, modulePositions);
Tracer.endTrace();
}
}

Expand Down
Loading

0 comments on commit 3f42b2b

Please sign in to comment.