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

Fixing command loop overruns #202

Open
wants to merge 10 commits into
base: main
Choose a base branch
from
Open

Fixing command loop overruns #202

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
fda4f27
changes
rechsby Jan 18, 2025
496ef38
Mechanical Advantage GC Updates
mmilunicmobile Jan 27, 2025
f866884
Merge branch 'main' into fixing-command-loop-overruns
mmilunicmobile Feb 11, 2025
93be415
appeared to fix sim overruns with vision modificaitons
mmilunicmobile Feb 12, 2025
471098f
Made the arm and swerve more optimal in their status signal usage.
mmilunicmobile Feb 12, 2025
374d26f
Formatted
mmilunicmobile Feb 12, 2025
57ba135
refactor: optimize Phoenix6 status signals across subsystems
mmilunicmobile Feb 12, 2025
98a5e85
Merge branch 'main' into fixing-command-loop-overruns
mmilunicmobile Feb 15, 2025
5783a41
Yay!
mmilunicmobile Feb 15, 2025
99e32e3
spotless
mmilunicmobile Feb 15, 2025
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
22 changes: 22 additions & 0 deletions build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,28 @@ deploy {
// getTargetTypeClass is a shortcut to get the class type using a string

frcJava(getArtifactTypeClass('FRCJavaArtifact')) {

// Advantage Kit suggestions
jvmArgs.add("-XX:+UnlockExperimentalVMOptions")
jvmArgs.add("-XX:GCTimeRatio=5")
jvmArgs.add("-XX:+UseSerialGC")
jvmArgs.add("-XX:MaxGCPauseMillis=50")
// jvmArgs.add("-XX:+TieredCompilation")

// The options below may improve performance, but should only be enabled on the RIO 2

final MAX_JAVA_HEAP_SIZE_MB = 100;
jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M")
jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M")
jvmArgs.add("-XX:+AlwaysPreTouch")

// 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.25.39.2") // Replace TE.AM with team number
}

// Static files artifact
Expand Down
19 changes: 17 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 @@ -4,14 +4,18 @@

package frc.robot;

import com.pathplanner.lib.commands.PathfindingCommand;
import edu.wpi.first.net.WebServer;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.util.Elastic;
import frc.robot.util.Elastic.Notification;
import java.lang.management.CompilationMXBean;
import java.lang.management.ManagementFactory;
import java.nio.charset.Charset;
import java.nio.charset.StandardCharsets;
import java.nio.file.Files;
Expand All @@ -27,9 +31,17 @@ public class Robot extends LoggedRobot {

private final RobotContainer m_robotContainer;

private final CompilationMXBean compMXBean;

public Robot() {
m_robotContainer = new RobotContainer();

compMXBean = ManagementFactory.getCompilationMXBean();

DriverStation.silenceJoystickConnectionWarning(true);

WebServer.start(5800, Filesystem.getDeployDirectory().getPath());

Logger.recordMetadata("ProjectName", "JavaBot-2025"); // Set a metadata value

if (isReal()) {
Expand Down Expand Up @@ -63,14 +75,17 @@ public Robot() {

Logger.start(); // Start logging! No more data receivers, replay sources, or metadata
// values may be added.

WebServer.start(5800, Filesystem.getDeployDirectory().getPath());
PathfindingCommand.warmupCommand();
}

@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
m_robotContainer.auto.logAutoInformation();

if (compMXBean != null) {
Logger.recordOutput("LoggedRobot/CompilationMS", compMXBean.getTotalCompilationTime());
}
}

@Override
Expand Down
98 changes: 6 additions & 92 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -190,76 +190,6 @@ private void configureBindings() {
return drivetrain.driveDriverRelative(driverVelocitySupplier.get());
}));

// drive.withVelocityX(-leftDriveController.getYAxis().get() *
// GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y
// (forward)
// .withVelocityY(-leftDriveController.getXAxis().get() *
// GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left)
// .withRotationalRate(-rightDriveController.getXAxis().get() *
// GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative
// X (left)

// operatorController.getA().whileTrue(drivetrain.applyRequest(() -> brake));
// operatorController.getA().onTrue(new alignToTargetX(drivetrain, vision, 10,
// 0));

// operatorController
// .getA()
// .onTrue(
// new AlignToAngle(
// drivetrain,
// new Rotation2d(),
// true,
// leftJoystickVelocityX,
// leftJoystickVelocityY)
// .andThen(
// new alignToTargetX(
// drivetrain, vision, 10, 0,
// leftJoystickVelocityX)));

// operatorController.getA().toggleOnTrue(alignToReef(9, 0));
// leftDriveController.getBottomThumb().whileTrue(alignToReef(9, 0));
// leftDriveController.getRightThumb().whileTrue(alignToReef(9, 0.4));
// leftDriveController.getLeftThumb().whileTrue(alignToReef(9, -0.4));
// leftDriveController.getBottomThumb().whileTrue(alignAndDriveToReef(19, 0));
// operatorController
// .getB()
// .whileTrue(
// drivetrain.applyRequest(
// () ->
// point.withModuleDirection(
// new Rotation2d(
// -operatorController.getLeftYAxis().get(),
// -operatorController
// .getLeftXAxis()
// .get()))));

// leftDriveController
// .getTrigger()
// .whileTrue(
// new WheelRadiusCharacterization(
// WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain));

// Run SysId routines when holding back/start and X/Y.
// Note that each routine should be run exactly once in a single log.

// operatorController
// .getBack()
// .and(operatorController.getY())
// .whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
// operatorController
// .getBack()
// .and(operatorController.getX())
// .whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
// operatorController
// .getStart()
// .and(operatorController.getY())
// .whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
// operatorController
// .getStart()
// .and(operatorController.getX())
// .whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));

SmartDashboard.putData(
drivetrain
.sysIdDynamic(Direction.kForward)
Expand Down Expand Up @@ -325,25 +255,7 @@ private void configureBindings() {
.withName("Arm SysId Quasistatic Reverse"));

SmartDashboard.putData(armSubsystem);
// operatorController
// operatorController.getA().onTrue(stateManager.moveToPosition(Position.L4));
// operatorController.getB().onTrue(stateManager.moveToPosition(Position.L3));
// operatorController.getX().onTrue(stateManager.moveToPosition(Position.Source));
// operatorController.getY().onTrue(stateManager.moveToPosition(Position.Home));

// reset the field-centric heading on left bumper press
// operatorController
// .getLeftBumper()
// .onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
// operatorController
// .getRightBumper()
// .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(Pose2d.kZero)));

// Operator Mode Setting
operatorController.getLeftBumper().onTrue(stateManager.setLeftCoralMode());
operatorController.getRightBumper().onTrue(stateManager.setRightCoralMode());
operatorController.getRightTrigger().onTrue(stateManager.setAlgaeMode());
leftDriveController.getRightTopRight().onTrue(stateManager.setArmWristMode());

operatorController
.getLeftJoystick()
.toggleOnTrue(
Expand All @@ -360,6 +272,11 @@ private void configureBindings() {
LightsSubsystem.purple, 1)))); // L2 tation Lights

// Coral Mode Bindings
operatorController.getLeftBumper().onTrue(stateManager.setLeftCoralMode());
operatorController.getRightBumper().onTrue(stateManager.setRightCoralMode());
operatorController.getRightTrigger().onTrue(stateManager.setAlgaeMode());
leftDriveController.getRightTopRight().onTrue(stateManager.setArmWristMode());

final Trigger CORAL = stateManager.LEFT_CORAL.or(stateManager.RIGHT_CORAL);
final Trigger ALGAE = stateManager.ALGAE;
final Trigger ARMWRIST = stateManager.ARMWRIST;
Expand Down Expand Up @@ -400,9 +317,6 @@ private void configureBindings() {

operatorController.getBack().onTrue(wristSubsystem.flipWristPosition());

// Driver Align Bindings, for a different/later day
// CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(9, 0));

// Climb Bindings
leftDriveController.getLeftThumb().whileTrue(climberSubsystem.downPosition());
leftDriveController.getRightThumb().whileTrue(climberSubsystem.upPosition());
Expand Down
57 changes: 46 additions & 11 deletions src/main/java/frc/robot/subsystems/arm/ArmPivotIOTalonFX.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,20 @@
package frc.robot.subsystems.arm;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.hardware.ParentDevice;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Temperature;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import frc.robot.constants.ArmConstants;
import frc.robot.util.PhoenixUtil;

public class ArmPivotIOTalonFX implements ArmPivotIO {
private final TalonFX armPivotMotor =
Expand All @@ -20,28 +29,54 @@ public class ArmPivotIOTalonFX implements ArmPivotIO {

private double lastVoltage = 0;

// Status signals for efficient updates
private final StatusSignal<Angle> positionSignal;
private final StatusSignal<Voltage> voltageSignal;
private final StatusSignal<AngularVelocity> velocitySignal;
private final StatusSignal<Temperature> temperatureSignal;
private final StatusSignal<Current> currentSignal;

public ArmPivotIOTalonFX() {
armPivotMotor.setPosition(0);

// motionMagicVoltage.Slot = 0;

TalonFXConfigurator talonConfig = armPivotMotor.getConfigurator();

// SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs();

talonConfig.apply(
new TalonFXConfiguration().withCurrentLimits(ArmConstants.currentLimitConfigs));

armPivotMotor.setNeutralMode(NeutralModeValue.Brake);

// Initialize status signals
positionSignal = armPivotMotor.getPosition();
voltageSignal = armPivotMotor.getMotorVoltage();
velocitySignal = armPivotMotor.getVelocity();
temperatureSignal = armPivotMotor.getDeviceTemp();
currentSignal = armPivotMotor.getStatorCurrent();

// Set update frequency and optimize bus utilization
PhoenixUtil.tryUntilOk(
5,
() ->
BaseStatusSignal.setUpdateFrequencyForAll(
50.0,
positionSignal,
voltageSignal,
velocitySignal,
temperatureSignal,
currentSignal));

PhoenixUtil.tryUntilOk(
5, () -> ParentDevice.optimizeBusUtilizationForAll(0, armPivotMotor));
}

public void updateInputs(ArmPivotIOInputs inputs) {
// Refresh all signals at once
BaseStatusSignal.refreshAll(
positionSignal, voltageSignal, velocitySignal, temperatureSignal, currentSignal);

inputs.position = armPivotMotor.getPosition().getValueAsDouble();
inputs.voltage = armPivotMotor.getMotorVoltage().getValueAsDouble();
inputs.velocity = armPivotMotor.getVelocity().getValueAsDouble();
inputs.temperature = armPivotMotor.getDeviceTemp().getValueAsDouble();
inputs.current = armPivotMotor.getStatorCurrent().getValueAsDouble();
inputs.position = positionSignal.getValueAsDouble();
inputs.voltage = voltageSignal.getValueAsDouble();
inputs.velocity = velocitySignal.getValueAsDouble();
inputs.temperature = temperatureSignal.getValueAsDouble();
inputs.current = currentSignal.getValueAsDouble();
inputs.throughboreEncoderPosition = throughboreEncoder.get();
inputs.throughboreConnected = throughboreEncoder.isConnected();

Expand Down
51 changes: 46 additions & 5 deletions src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,19 +1,35 @@
package frc.robot.subsystems.climber;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.hardware.ParentDevice;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Temperature;
import edu.wpi.first.units.measure.Voltage;
import frc.robot.constants.ClimberConstants;
import frc.robot.util.PhoenixUtil;

public class ClimberIOTalonFX implements ClimberIO {
// TBD: Hardcode IDs or add support to make changeable in method
private final TalonFX climbermotor =
new TalonFX(ClimberConstants.CLIMBER_ID, ClimberConstants.CANbus);
private final MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0);

// Status signals for efficient updates
private final StatusSignal<Angle> positionSignal;
private final StatusSignal<Voltage> voltageSignal;
private final StatusSignal<AngularVelocity> speedSignal;
private final StatusSignal<Temperature> temperatureSignal;
private final StatusSignal<Current> currentSignal;

public ClimberIOTalonFX() {
climbermotor.setPosition(0);

Expand All @@ -36,14 +52,39 @@ public ClimberIOTalonFX() {
.withCurrentLimits(ClimberConstants.currentLimitsConfigs));

climbermotor.setNeutralMode(NeutralModeValue.Brake);

// Initialize status signals
positionSignal = climbermotor.getPosition();
voltageSignal = climbermotor.getMotorVoltage();
speedSignal = climbermotor.getVelocity();
temperatureSignal = climbermotor.getDeviceTemp();
currentSignal = climbermotor.getStatorCurrent();

// Set update frequency and optimize bus utilization
PhoenixUtil.tryUntilOk(
5,
() ->
BaseStatusSignal.setUpdateFrequencyForAll(
50.0,
positionSignal,
voltageSignal,
speedSignal,
temperatureSignal,
currentSignal));

PhoenixUtil.tryUntilOk(5, () -> ParentDevice.optimizeBusUtilizationForAll(0, climbermotor));
}

public void updateInputs(ClimberIOInputs inputs) {
inputs.position = climbermotor.getPosition().getValueAsDouble();
inputs.voltage = climbermotor.getMotorVoltage().getValueAsDouble();
inputs.speed = climbermotor.getVelocity().getValueAsDouble();
inputs.temperature = climbermotor.getDeviceTemp().getValueAsDouble();
inputs.current = climbermotor.getStatorCurrent().getValueAsDouble();
// Refresh all signals at once
BaseStatusSignal.refreshAll(
positionSignal, voltageSignal, speedSignal, temperatureSignal, currentSignal);

inputs.position = positionSignal.getValueAsDouble();
inputs.voltage = voltageSignal.getValueAsDouble();
inputs.speed = speedSignal.getValueAsDouble();
inputs.temperature = temperatureSignal.getValueAsDouble();
inputs.current = currentSignal.getValueAsDouble();
}

public void setVoltage(double voltage) {
Expand Down
Loading
Loading