Skip to content
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

Add rudamentary performance profiling infrastructure and remove unnecessary NT fields #160

Merged
merged 4 commits into from
Mar 12, 2024
Merged

Add rudamentary performance profiling infrastructure and remove unnecessary NT fields #160

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c932872
Add debug configurations
charlesmackenzie Feb 27, 2024
6eb095c
Log memory usage to profile performance
charlesmackenzie Mar 8, 2024
85fcfcf
Remove unnecessary networktables outputs
charlesmackenzie Mar 8, 2024
6dd8d58
Don't construct scoring mechanisms outside sim
charlesmackenzie Mar 10, 2024
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
13 changes: 13 additions & 0 deletions build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@ java {

def ROBOT_MAIN_CLASS = "frc.robot.Main"

def VISUALVM_PROFILE = false

// Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project DeployUtils.
deploy {
Expand All @@ -32,6 +34,17 @@ deploy {
// getTargetTypeClass is a shortcut to get the class type using a string

frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
// jvmArgs.add("-XX:MaxHeapFreeRatio 40")

if (VISUALVM_PROFILE) {
// Enable VisualVM connection
jvmArgs.add("-Dcom.sun.management.jmxremote=true")
jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
jvmArgs.add("-Djava.rmi.server.hostname=10.4.1.2")
}
}

// Static files artifact
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,11 +47,11 @@ public static enum Mode {
}

public static final class FeatureFlags {
public static final boolean runVision = false;
public static final boolean runVision = true;

public static final boolean runIntake = false;
public static final boolean runScoring = false;
public static final boolean runEndgame = false;
public static final boolean runIntake = true;
public static final boolean runScoring = true;
public static final boolean runEndgame = true;
public static final boolean runDrive = true;

public static final boolean enableLEDS = false;
Expand Down
14 changes: 12 additions & 2 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,9 @@
* project.
*/
public class Robot extends LoggedRobot {
@SuppressWarnings("unused")

private int cycle = 0;

private RobotContainer robotContainer;

@SuppressWarnings("unused")
Expand All @@ -41,7 +43,6 @@ public void robotInit() {
Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);

if (Constants.currentMode == Constants.Mode.REAL) {
// TODO: Log data to a USB drive on the RIO
Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
pdh = new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
Expand Down Expand Up @@ -71,6 +72,15 @@ public void robotPeriodic() {
CommandScheduler.getInstance().run();

robotContainer.robotPeriodic();

if (cycle % 20 == 0) {
Logger.recordOutput("JVM/total memory", Runtime.getRuntime().totalMemory());
Logger.recordOutput("JVM/max memory", Runtime.getRuntime().maxMemory());
Logger.recordOutput(
"JVM/used memory",
Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory());
}
cycle++;
}

@Override
Expand Down
8 changes: 6 additions & 2 deletions src/main/java/frc/robot/commands/DriveWithJoysticks.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@ public class DriveWithJoysticks extends Command {
double yMpS;
double rotRadpS;

ChassisSpeeds chassisSpeeds = new ChassisSpeeds();

public DriveWithJoysticks(
CommandSwerveDrivetrain drivetrain,
DoubleSupplier x,
Expand Down Expand Up @@ -77,8 +79,10 @@ public void execute() {
rotRadpS *= 0.5;
}

drivetrain.setGoalChassisSpeeds(
new ChassisSpeeds(xMpS, yMpS, rotRadpS), fieldCentric.getAsBoolean());
chassisSpeeds.vxMetersPerSecond = xMpS;
chassisSpeeds.vyMetersPerSecond = yMpS;
chassisSpeeds.omegaRadiansPerSecond = rotRadpS;
drivetrain.setGoalChassisSpeeds(chassisSpeeds, fieldCentric.getAsBoolean());
}

@Override
Expand Down
31 changes: 21 additions & 10 deletions src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color8Bit;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.Constants.Mode;
import frc.robot.Constants.ScoringConstants;
import frc.robot.utils.FieldFinder;
import frc.robot.utils.FieldFinder.FieldLocations;
Expand Down Expand Up @@ -59,13 +61,10 @@ public class ScoringSubsystem extends SubsystemBase implements Tunable {
private boolean overrideShoot = false;
private boolean overrideStageAvoidance = false;

private final Mechanism2d mechanism = new Mechanism2d(2.2, 2.0);
private final MechanismRoot2d rootMechanism = mechanism.getRoot("scoring", 0.6, 0.3);
private final MechanismLigament2d aimMechanism =
rootMechanism.append(new MechanismLigament2d("aimer", 0.5, 0.0));
private final MechanismLigament2d hoodMechanism =
aimMechanism.append(
new MechanismLigament2d("hood", 0.2, 0.0, 10.0, new Color8Bit(0, 200, 50)));
private Mechanism2d mechanism;
private MechanismRoot2d rootMechanism;
private MechanismLigament2d aimMechanism;
private MechanismLigament2d hoodMechanism;

public enum ScoringState {
IDLE,
Expand Down Expand Up @@ -122,6 +121,16 @@ public ScoringSubsystem(ShooterIO shooterIo, AimerIO aimerIo, HoodIO hoodIo) {
aimerAvoidElevator =
new InterpolateDouble(
ScoringConstants.aimerAvoidElevatorTable(), 0.0, Math.PI / 2.0);

if (Constants.currentMode == Mode.SIM) {
mechanism = new Mechanism2d(2.2, 2.0);
rootMechanism = mechanism.getRoot("scoring", 0.6, 0.3);
aimMechanism = rootMechanism.append(new MechanismLigament2d("aimer", 0.5, 0.0));
hoodMechanism =
aimMechanism.append(
new MechanismLigament2d(
"hood", 0.2, 0.0, 10.0, new Color8Bit(0, 200, 50)));
}
}

public void setAction(ScoringAction action) {
Expand Down Expand Up @@ -475,9 +484,11 @@ && willHitStage()) {

Logger.recordOutput("aimer/willIHitStage", willHitStage());

aimMechanism.setAngle(Units.radiansToDegrees(aimerInputs.aimAngleRad));
hoodMechanism.setAngle(Units.radiansToDegrees(hoodInputs.hoodAngleRad));
Logger.recordOutput("scoring/mechanism2d", mechanism);
if (Constants.currentMode == Mode.SIM) {
aimMechanism.setAngle(Units.radiansToDegrees(aimerInputs.aimAngleRad));
hoodMechanism.setAngle(Units.radiansToDegrees(hoodInputs.hoodAngleRad));
Logger.recordOutput("scoring/mechanism2d", mechanism);
}

switch (state) {
case IDLE:
Expand Down
102 changes: 0 additions & 102 deletions src/main/java/frc/robot/telemetry/Telemetry.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,22 +10,10 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import frc.robot.Constants;
import org.littletonrobotics.junction.Logger;

public class Telemetry {
private final double maxSpeed;

private final TelemetryIO telemetryIo;
private final TelemetryIOInputsAutoLogged telemetryInputs = new TelemetryIOInputsAutoLogged();

Expand All @@ -35,18 +23,9 @@ public class Telemetry {
* @param maxSpeed Maximum speed in meters per second
*/
public Telemetry(double maxSpeed, TelemetryIO telemetryIo) {
this.maxSpeed = maxSpeed;
this.telemetryIo = telemetryIo;
}

/* What to publish over networktables for telemetry */
NetworkTableInstance inst = NetworkTableInstance.getDefault();

/* Robot pose for field positioning */
NetworkTable table = inst.getTable("Pose");
DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
StringPublisher fieldTypePub = table.getStringTopic(".type").publish();

SwerveModuleState[] moduleStates =
new SwerveModuleState[] {
new SwerveModuleState(),
Expand All @@ -59,15 +38,6 @@ public Telemetry(double maxSpeed, TelemetryIO telemetryIo) {
double robotRotationVelocity = 0;
double robotRotationLast = 0;

/* Robot speeds for general checking */
NetworkTable driveStats = inst.getTable("Drive");
DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
DoublePublisher accelX = driveStats.getDoubleTopic("Acceleration X").publish();
DoublePublisher accelY = driveStats.getDoubleTopic("Acceleration Y").publish();
DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish();

double velocityXFiltered = 0.0;
double velocityYFiltered = 0.0;
LinearFilter velocityXFilter = LinearFilter.singlePoleIIR(0.1, Constants.loopTime);
Expand All @@ -83,55 +53,6 @@ public Telemetry(double maxSpeed, TelemetryIO telemetryIo) {

SwerveModuleState[] latestModuleStates = new SwerveModuleState[4];

/* Mechanisms to represent the swerve module states */
Mechanism2d[] m_moduleMechanisms =
new Mechanism2d[] {
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
};
/* A direction and length changing ligament for speed representation */
MechanismLigament2d[] m_moduleSpeeds =
new MechanismLigament2d[] {
m_moduleMechanisms[0]
.getRoot("RootSpeed", 0.5, 0.5)
.append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[1]
.getRoot("RootSpeed", 0.5, 0.5)
.append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[2]
.getRoot("RootSpeed", 0.5, 0.5)
.append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[3]
.getRoot("RootSpeed", 0.5, 0.5)
.append(new MechanismLigament2d("Speed", 0.5, 0)),
};
/* A direction changing and length constant ligament for module direction */
MechanismLigament2d[] m_moduleDirections =
new MechanismLigament2d[] {
m_moduleMechanisms[0]
.getRoot("RootDirection", 0.5, 0.5)
.append(
new MechanismLigament2d(
"Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[1]
.getRoot("RootDirection", 0.5, 0.5)
.append(
new MechanismLigament2d(
"Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[2]
.getRoot("RootDirection", 0.5, 0.5)
.append(
new MechanismLigament2d(
"Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[3]
.getRoot("RootDirection", 0.5, 0.5)
.append(
new MechanismLigament2d(
"Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
};

/* Accept the swerve drive state and telemeterize it to smartdashboard */
public void telemeterize(SwerveDriveState state) {
/*
Expand All @@ -141,54 +62,31 @@ public void telemeterize(SwerveDriveState state) {
*/
/* Telemeterize the pose */
Pose2d pose = state.Pose;
fieldTypePub.set("Field2d");
fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getRadians()});

robotRotation = pose.getRotation().getRadians();

/* Telemeterize the robot's general speeds */
double currentTime = Utils.getCurrentTimeSeconds();
double diffTime = currentTime - lastTime;
lastTime = currentTime;
Translation2d distanceDiff = pose.minus(latestPose).getTranslation();

Translation2d velocityFieldRelative =
new Translation2d(pose.getX() - latestPose.getX(), pose.getY() - latestPose.getY())
.div(diffTime);

latestPose = pose;

Translation2d velocities = distanceDiff.div(diffTime);

double robotRotationDiff = robotRotation - robotRotationLast;
robotRotationVelocity = robotRotationDiff / diffTime;

robotRotationLast = robotRotation;

speed.set(velocities.getNorm());
velocityX.set(velocities.getX());
velocityY.set(velocities.getY());
accelX.set(telemetryInputs.accelerationX);
accelY.set(telemetryInputs.accelerationY);

velocityXFiltered = velocityXFilter.calculate(velocityFieldRelative.getX());
velocityYFiltered = velocityYFilter.calculate(velocityFieldRelative.getY());
accelXFiltered = accelXFilter.calculate(telemetryInputs.accelerationX);
accelYFiltered = accelYFilter.calculate(telemetryInputs.accelerationY);
odomPeriod.set(state.OdometryPeriod);

latestModuleStates = state.ModuleStates;

/* Telemeterize the module's states */
for (int i = 0; i < 4; ++i) {
m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
m_moduleSpeeds[i].setLength(
state.ModuleStates[i].speedMetersPerSecond / (2 * maxSpeed));

moduleStates[i] = state.ModuleStates[i];
SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
}
}

/**
Expand Down
Loading