diff --git a/simgui.json b/simgui.json index 7c54dd7..646a868 100644 --- a/simgui.json +++ b/simgui.json @@ -7,11 +7,9 @@ "/SmartDashboard/Module 0": "Mechanism2d", "/SmartDashboard/Module 1": "Mechanism2d", "/SmartDashboard/Module 2": "Mechanism2d", - "/SmartDashboard/Module 3": "Mechanism2d", "/SmartDashboard/PhotonSimField": "Field2d", - "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d", - "/AdvantageKit/RealOutputs/scoring/mechanism2d": "Mechanism2d" + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" } }, "NetworkTables": { @@ -32,30 +30,32 @@ }, "NetworkTables Info": { "Connections": { - "Server": { - "Publishers": { + "Server": { + "Publishers": { + "open": true + }, "open": true }, - "open": true + "visible": true }, - "visible": true - }, - "Plot": { - "Plot <0>": { - "plots": [ - { - "backgroundColor": [ - 0.0, - 0.0, - 0.0, - 0.8500000238418579 - ], - "height": 338 + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 338 + } + ], + "window": { + "visible": false } - ], - "window": { - "visible": false } - } + }, + "visible": true } -}} +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7948123..ef0ecf4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,7 +14,6 @@ import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { @@ -37,19 +36,20 @@ public void robotInit() { setUseTiming(false); Logger.addDataReceiver(new WPILOGWriter("logs/")); // This folder is gitignored Logger.addDataReceiver(new NT4Publisher()); - } else { - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope - // (or prompt the user) - Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - Logger.addDataReceiver( - new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save - // outputs - // to - // a - // new - // log - } + } /*TODO: Fix replay mode! + else { + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope + // (or prompt the user) + Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log + Logger.addDataReceiver( + new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save + // outputs + // to + // a + // new + // log + }*/ Logger.start(); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index b3b7506..78189fa 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -5,7 +5,7 @@ public interface IntakeIO { @AutoLog - public static class IntakeIOInputs { + public static class IntakeInputs { public double intakeMotorVoltage = 0.0; public double intakeMotorStatorCurrentAmps = 0.0; public double intakeMotorSupplyCurrentAmps = 0.0; @@ -18,15 +18,15 @@ public static class IntakeIOInputs { } @AutoLog - public static class IntakeIOOutputs { + public static class IntakeOutputs { public double intakeMotorVoltage = 0.0; public double centeringMotorVoltage = 0.0; } - public default void updateInputs(IntakeIOInputs inputs) {} + public default void updateInputs(IntakeInputs inputs) {} - public default void applyOutputs(IntakeIOOutputs outputs) {} + public default void applyOutputs(IntakeOutputs outputs) {} public default void setIntakeVoltage(double volts) {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 62a859d..c2c5b9f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -17,7 +17,7 @@ public class IntakeIOSim implements IntakeIO { public IntakeIOSim() {} @Override - public void updateInputs(IntakeIOInputs inputs) { + public void updateInputs(IntakeInputs inputs) { // inputs.leftIntakeVoltage = intakeWheelsAppliedVolts; // inputs.leftIntakeStatorCurrent = noteInIntakeWheels ? 100000 : 0; // inputs.rightIntakeVoltage = intakeWheelsAppliedVolts; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeNEOVortex.java b/src/main/java/frc/robot/subsystems/intake/IntakeNEOVortex.java index bf79b10..2a0b871 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeNEOVortex.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeNEOVortex.java @@ -28,7 +28,7 @@ public IntakeNEOVortex() { } @Override - public void updateInputs(IntakeIOInputs inputs) { + public void updateInputs(IntakeInputs inputs) { inputs.intakeMotorVoltage = intakeMotor.getAppliedOutput(); inputs.intakeMotorStatorCurrentAmps = intakeMotor.getOutputCurrent(); @@ -37,7 +37,7 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void applyOutputs(IntakeIOOutputs outputs) { + public void applyOutputs(IntakeOutputs outputs) { outputs.intakeMotorVoltage = intakeMotorVolts; outputs.centeringMotorVoltage = centeringMotorVolts; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 5373d77..1b746ce 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -8,8 +8,9 @@ public class IntakeSubsystem extends SubsystemBase { private IntakeIO io; - private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - private IntakeIOOutputsAutoLogged outputs = new IntakeIOOutputsAutoLogged(); + private IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged(); + private IntakeOutputsAutoLogged outputs = new IntakeOutputsAutoLogged(); + private State state = State.IDLE; private BooleanSupplier scorerWantsNote = () -> true; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java index d144f7f..019c3c8 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java @@ -4,7 +4,7 @@ public interface AimerIO { @AutoLog - public static class AimerIOInputs { + public static class AimerInputs { public double aimAngleRad = 0.0; public double aimGoalAngleRad = 0.0; public double aimProfileGoalAngleRad = 0.0; @@ -17,13 +17,13 @@ public static class AimerIOInputs { } @AutoLog - public static class AimerIOOutputs { + public static class AimerOutputs { public double aimAppliedVoltage = 0.0; } - public default void updateInputs(AimerIOInputs inputs) {} + public default void updateInputs(AimerInputs inputs) {} - public default void applyOutputs(AimerIOOutputs outputs) {} + public default void applyOutputs(AimerOutputs outputs) {} public default void setAimAngleRad(double angle, boolean newProfile) {} diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index 652056c..de68e50 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -182,7 +182,7 @@ public void setMotorDisabled(boolean disabled) { } @Override - public void updateInputs(AimerIOInputs inputs) { + public void updateInputs(AimerInputs inputs) { if (getEncoderPosition() == -1.75) { motorDisabled = true; @@ -211,7 +211,7 @@ public void updateInputs(AimerIOInputs inputs) { } @Override - public void applyOutputs(AimerIOOutputs outputs) { + public void applyOutputs(AimerOutputs outputs) { State trapezoidSetpoint = profile.calculate( diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java index 1d4f79c..cada3f7 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java @@ -103,7 +103,7 @@ public void setPID(double p, double i, double d) { } @Override - public void updateInputs(AimerIOInputs inputs) { + public void updateInputs(AimerInputs inputs) { sim.update(SimConstants.loopTime); inputs.aimGoalAngleRad = goalAngleRad; @@ -116,7 +116,7 @@ public void updateInputs(AimerIOInputs inputs) { } @Override - public void applyOutputs(AimerIOOutputs outputs) { + public void applyOutputs(AimerOutputs outputs) { State trapezoidSetpoint = profile.calculate( timer.get(), diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index eb8818a..a467941 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -28,12 +28,12 @@ public class ScoringSubsystem extends SubsystemBase implements Tunable { private final ShooterIO shooterIo; - private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); - private final ShooterIOOutputsAutoLogged shooterOutputs = new ShooterIOOutputsAutoLogged(); + private final ShooterInputsAutoLogged shooterInputs = new ShooterInputsAutoLogged(); + private final ShooterOutputsAutoLogged shooterOutputs = new ShooterOutputsAutoLogged(); private final AimerIO aimerIo; - private final AimerIOInputsAutoLogged aimerInputs = new AimerIOInputsAutoLogged(); - private final AimerIOOutputsAutoLogged aimerOutputs = new AimerIOOutputsAutoLogged(); + private final AimerInputsAutoLogged aimerInputs = new AimerInputsAutoLogged(); + private final AimerOutputsAutoLogged aimerOutputs = new AimerOutputsAutoLogged(); private final Timer shootTimer = new Timer(); diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java index 8288e4e..c6013da 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java @@ -4,7 +4,7 @@ public interface ShooterIO { @AutoLog - public static class ShooterIOInputs { + public static class ShooterInputs { public double shooterLeftVelocityRPM = 0.0; public double shooterLeftAppliedVolts = 0.0; public double shooterLeftStatorCurrentAmps = 0.0; @@ -22,7 +22,7 @@ public static class ShooterIOInputs { } @AutoLog - public static class ShooterIOOutputs { + public static class ShooterOutputs { public double shooterLeftGoalVelocityRPM = 0.0; public double shooterRightGoalVelocityRPM = 0.0; @@ -30,9 +30,9 @@ public static class ShooterIOOutputs { public double kickerGoalVolts = 0.0; } - public default void updateInputs(ShooterIOInputs inputs) {} + public default void updateInputs(ShooterInputs inputs) {} - public default void applyOutputs(ShooterIOOutputs outputs) {} + public default void applyOutputs(ShooterOutputs outputs) {} public default void setShooterVelocityRPM(double velocity) {} diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java index e94882e..d5cb53f 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java @@ -71,7 +71,7 @@ public void setPID(double p, double i, double d) { } @Override - public void updateInputs(ShooterIOInputs inputs) { + public void updateInputs(ShooterInputs inputs) { shooterLeftSim.update(SimConstants.loopTime); shooterRightSim.update(SimConstants.loopTime); @@ -95,7 +95,7 @@ public void updateInputs(ShooterIOInputs inputs) { } @Override - public void applyOutputs(ShooterIOOutputs outputs) { + public void applyOutputs(ShooterOutputs outputs) { outputs.shooterLeftGoalVelocityRPM = shooterLeftGoalVelRPM; outputs.shooterRightGoalVelocityRPM = shooterRightGoalVelRPM; diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java index 8f2953c..a21f465 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java @@ -110,7 +110,7 @@ public void setFF(double kS, double kV, double kA) { } @Override - public void updateInputs(ShooterIOInputs inputs) { + public void updateInputs(ShooterInputs inputs) { inputs.shooterLeftVelocityRPM = shooterLeft.getVelocity().getValueAsDouble() @@ -133,7 +133,7 @@ public void updateInputs(ShooterIOInputs inputs) { } @Override - public void applyOutputs(ShooterIOOutputs outputs) { + public void applyOutputs(ShooterOutputs outputs) { outputs.shooterLeftGoalVelocityRPM = goalLeftVelocityRPM; outputs.shooterRightGoalVelocityRPM = goalRightVelocityRPM; outputs.kickerGoalVolts = kickerVolts;