diff --git a/build.gradle b/build.gradle index 26186ce6..57eb86eb 100644 --- a/build.gradle +++ b/build.gradle @@ -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 { @@ -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 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5571e50d..dc09d154 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -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; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 18677f0a..c257961e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,7 +21,9 @@ * project. */ public class Robot extends LoggedRobot { - @SuppressWarnings("unused") + + private int cycle = 0; + private RobotContainer robotContainer; @SuppressWarnings("unused") @@ -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 @@ -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 diff --git a/src/main/java/frc/robot/commands/DriveWithJoysticks.java b/src/main/java/frc/robot/commands/DriveWithJoysticks.java index 21bbe09e..3f140d0f 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoysticks.java +++ b/src/main/java/frc/robot/commands/DriveWithJoysticks.java @@ -21,6 +21,8 @@ public class DriveWithJoysticks extends Command { double yMpS; double rotRadpS; + ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + public DriveWithJoysticks( CommandSwerveDrivetrain drivetrain, DoubleSupplier x, @@ -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 diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 1090e63c..e0eb24dd 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -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; @@ -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, @@ -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) { @@ -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: diff --git a/src/main/java/frc/robot/telemetry/Telemetry.java b/src/main/java/frc/robot/telemetry/Telemetry.java index a0b55c22..b4787a08 100644 --- a/src/main/java/frc/robot/telemetry/Telemetry.java +++ b/src/main/java/frc/robot/telemetry/Telemetry.java @@ -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(); @@ -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(), @@ -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); @@ -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) { /* @@ -141,8 +62,6 @@ 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(); @@ -150,7 +69,6 @@ public void telemeterize(SwerveDriveState state) { 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()) @@ -158,37 +76,17 @@ public void telemeterize(SwerveDriveState state) { 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]); - } } /**