From a3d2b0463973d0cf011d4552b02376f48a384d22 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 4 Nov 2025 17:37:58 -0800 Subject: [PATCH 01/51] initial commit --- .vscode/settings.json | 4 +++- README.md | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 660d196..eae3fe5 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -18,7 +18,9 @@ { "name": "WPIlibUnitTests", "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": ["-Djava.library.path=${workspaceFolder}/build/jni/release"], + "vmargs": [ + "-Djava.library.path=${workspaceFolder}/build/jni/release" + ], "env": { "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" diff --git a/README.md b/README.md index c3a6fe9..7025d0b 100644 --- a/README.md +++ b/README.md @@ -56,4 +56,4 @@ double rotations = positionSignal.getValue().in(Units.Rotations); // Incorrect (won't compile) StatusSignal positionSignal = motor.getPosition(); // Type mismatch! -``` \ No newline at end of file +``` From 1bd30c548811ff7d8080ca5f6939679a3669407e Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Wed, 5 Nov 2025 10:35:58 -0800 Subject: [PATCH 02/51] added frame for shooter and intake subsystems --- src/main/java/frc/robot/subsystems/intake/Intake.java | 7 +++++++ src/main/java/frc/robot/subsystems/intake/Module.java | 5 +++++ src/main/java/frc/robot/subsystems/intake/ModuleIO.java | 5 +++++ .../frc/robot/subsystems/intake/ModuleIOTalonFXReal.java | 5 +++++ .../frc/robot/subsystems/intake/ModuleIOTalonFXSim.java | 5 +++++ .../java/frc/robot/subsystems/intake/ModuleIOTalonFx.java | 5 +++++ src/main/java/frc/robot/subsystems/shooter/Module.java | 5 +++++ src/main/java/frc/robot/subsystems/shooter/ModuleIO.java | 5 +++++ .../frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java | 5 +++++ .../frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java | 5 +++++ .../java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java | 5 +++++ src/main/java/frc/robot/subsystems/shooter/Shooter.java | 7 +++++++ 12 files changed, 64 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/Module.java create mode 100644 src/main/java/frc/robot/subsystems/intake/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXReal.java create mode 100644 src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXSim.java create mode 100644 src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFx.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/Module.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/Shooter.java diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..e3355c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase{ + +} diff --git a/src/main/java/frc/robot/subsystems/intake/Module.java b/src/main/java/frc/robot/subsystems/intake/Module.java new file mode 100644 index 0000000..d3a8d9e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Module.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.intake; + +public class Module { + +} diff --git a/src/main/java/frc/robot/subsystems/intake/ModuleIO.java b/src/main/java/frc/robot/subsystems/intake/ModuleIO.java new file mode 100644 index 0000000..5df5157 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/ModuleIO.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.intake; + +public class ModuleIO { + +} diff --git a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXReal.java new file mode 100644 index 0000000..2108202 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXReal.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.intake; + +public class ModuleIOTalonFXReal { + +} diff --git a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXSim.java new file mode 100644 index 0000000..908dfa7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.intake; + +public class ModuleIOTalonFXSim { + +} diff --git a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFx.java b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFx.java new file mode 100644 index 0000000..22ae34e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFx.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.intake; + +public class ModuleIOTalonFx { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Module.java b/src/main/java/frc/robot/subsystems/shooter/Module.java new file mode 100644 index 0000000..50ec864 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Module.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.shooter; + +public class Module { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java new file mode 100644 index 0000000..31babd4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.shooter; + +public class ModuleIO { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java new file mode 100644 index 0000000..4a7641e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.shooter; + +public class ModuleIOTalonFXReal { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java new file mode 100644 index 0000000..679b417 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.shooter; + +public class ModuleIOTalonFXSim { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java new file mode 100644 index 0000000..642c50f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.shooter; + +public class ModuleIOTalonFx { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..8ee5de8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase{ + +} From 4eb4bc0844b2111850540a659187f83386c4bc09 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Thu, 13 Nov 2025 18:12:32 -0800 Subject: [PATCH 03/51] Intake updates --- .../frc/robot/subsystems/intake/IntakeIO.java | 32 +++++ .../intake/IntakeIOTalonFXReal.java | 126 ++++++++++++++++++ .../{Module.java => IntakeIOTalonFXSim.java} | 2 +- .../frc/robot/subsystems/intake/ModuleIO.java | 5 - .../intake/ModuleIOTalonFXReal.java | 5 - .../subsystems/intake/ModuleIOTalonFXSim.java | 5 - .../subsystems/intake/ModuleIOTalonFx.java | 5 - 7 files changed, 159 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java rename src/main/java/frc/robot/subsystems/intake/{Module.java => IntakeIOTalonFXSim.java} (56%) delete mode 100644 src/main/java/frc/robot/subsystems/intake/ModuleIO.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXReal.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXSim.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFx.java diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..6e127a6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,32 @@ +package frc.robot.subsystems.intake; +import org.littletonrobotics.junction.AutoLog; +public class IntakeIO { + public interface IntakeIOInterface { + @AutoLog + class IntakeIOInputs{ + // Roller Motor + public double rollerCurrent = 0.0; + public double rollerVoltage = 0.0; + public double rollerPosition = 0.0; + public double rollerVelocity = 0.0; + + // Arm Motor + public double armCurrent = 0.0; + public double armVoltage = 0.0; + public double armPosition = 0.0; + public double armVelocity = 0.0; + + // Sensing + public boolean hasGamePiece = false; + } + void updateInputs(IntakeIOInterface.IntakeIOInputs inputs); + + default void setRollerVoltage(double voltage) {}; + default void setArmVoltage(double voltage) {}; + default void resetArmPosition() {}; + default void deployArm() {}; + default void stowArm() {}; + default void stopRoller() {}; + + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java new file mode 100644 index 0000000..d4af130 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -0,0 +1,126 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import frc.robot.generated.TunerConstants; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +public class IntakeIOTalonFXReal implements IntakeIO { + // Hardware configuration constants + private static final int ROLLER_MOTOR_ID = 20; + private static final double ROLLER_GEAR_RATIO = 3.0; + + // Hardware + private final TalonFX rollerMotor; + private final TalonFX arm = new TalonFX(21, TunerConstants.DrivetrainConstants.CANBusName); + + // Control requests + private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0); + private final VoltageOut voltageRequest = new VoltageOut(0.0); + + // Status signals + private final StatusSignal rollerVelocity; + private final StatusSignal rollerAppliedVolts; + private final StatusSignal rollerCurrent; + private final StatusSignal rollerTemp; + + // Connection debouncer + private final Debouncer rollerConnectedDebounce = new Debouncer(0.5); + + public IntakeIOTalonFX() { + // Initialize roller motor + rollerMotor = new TalonFX(ROLLER_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); + + // Configure roller motor + var config = new TalonFXConfiguration(); + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + // Velocity PID configuration + config.Slot0.kP = 0.1; + config.Slot0.kI = 0.0; + config.Slot0.kD = 0.0; + config.Slot0.kV = 0.12; + + // Current limits + config.CurrentLimits.StatorCurrentLimit = 40.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 30.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + + tryUntilOk(5, () -> rollerMotor.getConfigurator().apply(config, 0.25)); + + // Create status signals + rollerVelocity = rollerMotor.getVelocity(); + rollerAppliedVolts = rollerMotor.getMotorVoltage(); + rollerPosition = rollerMotor.getRollerPosition(); + rollerCurrent = rollerMotor.getStatorCurrent(); + armVelocity = arm.getArmVelocity(); + armCurrent = arm.getStatorCurrent(); + armPosition = arm.getArmPosition(); + armVoltage = arm.getMotorVoltage(); + + // Configure update frequencies + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + rollerVelocity, rollerAppliedVolts, rollerCurrent, rollerTemp + ); + rollerMotor.optimizeBusUtilization(); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + var status = BaseStatusSignal.refreshAll( + rollerVelocity, rollerCurrent, rollerPosition, rollerAppliedVolts, armVelocity, armCurrent, armPosition, armVoltage + ); + + inputs.rollerConnected = rollerConnectedDebounce.calculate(status.isOK()); + inputs.rollerVelocityRPM = Units.radiansToRotations( + rollerVelocity.getValueAsDouble() + ) * 60.0 / ROLLER_GEAR_RATIO; + inputs.rollerAppliedVolts = rollerAppliedVolts.getValueAsDouble(); + inputs.rollerCurrentAmps = rollerCurrent.getValueAsDouble(); + inputs.rollerTempCelsius = rollerTemp.getValueAsDouble(); + + inputs.deployed = deploySolenoid.get() == DoubleSolenoid.Value.kForward; + inputs.hasGamePiece = !beamBreak.get(); + } + + @Override + public void setRollerVelocity(double rpm) { + double rotPerSec = rpm / 60.0 * ROLLER_GEAR_RATIO; + rollerMotor.setControl(velocityRequest.withVelocity(rotPerSec)); + } + + @Override + public void setRollerVoltage(double volts) { + rollerMotor.setControl(voltageRequest.withOutput(volts)); + } + + @Override + public void stopRoller() { + rollerMotor.stopMotor(); + } + + @Override + public void deploy() { + deploySolenoid.set(DoubleSolenoid.Value.kForward); + } + + @Override + public void retract() { + deploySolenoid.set(DoubleSolenoid.Value.kReverse); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/Module.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java similarity index 56% rename from src/main/java/frc/robot/subsystems/intake/Module.java rename to src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index d3a8d9e..aa80373 100644 --- a/src/main/java/frc/robot/subsystems/intake/Module.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -1,5 +1,5 @@ package frc.robot.subsystems.intake; -public class Module { +public class IntakeIOTalonFXSim { } diff --git a/src/main/java/frc/robot/subsystems/intake/ModuleIO.java b/src/main/java/frc/robot/subsystems/intake/ModuleIO.java deleted file mode 100644 index 5df5157..0000000 --- a/src/main/java/frc/robot/subsystems/intake/ModuleIO.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.intake; - -public class ModuleIO { - -} diff --git a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXReal.java deleted file mode 100644 index 2108202..0000000 --- a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXReal.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.intake; - -public class ModuleIOTalonFXReal { - -} diff --git a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXSim.java deleted file mode 100644 index 908dfa7..0000000 --- a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFXSim.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.intake; - -public class ModuleIOTalonFXSim { - -} diff --git a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFx.java b/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFx.java deleted file mode 100644 index 22ae34e..0000000 --- a/src/main/java/frc/robot/subsystems/intake/ModuleIOTalonFx.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.intake; - -public class ModuleIOTalonFx { - -} From 4225512cd4002b3bffb60a332b77d9f60193e99e Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Thu, 13 Nov 2025 18:59:22 -0800 Subject: [PATCH 04/51] Added more code for intake --- .../frc/robot/subsystems/intake/Intake.java | 36 ++++- .../frc/robot/subsystems/intake/IntakeIO.java | 24 ++-- .../intake/IntakeIOTalonFXReal.java | 132 +++++++++++------- 3 files changed, 125 insertions(+), 67 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e3355c8..92e65ca 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,7 +1,39 @@ package frc.robot.subsystems.intake; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; -public class Intake extends SubsystemBase{ +public class Intake extends SubsystemBase { + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); -} + public Intake(IntakeIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + } + + public void setRollerVoltage(double voltage) { + io.setRollerVoltage(voltage); + } + + public void deployArm() { + io.deployArm(); + } + + public void stowArm() { + io.stowArm(); + } + + public boolean hasGamePiece() { + return inputs.hasGamePiece; + } + + public void stop() { + io.stopRoller(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 6e127a6..95ec1a5 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -1,9 +1,12 @@ package frc.robot.subsystems.intake; import org.littletonrobotics.junction.AutoLog; + +import frc.robot.subsystems.intake.IntakeIO.IntakeIOInputs; + public class IntakeIO { - public interface IntakeIOInterface { + public interface IntakeIO { @AutoLog - class IntakeIOInputs{ + class IntakeIOInputs { // Roller Motor public double rollerCurrent = 0.0; public double rollerVoltage = 0.0; @@ -19,14 +22,13 @@ class IntakeIOInputs{ // Sensing public boolean hasGamePiece = false; } - void updateInputs(IntakeIOInterface.IntakeIOInputs inputs); - - default void setRollerVoltage(double voltage) {}; - default void setArmVoltage(double voltage) {}; - default void resetArmPosition() {}; - default void deployArm() {}; - default void stowArm() {}; - default void stopRoller() {}; - + + default void updateInputs(IntakeIOInputs inputs) {} + default void setRollerVoltage(double voltage) {} + default void setArmVoltage(double voltage) {} + default void resetArmPosition() {} + default void deployArm() {} + default void stowArm() {} + default void stopRoller() {} } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index d4af130..26f6a0a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -3,7 +3,6 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -20,107 +19,132 @@ public class IntakeIOTalonFXReal implements IntakeIO { // Hardware configuration constants private static final int ROLLER_MOTOR_ID = 20; + private static final int ARM_MOTOR_ID = 21; private static final double ROLLER_GEAR_RATIO = 3.0; + private static final double ARM_GEAR_RATIO = 100.0; // Example value // Hardware private final TalonFX rollerMotor; - private final TalonFX arm = new TalonFX(21, TunerConstants.DrivetrainConstants.CANBusName); + private final TalonFX armMotor; // Control requests - private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0); private final VoltageOut voltageRequest = new VoltageOut(0.0); - // Status signals + // Status signals - Roller + private final StatusSignal rollerPosition; private final StatusSignal rollerVelocity; private final StatusSignal rollerAppliedVolts; private final StatusSignal rollerCurrent; - private final StatusSignal rollerTemp; - // Connection debouncer - private final Debouncer rollerConnectedDebounce = new Debouncer(0.5); + // Status signals - Arm + private final StatusSignal armPosition; + private final StatusSignal armVelocity; + private final StatusSignal armAppliedVolts; + private final StatusSignal armCurrent; - public IntakeIOTalonFX() { - // Initialize roller motor + public IntakeIOTalonFXReal() { + // Initialize hardware rollerMotor = new TalonFX(ROLLER_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); + armMotor = new TalonFX(ARM_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); // Configure roller motor - var config = new TalonFXConfiguration(); - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + var rollerConfig = new TalonFXConfiguration(); + rollerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + rollerConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - // Velocity PID configuration - config.Slot0.kP = 0.1; - config.Slot0.kI = 0.0; - config.Slot0.kD = 0.0; - config.Slot0.kV = 0.12; + // Current limits for roller + rollerConfig.CurrentLimits.StatorCurrentLimit = 40.0; + rollerConfig.CurrentLimits.StatorCurrentLimitEnable = true; + rollerConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + rollerConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - // Current limits - config.CurrentLimits.StatorCurrentLimit = 40.0; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = 30.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; + tryUntilOk(5, () -> rollerMotor.getConfigurator().apply(rollerConfig, 0.25)); - tryUntilOk(5, () -> rollerMotor.getConfigurator().apply(config, 0.25)); + // Configure arm motor + var armConfig = new TalonFXConfiguration(); + armConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + armConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - // Create status signals + // Current limits for arm + armConfig.CurrentLimits.StatorCurrentLimit = 30.0; + armConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + // PID for arm position control + armConfig.Slot0.kP = 10.0; + armConfig.Slot0.kI = 0.0; + armConfig.Slot0.kD = 0.5; + + tryUntilOk(5, () -> armMotor.getConfigurator().apply(armConfig, 0.25)); + + // Create status signals - Roller + rollerPosition = rollerMotor.getPosition(); rollerVelocity = rollerMotor.getVelocity(); rollerAppliedVolts = rollerMotor.getMotorVoltage(); - rollerPosition = rollerMotor.getRollerPosition(); rollerCurrent = rollerMotor.getStatorCurrent(); - armVelocity = arm.getArmVelocity(); - armCurrent = arm.getStatorCurrent(); - armPosition = arm.getArmPosition(); - armVoltage = arm.getMotorVoltage(); + + // Create status signals - Arm + armPosition = armMotor.getPosition(); + armVelocity = armMotor.getVelocity(); + armAppliedVolts = armMotor.getMotorVoltage(); + armCurrent = armMotor.getStatorCurrent(); // Configure update frequencies BaseStatusSignal.setUpdateFrequencyForAll( 50.0, - rollerVelocity, rollerAppliedVolts, rollerCurrent, rollerTemp + rollerPosition, rollerVelocity, rollerAppliedVolts, rollerCurrent, + armPosition, armVelocity, armAppliedVolts, armCurrent ); rollerMotor.optimizeBusUtilization(); + armMotor.optimizeBusUtilization(); } @Override public void updateInputs(IntakeIOInputs inputs) { - var status = BaseStatusSignal.refreshAll( - rollerVelocity, rollerCurrent, rollerPosition, rollerAppliedVolts, armVelocity, armCurrent, armPosition, armVoltage + var rollerStatus = BaseStatusSignal.refreshAll( + rollerPosition, rollerVelocity, rollerAppliedVolts, rollerCurrent + ); + var armStatus = BaseStatusSignal.refreshAll( + armPosition, armVelocity, armAppliedVolts, armCurrent ); - inputs.rollerConnected = rollerConnectedDebounce.calculate(status.isOK()); - inputs.rollerVelocityRPM = Units.radiansToRotations( + // Roller inputs + inputs.rollerPosition = Units.rotationsToRadians( + rollerPosition.getValueAsDouble() + ) / ROLLER_GEAR_RATIO; + inputs.rollerVelocity = Units.rotationsToRadians( rollerVelocity.getValueAsDouble() - ) * 60.0 / ROLLER_GEAR_RATIO; - inputs.rollerAppliedVolts = rollerAppliedVolts.getValueAsDouble(); - inputs.rollerCurrentAmps = rollerCurrent.getValueAsDouble(); - inputs.rollerTempCelsius = rollerTemp.getValueAsDouble(); + ) / ROLLER_GEAR_RATIO; + inputs.rollerVoltage = rollerAppliedVolts.getValueAsDouble(); + inputs.rollerCurrent = rollerCurrent.getValueAsDouble(); - inputs.deployed = deploySolenoid.get() == DoubleSolenoid.Value.kForward; - inputs.hasGamePiece = !beamBreak.get(); - } - - @Override - public void setRollerVelocity(double rpm) { - double rotPerSec = rpm / 60.0 * ROLLER_GEAR_RATIO; - rollerMotor.setControl(velocityRequest.withVelocity(rotPerSec)); + // Arm inputs + inputs.armPosition = Units.rotationsToRadians( + armPosition.getValueAsDouble() + ) / ARM_GEAR_RATIO; + inputs.armVelocity = Units.rotationsToRadians( + armVelocity.getValueAsDouble() + ) / ARM_GEAR_RATIO; + inputs.armVoltage = armAppliedVolts.getValueAsDouble(); + inputs.armCurrent = armCurrent.getValueAsDouble(); } @Override - public void setRollerVoltage(double volts) { - rollerMotor.setControl(voltageRequest.withOutput(volts)); + public void setRollerVoltage(double voltage) { + rollerMotor.setControl(voltageRequest.withOutput(voltage)); } @Override - public void stopRoller() { - rollerMotor.stopMotor(); + public void setArmVoltage(double voltage) { + armMotor.setControl(voltageRequest.withOutput(voltage)); } @Override - public void deploy() { - deploySolenoid.set(DoubleSolenoid.Value.kForward); + public void resetArmPosition() { + tryUntilOk(5, () -> armMotor.setPosition(0.0, 0.25)); } @Override - public void retract() { - deploySolenoid.set(DoubleSolenoid.Value.kReverse); + public void stopRoller() { + rollerMotor.stopMotor(); } } \ No newline at end of file From 0930f44ddfac8b10df0fcd7ffbff8b702577402b Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Thu, 13 Nov 2025 19:29:33 -0800 Subject: [PATCH 05/51] Updated Intake implementation --- .../frc/robot/subsystems/intake/Intake.java | 31 ++++++++++ .../frc/robot/subsystems/intake/IntakeIO.java | 59 ++++++++++--------- .../intake/IntakeIOTalonFXReal.java | 31 +++++++++- 3 files changed, 90 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 92e65ca..07a5fd7 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -17,23 +17,54 @@ public void periodic() { Logger.processInputs("Intake", inputs); } + /** Set roller voltage (-12 to +12 volts) */ public void setRollerVoltage(double voltage) { io.setRollerVoltage(voltage); } + /** Set arm voltage (-12 to +12 volts) */ + public void setArmVoltage(double voltage) { + io.setArmVoltage(voltage); + } + + /** Deploy the intake arm */ public void deployArm() { io.deployArm(); } + /** Stow the intake arm */ public void stowArm() { io.stowArm(); } + /** Reset arm encoder position to zero */ + public void resetArmPosition() { + io.resetArmPosition(); + } + + /** Check if a game piece is detected */ public boolean hasGamePiece() { return inputs.hasGamePiece; } + /** Get roller position in radians */ + public double getRollerPositionRad() { + return inputs.rollerPosition; + } + + /** Get roller velocity in rad/s */ + public double getRollerVelocityRadPerSec() { + return inputs.rollerVelocity; + } + + /** Get arm position in radians */ + public double getArmPositionRad() { + return inputs.armPosition; + } + + /** Stop all motors */ public void stop() { io.stopRoller(); + io.setArmVoltage(0.0); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 95ec1a5..701524a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -1,34 +1,37 @@ package frc.robot.subsystems.intake; -import org.littletonrobotics.junction.AutoLog; -import frc.robot.subsystems.intake.IntakeIO.IntakeIOInputs; +import org.littletonrobotics.junction.AutoLog; -public class IntakeIO { - public interface IntakeIO { - @AutoLog - class IntakeIOInputs { - // Roller Motor - public double rollerCurrent = 0.0; - public double rollerVoltage = 0.0; - public double rollerPosition = 0.0; - public double rollerVelocity = 0.0; +public interface IntakeIO { + @AutoLog + class IntakeIOInputs { + // Roller Motor + public double rollerCurrent = 0.0; + public double rollerVoltage = 0.0; + public double rollerPosition = 0.0; + public double rollerVelocity = 0.0; - // Arm Motor - public double armCurrent = 0.0; - public double armVoltage = 0.0; - public double armPosition = 0.0; - public double armVelocity = 0.0; + // Arm Motor + public double armCurrent = 0.0; + public double armVoltage = 0.0; + public double armPosition = 0.0; + public double armVelocity = 0.0; - // Sensing - public boolean hasGamePiece = false; - } - - default void updateInputs(IntakeIOInputs inputs) {} - default void setRollerVoltage(double voltage) {} - default void setArmVoltage(double voltage) {} - default void resetArmPosition() {} - default void deployArm() {} - default void stowArm() {} - default void stopRoller() {} + // Sensing + public boolean hasGamePiece = false; } -} + + default void updateInputs(IntakeIOInputs inputs) {} + + default void setRollerVoltage(double voltage) {} + + default void setArmVoltage(double voltage) {} + + default void resetArmPosition() {} + + default void deployArm() {} + + default void stowArm() {} + + default void stopRoller() {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 26f6a0a..7b87b26 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.DigitalInput; @@ -21,11 +20,18 @@ public class IntakeIOTalonFXReal implements IntakeIO { private static final int ROLLER_MOTOR_ID = 20; private static final int ARM_MOTOR_ID = 21; private static final double ROLLER_GEAR_RATIO = 3.0; - private static final double ARM_GEAR_RATIO = 100.0; // Example value + private static final double ARM_GEAR_RATIO = 100.0; + + // Pneumatics and sensor IDs + private static final int DEPLOY_SOLENOID_FORWARD = 0; + private static final int DEPLOY_SOLENOID_REVERSE = 1; + private static final int BEAM_BREAK_DIO = 0; // Hardware private final TalonFX rollerMotor; private final TalonFX armMotor; + private final DoubleSolenoid deploySolenoid; + private final DigitalInput beamBreak; // Control requests private final VoltageOut voltageRequest = new VoltageOut(0.0); @@ -46,6 +52,12 @@ public IntakeIOTalonFXReal() { // Initialize hardware rollerMotor = new TalonFX(ROLLER_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); armMotor = new TalonFX(ARM_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); + deploySolenoid = new DoubleSolenoid( + PneumaticsModuleType.REVPH, + DEPLOY_SOLENOID_FORWARD, + DEPLOY_SOLENOID_REVERSE + ); + beamBreak = new DigitalInput(BEAM_BREAK_DIO); // Configure roller motor var rollerConfig = new TalonFXConfiguration(); @@ -69,7 +81,7 @@ public IntakeIOTalonFXReal() { armConfig.CurrentLimits.StatorCurrentLimit = 30.0; armConfig.CurrentLimits.StatorCurrentLimitEnable = true; - // PID for arm position control + // PID for arm position control (if needed) armConfig.Slot0.kP = 10.0; armConfig.Slot0.kI = 0.0; armConfig.Slot0.kD = 0.5; @@ -126,6 +138,9 @@ public void updateInputs(IntakeIOInputs inputs) { ) / ARM_GEAR_RATIO; inputs.armVoltage = armAppliedVolts.getValueAsDouble(); inputs.armCurrent = armCurrent.getValueAsDouble(); + + // Sensor inputs + inputs.hasGamePiece = !beamBreak.get(); // Beam break is typically active low } @Override @@ -143,6 +158,16 @@ public void resetArmPosition() { tryUntilOk(5, () -> armMotor.setPosition(0.0, 0.25)); } + @Override + public void deployArm() { + deploySolenoid.set(DoubleSolenoid.Value.kForward); + } + + @Override + public void stowArm() { + deploySolenoid.set(DoubleSolenoid.Value.kReverse); + } + @Override public void stopRoller() { rollerMotor.stopMotor(); From bd6b38ff448ae98cc7d34bc5cee70f3cbe8d6679 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Fri, 14 Nov 2025 13:56:07 -0800 Subject: [PATCH 06/51] Got rid of auto generated solenoid usage --- .../subsystems/intake/IntakeIOTalonFXReal.java | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 7b87b26..b985d05 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -140,7 +140,7 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armCurrent = armCurrent.getValueAsDouble(); // Sensor inputs - inputs.hasGamePiece = !beamBreak.get(); // Beam break is typically active low + inputs.hasGamePiece = inputs.rollerCurrent>50 ? true:false;//CHANGE LATER DEPENDING ON ACTUAL CURRENT READINGS } @Override @@ -157,15 +157,11 @@ public void setArmVoltage(double voltage) { public void resetArmPosition() { tryUntilOk(5, () -> armMotor.setPosition(0.0, 0.25)); } - - @Override - public void deployArm() { - deploySolenoid.set(DoubleSolenoid.Value.kForward); - } - + + //ADD METHODS FOR DEPLOYING AND STOWING THE INTAKE @Override - public void stowArm() { - deploySolenoid.set(DoubleSolenoid.Value.kReverse); + public void deployIntake(){ + armMotor.setPosition(); } @Override From 3bd13a6d4a0c8482cbebc7379bc66f7973a53de3 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Sat, 15 Nov 2025 20:34:50 -0800 Subject: [PATCH 07/51] Completed basic intake implementation. Sim implementation will need tuning --- src/main/java/frc/robot/RobotContainer.java | 46 +++---- .../frc/robot/subsystems/intake/Intake.java | 29 ++++- .../frc/robot/subsystems/intake/IntakeIO.java | 6 +- .../intake/IntakeIOTalonFXReal.java | 38 +++--- .../subsystems/intake/IntakeIOTalonFXSim.java | 117 +++++++++++++++++- 5 files changed, 174 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d8ffb1..87921b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,6 @@ import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Degrees; import static frc.robot.subsystems.vision.VisionConstants.*; -import static frc.robot.subsystems.vision.VisionConstants.robotToCamera1; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -32,6 +31,7 @@ import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.vision.*; +import frc.robot.subsystems.intake.*; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly; @@ -47,6 +47,7 @@ public class RobotContainer { // Subsystems private final Drive drive; private final Vision vision; + private final Intake intake; private SwerveDriveSimulation driveSimulation = null; @@ -72,7 +73,7 @@ public RobotContainer() { drive, new VisionIOLimelight(VisionConstants.camera0Name, drive::getRotation), new VisionIOLimelight(VisionConstants.camera1Name, drive::getRotation)); - + intake = new Intake(new IntakeIOTalonFXReal()); break; case SIM: // Sim robot, instantiate physics sim IO implementations @@ -96,7 +97,7 @@ public RobotContainer() { camera0Name, robotToCamera0, driveSimulation::getSimulatedDriveTrainPose), new VisionIOPhotonVisionSim( camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); - + intake = new Intake(new IntakeIOTalonFXSim()); break; default: @@ -109,7 +110,7 @@ public RobotContainer() { new ModuleIO() {}, (pose) -> {}); vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); - + intake = new Intake(new IntakeIO() {}); break; } @@ -156,31 +157,18 @@ private void configureButtonBindings() { // simulation : () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); - - // Example Coral Placement Code - // TODO: delete these code for your own project - if (Constants.currentMode == Constants.Mode.SIM) { - // L4 placement - controller.y().onTrue(Commands.runOnce(() -> SimulatedArena.getInstance() - .addGamePieceProjectile(new ReefscapeCoralOnFly( - driveSimulation.getSimulatedDriveTrainPose().getTranslation(), - new Translation2d(0.4, 0), - driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(), - driveSimulation.getSimulatedDriveTrainPose().getRotation(), - Meters.of(2), - MetersPerSecond.of(1.5), - Degrees.of(-80))))); - // L3 placement - controller.b().onTrue(Commands.runOnce(() -> SimulatedArena.getInstance() - .addGamePieceProjectile(new ReefscapeCoralOnFly( - driveSimulation.getSimulatedDriveTrainPose().getTranslation(), - new Translation2d(0.4, 0), - driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(), - driveSimulation.getSimulatedDriveTrainPose().getRotation(), - Meters.of(1.35), - MetersPerSecond.of(1.5), - Degrees.of(-60))))); - } + controller.rightBumper() + .whileTrue(Commands.startEnd( + () -> { + intake.deployArm(); + intake.setRollerVoltage(8.0); + }, + () -> { + intake.stowArm(); + intake.stop(); + }, + intake + )); } /** diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 07a5fd7..0967bca 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -4,6 +4,10 @@ import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { + // Arm position constants (radians) + public static final double ARM_STOWED_POSITION = 0.0; + public static final double ARM_DEPLOYED_POSITION = Math.PI / 2; // 90 degrees + private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); @@ -22,19 +26,24 @@ public void setRollerVoltage(double voltage) { io.setRollerVoltage(voltage); } - /** Set arm voltage (-12 to +12 volts) */ + /** Set arm voltage (-12 to +12 volts) for manual control */ public void setArmVoltage(double voltage) { io.setArmVoltage(voltage); } - /** Deploy the intake arm */ + /** Set arm to a specific position in radians */ + public void setArmPosition(double positionRad) { + io.setArmPosition(positionRad); + } + + /** Deploy the intake arm to deployed position */ public void deployArm() { - io.deployArm(); + io.setArmPosition(ARM_DEPLOYED_POSITION); } - /** Stow the intake arm */ + /** Stow the intake arm to stowed position */ public void stowArm() { - io.stowArm(); + io.setArmPosition(ARM_STOWED_POSITION); } /** Reset arm encoder position to zero */ @@ -62,6 +71,16 @@ public double getArmPositionRad() { return inputs.armPosition; } + /** Get arm velocity in rad/s */ + public double getArmVelocityRadPerSec() { + return inputs.armVelocity; + } + + /** Check if arm is at target position (within tolerance) */ + public boolean isArmAtPosition(double targetRad, double toleranceRad) { + return Math.abs(inputs.armPosition - targetRad) < toleranceRad; + } + /** Stop all motors */ public void stop() { io.stopRoller(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 701524a..3616812 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -27,11 +27,9 @@ default void setRollerVoltage(double voltage) {} default void setArmVoltage(double voltage) {} - default void resetArmPosition() {} - - default void deployArm() {} + default void setArmPosition(double positionRad) {} - default void stowArm() {} + default void resetArmPosition() {} default void stopRoller() {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index b985d05..bf8adf8 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -10,8 +11,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; import frc.robot.generated.TunerConstants; import static frc.robot.util.PhoenixUtil.tryUntilOk; @@ -22,19 +21,17 @@ public class IntakeIOTalonFXReal implements IntakeIO { private static final double ROLLER_GEAR_RATIO = 3.0; private static final double ARM_GEAR_RATIO = 100.0; - // Pneumatics and sensor IDs - private static final int DEPLOY_SOLENOID_FORWARD = 0; - private static final int DEPLOY_SOLENOID_REVERSE = 1; + // Beam break sensor private static final int BEAM_BREAK_DIO = 0; // Hardware private final TalonFX rollerMotor; private final TalonFX armMotor; - private final DoubleSolenoid deploySolenoid; private final DigitalInput beamBreak; // Control requests private final VoltageOut voltageRequest = new VoltageOut(0.0); + private final PositionVoltage positionRequest = new PositionVoltage(0.0); // Status signals - Roller private final StatusSignal rollerPosition; @@ -52,11 +49,6 @@ public IntakeIOTalonFXReal() { // Initialize hardware rollerMotor = new TalonFX(ROLLER_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); armMotor = new TalonFX(ARM_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); - deploySolenoid = new DoubleSolenoid( - PneumaticsModuleType.REVPH, - DEPLOY_SOLENOID_FORWARD, - DEPLOY_SOLENOID_REVERSE - ); beamBreak = new DigitalInput(BEAM_BREAK_DIO); // Configure roller motor @@ -81,10 +73,12 @@ public IntakeIOTalonFXReal() { armConfig.CurrentLimits.StatorCurrentLimit = 30.0; armConfig.CurrentLimits.StatorCurrentLimitEnable = true; - // PID for arm position control (if needed) + // PID for arm position control armConfig.Slot0.kP = 10.0; armConfig.Slot0.kI = 0.0; armConfig.Slot0.kD = 0.5; + armConfig.Slot0.kS = 0.0; + armConfig.Slot0.kV = 0.0; tryUntilOk(5, () -> armMotor.getConfigurator().apply(armConfig, 0.25)); @@ -112,10 +106,10 @@ public IntakeIOTalonFXReal() { @Override public void updateInputs(IntakeIOInputs inputs) { - var rollerStatus = BaseStatusSignal.refreshAll( + BaseStatusSignal.refreshAll( rollerPosition, rollerVelocity, rollerAppliedVolts, rollerCurrent ); - var armStatus = BaseStatusSignal.refreshAll( + BaseStatusSignal.refreshAll( armPosition, armVelocity, armAppliedVolts, armCurrent ); @@ -139,8 +133,8 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armVoltage = armAppliedVolts.getValueAsDouble(); inputs.armCurrent = armCurrent.getValueAsDouble(); - // Sensor inputs - inputs.hasGamePiece = inputs.rollerCurrent>50 ? true:false;//CHANGE LATER DEPENDING ON ACTUAL CURRENT READINGS + // Sensor inputs - beam break is inverted (false = blocked) + inputs.hasGamePiece = !beamBreak.get(); } @Override @@ -154,14 +148,14 @@ public void setArmVoltage(double voltage) { } @Override - public void resetArmPosition() { - tryUntilOk(5, () -> armMotor.setPosition(0.0, 0.25)); + public void setArmPosition(double positionRad) { + double motorRotations = Units.radiansToRotations(positionRad) * ARM_GEAR_RATIO; + armMotor.setControl(positionRequest.withPosition(motorRotations)); } - - //ADD METHODS FOR DEPLOYING AND STOWING THE INTAKE + @Override - public void deployIntake(){ - armMotor.setPosition(); + public void resetArmPosition() { + tryUntilOk(5, () -> armMotor.setPosition(0.0, 0.25)); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index aa80373..dbea029 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -1,5 +1,118 @@ package frc.robot.subsystems.intake; -public class IntakeIOTalonFXSim { +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class IntakeIOTalonFXSim implements IntakeIO { + // Hardware configuration constants + private static final double ROLLER_GEAR_RATIO = 3.0; + private static final double ARM_GEAR_RATIO = 100.0; + + // Simulation parameters + private static final double ROLLER_MOI = 0.001; // kg*m^2 + private static final double ARM_MOI = 0.1; // kg*m^2 + + // Arm position constants (radians) + private static final double ARM_MIN_ANGLE = 0.0; + private static final double ARM_MAX_ANGLE = Math.PI / 2; + + // Motor simulations + private final DCMotorSim rollerSim; + private final DCMotorSim armSim; + + // Applied voltages + private double rollerAppliedVolts = 0.0; + private double armAppliedVolts = 0.0; + + // Simulated game piece detection + private boolean simulatedGamePiece = false; + + public IntakeIOTalonFXSim() { + // Kraken X60 for roller (1 motor, geared) + // Using LinearSystemId to create the plant + rollerSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ROLLER_MOI, ROLLER_GEAR_RATIO), + DCMotor.getKrakenX60(1) + ); + + // Kraken X60 for arm (1 motor, highly geared) + armSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ARM_MOI, ARM_GEAR_RATIO), + DCMotor.getKrakenX60(1) + ); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + // Update simulations + rollerSim.update(0.02); + armSim.update(0.02); + + // Clamp arm position to realistic bounds + if (armSim.getAngularPositionRad() < ARM_MIN_ANGLE) { + armSim.setState(ARM_MIN_ANGLE, 0.0); + } else if (armSim.getAngularPositionRad() > ARM_MAX_ANGLE) { + armSim.setState(ARM_MAX_ANGLE, 0.0); + } + + // Update roller inputs + inputs.rollerPosition = rollerSim.getAngularPositionRad(); + inputs.rollerVelocity = rollerSim.getAngularVelocityRadPerSec(); + inputs.rollerVoltage = rollerAppliedVolts; + inputs.rollerCurrent = rollerSim.getCurrentDrawAmps(); + + // Update arm inputs + inputs.armPosition = armSim.getAngularPositionRad(); + inputs.armVelocity = armSim.getAngularVelocityRadPerSec(); + inputs.armVoltage = armAppliedVolts; + inputs.armCurrent = armSim.getCurrentDrawAmps(); + + // Simulate game piece detection based on roller speed + // If roller is spinning fast enough, assume we have a game piece + if (Math.abs(inputs.rollerVelocity) > 5.0) { // rad/s threshold + simulatedGamePiece = true; + } + inputs.hasGamePiece = simulatedGamePiece; + } + + @Override + public void setRollerVoltage(double voltage) { + rollerAppliedVolts = voltage; + rollerSim.setInputVoltage(voltage); + } + + @Override + public void setArmVoltage(double voltage) { + armAppliedVolts = voltage; + armSim.setInputVoltage(voltage); + } + + @Override + public void setArmPosition(double positionRad) { + // Simple P controller for position control in sim + double kP = 10.0; + double error = positionRad - armSim.getAngularPositionRad(); + double voltage = error * kP; + + // Clamp voltage + voltage = Math.max(-12.0, Math.min(12.0, voltage)); + + setArmVoltage(voltage); + } + + @Override + public void resetArmPosition() { + armSim.setState(ARM_MIN_ANGLE, 0.0); + } + + @Override + public void stopRoller() { + setRollerVoltage(0.0); + } -} + /** For testing - manually set game piece detection */ + public void setSimulatedGamePiece(boolean hasGamePiece) { + this.simulatedGamePiece = hasGamePiece; + } +} \ No newline at end of file From 8b40b8495273af12c03bd8e87cec4c0eb3047a46 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Sat, 15 Nov 2025 20:40:53 -0800 Subject: [PATCH 08/51] cleaned up some stuff --- .../frc/robot/subsystems/intake/Intake.java | 26 +++++++++---------- .../subsystems/intake/IntakeIOTalonFXSim.java | 2 +- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 0967bca..a45173a 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -21,67 +21,67 @@ public void periodic() { Logger.processInputs("Intake", inputs); } - /** Set roller voltage (-12 to +12 volts) */ + //Set roller voltage (-12 to +12 volts) public void setRollerVoltage(double voltage) { io.setRollerVoltage(voltage); } - /** Set arm voltage (-12 to +12 volts) for manual control */ + //Set arm voltage (-12 to +12 volts) for manual control public void setArmVoltage(double voltage) { io.setArmVoltage(voltage); } - /** Set arm to a specific position in radians */ + //Set arm to a specific position in radians public void setArmPosition(double positionRad) { io.setArmPosition(positionRad); } - /** Deploy the intake arm to deployed position */ + // Deploy the intake arm to deployed position public void deployArm() { io.setArmPosition(ARM_DEPLOYED_POSITION); } - /** Stow the intake arm to stowed position */ + // Stow the intake arm to stowed position public void stowArm() { io.setArmPosition(ARM_STOWED_POSITION); } - /** Reset arm encoder position to zero */ + // Reset arm encoder position to zero public void resetArmPosition() { io.resetArmPosition(); } - /** Check if a game piece is detected */ + // Check if a game piece is detected public boolean hasGamePiece() { return inputs.hasGamePiece; } - /** Get roller position in radians */ + // Get roller position in radians public double getRollerPositionRad() { return inputs.rollerPosition; } - /** Get roller velocity in rad/s */ + //Get roller velocity in rad/s public double getRollerVelocityRadPerSec() { return inputs.rollerVelocity; } - /** Get arm position in radians */ + // Get arm position in radians public double getArmPositionRad() { return inputs.armPosition; } - /** Get arm velocity in rad/s */ + // Get arm velocity in rad/s public double getArmVelocityRadPerSec() { return inputs.armVelocity; } - /** Check if arm is at target position (within tolerance) */ + // Check if arm is at target position (within tolerance) public boolean isArmAtPosition(double targetRad, double toleranceRad) { return Math.abs(inputs.armPosition - targetRad) < toleranceRad; } - /** Stop all motors */ + // Stop all motors public void stop() { io.stopRoller(); io.setArmVoltage(0.0); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index dbea029..73182dd 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -111,7 +111,7 @@ public void stopRoller() { setRollerVoltage(0.0); } - /** For testing - manually set game piece detection */ + // For testing - manually set game piece detection public void setSimulatedGamePiece(boolean hasGamePiece) { this.simulatedGamePiece = hasGamePiece; } From 9e7576e8d406ef1f1edf910f62b81c3cfae36942 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Sat, 15 Nov 2025 20:43:10 -0800 Subject: [PATCH 09/51] Renamed and deleted unecessary files in prep for shooter code --- .../frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java | 5 ----- .../java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java | 5 ----- .../robot/subsystems/shooter/{Module.java => ShooterIO.java} | 2 +- .../{ModuleIOTalonFXReal.java => ShooterIOTalonFXReal.java} | 2 +- .../shooter/{ModuleIO.java => ShooterIOTalonFXSim.java} | 2 +- 5 files changed, 3 insertions(+), 13 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java delete mode 100644 src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java rename src/main/java/frc/robot/subsystems/shooter/{Module.java => ShooterIO.java} (64%) rename src/main/java/frc/robot/subsystems/shooter/{ModuleIOTalonFXReal.java => ShooterIOTalonFXReal.java} (56%) rename src/main/java/frc/robot/subsystems/shooter/{ModuleIO.java => ShooterIOTalonFXSim.java} (56%) diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java deleted file mode 100644 index 679b417..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.shooter; - -public class ModuleIOTalonFXSim { - -} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java deleted file mode 100644 index 642c50f..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.subsystems.shooter; - -public class ModuleIOTalonFx { - -} diff --git a/src/main/java/frc/robot/subsystems/shooter/Module.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java similarity index 64% rename from src/main/java/frc/robot/subsystems/shooter/Module.java rename to src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 50ec864..05f3014 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Module.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -1,5 +1,5 @@ package frc.robot.subsystems.shooter; -public class Module { +public class ShooterIO { } diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java similarity index 56% rename from src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java rename to src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java index 4a7641e..47bee02 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java @@ -1,5 +1,5 @@ package frc.robot.subsystems.shooter; -public class ModuleIOTalonFXReal { +public class ShooterIOTalonFXReal { } diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java similarity index 56% rename from src/main/java/frc/robot/subsystems/shooter/ModuleIO.java rename to src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java index 31babd4..645a9c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java @@ -1,5 +1,5 @@ package frc.robot.subsystems.shooter; -public class ModuleIO { +public class ShooterIOTalonFXSim { } From 7bcd3c34960bd7fea332d8e11673bf064ea95133 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Thu, 20 Nov 2025 16:38:03 -0800 Subject: [PATCH 10/51] Updated tuner constants --- src/main/java/frc/robot/RobotContainer.java | 29 +++--- .../frc/robot/generated/TunerConstants.java | 22 ++--- .../frc/robot/subsystems/intake/Intake.java | 34 +++---- .../frc/robot/subsystems/intake/IntakeIO.java | 14 +-- .../intake/IntakeIOTalonFXReal.java | 92 +++++++++---------- .../subsystems/intake/IntakeIOTalonFXSim.java | 52 +++++------ .../frc/robot/subsystems/shooter/Module.java | 4 +- .../robot/subsystems/shooter/ModuleIO.java | 4 +- .../shooter/ModuleIOTalonFXReal.java | 4 +- .../shooter/ModuleIOTalonFXSim.java | 4 +- .../subsystems/shooter/ModuleIOTalonFx.java | 4 +- .../frc/robot/subsystems/shooter/Shooter.java | 4 +- 12 files changed, 122 insertions(+), 145 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87921b0..e8fb183 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,13 +14,11 @@ package frc.robot; import static edu.wpi.first.units.Units.*; -import static edu.wpi.first.units.Units.Degrees; import static frc.robot.subsystems.vision.VisionConstants.*; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -30,11 +28,10 @@ import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.*; -import frc.robot.subsystems.vision.*; import frc.robot.subsystems.intake.*; +import frc.robot.subsystems.vision.*; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -157,18 +154,18 @@ private void configureButtonBindings() { // simulation : () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); - controller.rightBumper() - .whileTrue(Commands.startEnd( - () -> { - intake.deployArm(); - intake.setRollerVoltage(8.0); - }, - () -> { - intake.stowArm(); - intake.stop(); - }, - intake - )); + controller + .rightBumper() + .whileTrue(Commands.startEnd( + () -> { + intake.deployArm(); + intake.setRollerVoltage(8.0); + }, + () -> { + intake.stowArm(); + intake.stop(); + }, + intake)); } /** diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 9edd7c1..b1ceb94 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -125,9 +125,9 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; + private static final int kFrontLeftDriveMotorId = 8; + private static final int kFrontLeftSteerMotorId = 4; + private static final int kFrontLeftEncoderId = 12; private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -136,9 +136,9 @@ public class TunerConstants { private static final Distance kFrontLeftYPos = Inches.of(10); // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; + private static final int kFrontRightDriveMotorId = 5; + private static final int kFrontRightSteerMotorId = 2; + private static final int kFrontRightEncoderId = 11; private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -148,8 +148,8 @@ public class TunerConstants { // Back Left private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; + private static final int kBackLeftSteerMotorId = 1; + private static final int kBackLeftEncoderId = 10; private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -158,9 +158,9 @@ public class TunerConstants { private static final Distance kBackLeftYPos = Inches.of(10); // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; + private static final int kBackRightDriveMotorId = 6; + private static final int kBackRightSteerMotorId = 3; + private static final int kBackRightEncoderId = 13; private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 0967bca..aadd5ad 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -7,83 +7,83 @@ public class Intake extends SubsystemBase { // Arm position constants (radians) public static final double ARM_STOWED_POSITION = 0.0; public static final double ARM_DEPLOYED_POSITION = Math.PI / 2; // 90 degrees - + private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - + public Intake(IntakeIO io) { this.io = io; } - + @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); } - + /** Set roller voltage (-12 to +12 volts) */ public void setRollerVoltage(double voltage) { io.setRollerVoltage(voltage); } - + /** Set arm voltage (-12 to +12 volts) for manual control */ public void setArmVoltage(double voltage) { io.setArmVoltage(voltage); } - + /** Set arm to a specific position in radians */ public void setArmPosition(double positionRad) { io.setArmPosition(positionRad); } - + /** Deploy the intake arm to deployed position */ public void deployArm() { io.setArmPosition(ARM_DEPLOYED_POSITION); } - + /** Stow the intake arm to stowed position */ public void stowArm() { io.setArmPosition(ARM_STOWED_POSITION); } - + /** Reset arm encoder position to zero */ public void resetArmPosition() { io.resetArmPosition(); } - + /** Check if a game piece is detected */ public boolean hasGamePiece() { return inputs.hasGamePiece; } - + /** Get roller position in radians */ public double getRollerPositionRad() { return inputs.rollerPosition; } - + /** Get roller velocity in rad/s */ public double getRollerVelocityRadPerSec() { return inputs.rollerVelocity; } - + /** Get arm position in radians */ public double getArmPositionRad() { return inputs.armPosition; } - + /** Get arm velocity in rad/s */ public double getArmVelocityRadPerSec() { return inputs.armVelocity; } - + /** Check if arm is at target position (within tolerance) */ public boolean isArmAtPosition(double targetRad, double toleranceRad) { return Math.abs(inputs.armPosition - targetRad) < toleranceRad; } - + /** Stop all motors */ public void stop() { io.stopRoller(); io.setArmVoltage(0.0); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 3616812..442b27d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -20,16 +20,16 @@ class IntakeIOInputs { // Sensing public boolean hasGamePiece = false; } - + default void updateInputs(IntakeIOInputs inputs) {} - + default void setRollerVoltage(double voltage) {} - + default void setArmVoltage(double voltage) {} - + default void setArmPosition(double positionRad) {} - + default void resetArmPosition() {} - + default void stopRoller() {} -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index bf8adf8..8b8c577 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.intake; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -12,7 +14,6 @@ import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.generated.TunerConstants; -import static frc.robot.util.PhoenixUtil.tryUntilOk; public class IntakeIOTalonFXReal implements IntakeIO { // Hardware configuration constants @@ -20,146 +21,139 @@ public class IntakeIOTalonFXReal implements IntakeIO { private static final int ARM_MOTOR_ID = 21; private static final double ROLLER_GEAR_RATIO = 3.0; private static final double ARM_GEAR_RATIO = 100.0; - + // Beam break sensor private static final int BEAM_BREAK_DIO = 0; - + // Hardware private final TalonFX rollerMotor; private final TalonFX armMotor; private final DigitalInput beamBreak; - + // Control requests private final VoltageOut voltageRequest = new VoltageOut(0.0); private final PositionVoltage positionRequest = new PositionVoltage(0.0); - + // Status signals - Roller private final StatusSignal rollerPosition; private final StatusSignal rollerVelocity; private final StatusSignal rollerAppliedVolts; private final StatusSignal rollerCurrent; - + // Status signals - Arm private final StatusSignal armPosition; private final StatusSignal armVelocity; private final StatusSignal armAppliedVolts; private final StatusSignal armCurrent; - + public IntakeIOTalonFXReal() { // Initialize hardware rollerMotor = new TalonFX(ROLLER_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); armMotor = new TalonFX(ARM_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); beamBreak = new DigitalInput(BEAM_BREAK_DIO); - + // Configure roller motor var rollerConfig = new TalonFXConfiguration(); rollerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; rollerConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - + // Current limits for roller rollerConfig.CurrentLimits.StatorCurrentLimit = 40.0; rollerConfig.CurrentLimits.StatorCurrentLimitEnable = true; rollerConfig.CurrentLimits.SupplyCurrentLimit = 30.0; rollerConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - + tryUntilOk(5, () -> rollerMotor.getConfigurator().apply(rollerConfig, 0.25)); - + // Configure arm motor var armConfig = new TalonFXConfiguration(); armConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; armConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - + // Current limits for arm armConfig.CurrentLimits.StatorCurrentLimit = 30.0; armConfig.CurrentLimits.StatorCurrentLimitEnable = true; - + // PID for arm position control armConfig.Slot0.kP = 10.0; armConfig.Slot0.kI = 0.0; armConfig.Slot0.kD = 0.5; armConfig.Slot0.kS = 0.0; armConfig.Slot0.kV = 0.0; - + tryUntilOk(5, () -> armMotor.getConfigurator().apply(armConfig, 0.25)); - + // Create status signals - Roller rollerPosition = rollerMotor.getPosition(); rollerVelocity = rollerMotor.getVelocity(); rollerAppliedVolts = rollerMotor.getMotorVoltage(); rollerCurrent = rollerMotor.getStatorCurrent(); - + // Create status signals - Arm armPosition = armMotor.getPosition(); armVelocity = armMotor.getVelocity(); armAppliedVolts = armMotor.getMotorVoltage(); armCurrent = armMotor.getStatorCurrent(); - + // Configure update frequencies BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - rollerPosition, rollerVelocity, rollerAppliedVolts, rollerCurrent, - armPosition, armVelocity, armAppliedVolts, armCurrent - ); + 50.0, + rollerPosition, + rollerVelocity, + rollerAppliedVolts, + rollerCurrent, + armPosition, + armVelocity, + armAppliedVolts, + armCurrent); rollerMotor.optimizeBusUtilization(); armMotor.optimizeBusUtilization(); } - + @Override public void updateInputs(IntakeIOInputs inputs) { - BaseStatusSignal.refreshAll( - rollerPosition, rollerVelocity, rollerAppliedVolts, rollerCurrent - ); - BaseStatusSignal.refreshAll( - armPosition, armVelocity, armAppliedVolts, armCurrent - ); - + BaseStatusSignal.refreshAll(rollerPosition, rollerVelocity, rollerAppliedVolts, rollerCurrent); + BaseStatusSignal.refreshAll(armPosition, armVelocity, armAppliedVolts, armCurrent); + // Roller inputs - inputs.rollerPosition = Units.rotationsToRadians( - rollerPosition.getValueAsDouble() - ) / ROLLER_GEAR_RATIO; - inputs.rollerVelocity = Units.rotationsToRadians( - rollerVelocity.getValueAsDouble() - ) / ROLLER_GEAR_RATIO; + inputs.rollerPosition = Units.rotationsToRadians(rollerPosition.getValueAsDouble()) / ROLLER_GEAR_RATIO; + inputs.rollerVelocity = Units.rotationsToRadians(rollerVelocity.getValueAsDouble()) / ROLLER_GEAR_RATIO; inputs.rollerVoltage = rollerAppliedVolts.getValueAsDouble(); inputs.rollerCurrent = rollerCurrent.getValueAsDouble(); - + // Arm inputs - inputs.armPosition = Units.rotationsToRadians( - armPosition.getValueAsDouble() - ) / ARM_GEAR_RATIO; - inputs.armVelocity = Units.rotationsToRadians( - armVelocity.getValueAsDouble() - ) / ARM_GEAR_RATIO; + inputs.armPosition = Units.rotationsToRadians(armPosition.getValueAsDouble()) / ARM_GEAR_RATIO; + inputs.armVelocity = Units.rotationsToRadians(armVelocity.getValueAsDouble()) / ARM_GEAR_RATIO; inputs.armVoltage = armAppliedVolts.getValueAsDouble(); inputs.armCurrent = armCurrent.getValueAsDouble(); - + // Sensor inputs - beam break is inverted (false = blocked) inputs.hasGamePiece = !beamBreak.get(); } - + @Override public void setRollerVoltage(double voltage) { rollerMotor.setControl(voltageRequest.withOutput(voltage)); } - + @Override public void setArmVoltage(double voltage) { armMotor.setControl(voltageRequest.withOutput(voltage)); } - + @Override public void setArmPosition(double positionRad) { double motorRotations = Units.radiansToRotations(positionRad) * ARM_GEAR_RATIO; armMotor.setControl(positionRequest.withPosition(motorRotations)); } - + @Override public void resetArmPosition() { tryUntilOk(5, () -> armMotor.setPosition(0.0, 0.25)); } - + @Override public void stopRoller() { rollerMotor.stopMotor(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index dbea029..1af412c 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -8,66 +8,64 @@ public class IntakeIOTalonFXSim implements IntakeIO { // Hardware configuration constants private static final double ROLLER_GEAR_RATIO = 3.0; private static final double ARM_GEAR_RATIO = 100.0; - + // Simulation parameters private static final double ROLLER_MOI = 0.001; // kg*m^2 private static final double ARM_MOI = 0.1; // kg*m^2 - + // Arm position constants (radians) private static final double ARM_MIN_ANGLE = 0.0; private static final double ARM_MAX_ANGLE = Math.PI / 2; - + // Motor simulations private final DCMotorSim rollerSim; private final DCMotorSim armSim; - + // Applied voltages private double rollerAppliedVolts = 0.0; private double armAppliedVolts = 0.0; - + // Simulated game piece detection private boolean simulatedGamePiece = false; - + public IntakeIOTalonFXSim() { // Kraken X60 for roller (1 motor, geared) // Using LinearSystemId to create the plant rollerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ROLLER_MOI, ROLLER_GEAR_RATIO), - DCMotor.getKrakenX60(1) - ); - + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ROLLER_MOI, ROLLER_GEAR_RATIO), + DCMotor.getKrakenX60(1)); + // Kraken X60 for arm (1 motor, highly geared) armSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ARM_MOI, ARM_GEAR_RATIO), - DCMotor.getKrakenX60(1) - ); + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ARM_MOI, ARM_GEAR_RATIO), + DCMotor.getKrakenX60(1)); } - + @Override public void updateInputs(IntakeIOInputs inputs) { // Update simulations rollerSim.update(0.02); armSim.update(0.02); - + // Clamp arm position to realistic bounds if (armSim.getAngularPositionRad() < ARM_MIN_ANGLE) { armSim.setState(ARM_MIN_ANGLE, 0.0); } else if (armSim.getAngularPositionRad() > ARM_MAX_ANGLE) { armSim.setState(ARM_MAX_ANGLE, 0.0); } - + // Update roller inputs inputs.rollerPosition = rollerSim.getAngularPositionRad(); inputs.rollerVelocity = rollerSim.getAngularVelocityRadPerSec(); inputs.rollerVoltage = rollerAppliedVolts; inputs.rollerCurrent = rollerSim.getCurrentDrawAmps(); - + // Update arm inputs inputs.armPosition = armSim.getAngularPositionRad(); inputs.armVelocity = armSim.getAngularVelocityRadPerSec(); inputs.armVoltage = armAppliedVolts; inputs.armCurrent = armSim.getCurrentDrawAmps(); - + // Simulate game piece detection based on roller speed // If roller is spinning fast enough, assume we have a game piece if (Math.abs(inputs.rollerVelocity) > 5.0) { // rad/s threshold @@ -75,44 +73,44 @@ public void updateInputs(IntakeIOInputs inputs) { } inputs.hasGamePiece = simulatedGamePiece; } - + @Override public void setRollerVoltage(double voltage) { rollerAppliedVolts = voltage; rollerSim.setInputVoltage(voltage); } - + @Override public void setArmVoltage(double voltage) { armAppliedVolts = voltage; armSim.setInputVoltage(voltage); } - + @Override public void setArmPosition(double positionRad) { // Simple P controller for position control in sim double kP = 10.0; double error = positionRad - armSim.getAngularPositionRad(); double voltage = error * kP; - + // Clamp voltage voltage = Math.max(-12.0, Math.min(12.0, voltage)); - + setArmVoltage(voltage); } - + @Override public void resetArmPosition() { armSim.setState(ARM_MIN_ANGLE, 0.0); } - + @Override public void stopRoller() { setRollerVoltage(0.0); } - + /** For testing - manually set game piece detection */ public void setSimulatedGamePiece(boolean hasGamePiece) { this.simulatedGamePiece = hasGamePiece; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Module.java b/src/main/java/frc/robot/subsystems/shooter/Module.java index 50ec864..b89d918 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Module.java +++ b/src/main/java/frc/robot/subsystems/shooter/Module.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.shooter; -public class Module { - -} +public class Module {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java index 31babd4..26e69aa 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIO.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.shooter; -public class ModuleIO { - -} +public class ModuleIO {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java index 4a7641e..ccfce31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXReal.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.shooter; -public class ModuleIOTalonFXReal { - -} +public class ModuleIOTalonFXReal {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java index 679b417..5d0113a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.shooter; -public class ModuleIOTalonFXSim { - -} +public class ModuleIOTalonFXSim {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java index 642c50f..9262655 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java +++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.shooter; -public class ModuleIOTalonFx { - -} +public class ModuleIOTalonFx {} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8ee5de8..f718411 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,6 +2,4 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Shooter extends SubsystemBase{ - -} +public class Shooter extends SubsystemBase {} From 19e41c1e4e9e267ab7c503863113ceb7b53eec44 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Thu, 20 Nov 2025 19:55:40 -0800 Subject: [PATCH 11/51] Implement Mechanism2D for Intake simulations --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++ .../frc/robot/subsystems/intake/Intake.java | 57 +++++++++++++++++++ .../intake/IntakeIOTalonFXReal.java | 10 +--- 3 files changed, 75 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e8fb183..f900e03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -45,6 +50,8 @@ public class RobotContainer { private final Drive drive; private final Vision vision; private final Intake intake; + private final Mechanism2d robotMechanism = new Mechanism2d(4, 4); //Intake visualization + private SwerveDriveSimulation driveSimulation = null; @@ -95,6 +102,7 @@ public RobotContainer() { new VisionIOPhotonVisionSim( camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); intake = new Intake(new IntakeIOTalonFXSim()); + setupRobotMechanism(); break; default: @@ -128,6 +136,14 @@ public RobotContainer() { configureButtonBindings(); } + private void setupRobotMechanism() { + // Robot base + MechanismRoot2d robotBase = robotMechanism.getRoot("RobotBase", 2, 0.2); + + // Drivetrain + robotBase.append(new MechanismLigament2d("Chassis", 0.8, 0, 10, new Color8Bit(Color.kBlue))); + } + /** * Use this method to define your button->command mappings. Buttons can be created by instantiating a * {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 45b6c46..2c802bb 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,5 +1,18 @@ package frc.robot.subsystems.intake; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -11,14 +24,58 @@ public class Intake extends SubsystemBase { private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + // Mechanism2d for visualization + private final Mechanism2d mechanism; + private final MechanismLigament2d armLigament; + private final MechanismLigament2d rollerLigament; + + public Intake(IntakeIO io) { this.io = io; + + // Create Mechanism2d (width, height in meters) + mechanism = new Mechanism2d(3, 3); + + // Create root at robot center (x, y in meters) + MechanismRoot2d root = mechanism.getRoot("IntakeRoot", 1.5, 0.5); + + // Create arm ligament (name, length in meters, angle in degrees, line width, color) + armLigament = root.append(new MechanismLigament2d( + "Arm", + 0.5, // 0.5 meter arm length + Math.toDegrees(ARM_STOWED_POSITION), + 6, + new Color8Bit(Color.kOrange) + )); + + // Create roller at end of arm + rollerLigament = armLigament.append(new MechanismLigament2d( + "Roller", + 0.15, // 0.15 meter roller visualization + 90, // Perpendicular to arm + 4, + new Color8Bit(Color.kGreen) + )); + + SmartDashboard.putData("Intake/Mechanism", mechanism); + } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); + + // Update mechanism visualization + armLigament.setAngle(Math.toDegrees(inputs.armPosition)); + + // Change roller color based on game piece detection + if (inputs.hasGamePiece) { + rollerLigament.setColor(new Color8Bit(Color.kYellow)); + } + else { + rollerLigament.setColor(new Color8Bit(Color.kGreen)); + } } //Set roller voltage (-12 to +12 volts) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 8b8c577..641a874 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -12,7 +12,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.*; -import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.generated.TunerConstants; public class IntakeIOTalonFXReal implements IntakeIO { @@ -22,13 +21,9 @@ public class IntakeIOTalonFXReal implements IntakeIO { private static final double ROLLER_GEAR_RATIO = 3.0; private static final double ARM_GEAR_RATIO = 100.0; - // Beam break sensor - private static final int BEAM_BREAK_DIO = 0; - // Hardware private final TalonFX rollerMotor; private final TalonFX armMotor; - private final DigitalInput beamBreak; // Control requests private final VoltageOut voltageRequest = new VoltageOut(0.0); @@ -50,7 +45,6 @@ public IntakeIOTalonFXReal() { // Initialize hardware rollerMotor = new TalonFX(ROLLER_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); armMotor = new TalonFX(ARM_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); - beamBreak = new DigitalInput(BEAM_BREAK_DIO); // Configure roller motor var rollerConfig = new TalonFXConfiguration(); @@ -127,8 +121,8 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armVoltage = armAppliedVolts.getValueAsDouble(); inputs.armCurrent = armCurrent.getValueAsDouble(); - // Sensor inputs - beam break is inverted (false = blocked) - inputs.hasGamePiece = !beamBreak.get(); + // Sensor inputs + inputs.hasGamePiece = rollerMotor.getStatorCurrent().getValueAsDouble() > 50.0;// Current limits, may need to change later } @Override From dad8f2c3ad8518e0afe5805458efba81439be826 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Thu, 20 Nov 2025 20:04:42 -0800 Subject: [PATCH 12/51] Completed sim implementation with implementation in robotcontainer --- Current AdvantageScope.json | 550 ++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 17 +- .../frc/robot/subsystems/intake/Intake.java | 78 ++- .../intake/IntakeIOTalonFXReal.java | 3 +- .../subsystems/intake/IntakeIOTalonFXSim.java | 4 +- .../robot/subsystems/shooter/ShooterIO.java | 1 + .../shooter/ShooterIOTalonFXReal.java | 4 +- .../shooter/ShooterIOTalonFXSim.java | 4 +- 8 files changed, 598 insertions(+), 63 deletions(-) create mode 100644 Current AdvantageScope.json diff --git a/Current AdvantageScope.json b/Current AdvantageScope.json new file mode 100644 index 0000000..b1de72e --- /dev/null +++ b/Current AdvantageScope.json @@ -0,0 +1,550 @@ +{ + "hubs": [ + { + "x": -8, + "y": -8, + "width": 1936, + "height": 1048, + "state": { + "sidebar": { + "width": 300, + "expanded": [ + "/Drive", + "/RealOutputs", + "/RealOutputs/SwerveStates", + "/RealOutputs/SwerveChassisSpeeds", + "/RealOutputs/Odometry", + "/AdvantageKit/RealOutputs", + "/AdvantageKit/RealOutputs/Odometry/Robot", + "/AdvantageKit/RealOutputs/Vision", + "/AdvantageKit/RealOutputs/Vision/Camera1", + "/SmartDashboard", + "/SmartDashboard/Intake" + ] + }, + "tabs": { + "selected": 2, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "grimvlgu9aluj8btvwotco2wpj0pto2h", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 9, + "title": "Drive Overview", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ffff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff0000" + } + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#00ffff" + } + }, + { + "type": "rotation", + "logKey": "/RealOutputs/Odometry/Robot/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + }, + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ff00", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ffff00" + } + }, + { + "type": "chassisSpeeds", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Setpoints", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff8c00" + } + }, + { + "type": "rotation", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "prdx7t2eedan6n46dxrjfu7eisf16f9o", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 3, + "title": "Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot" + } + }, + { + "type": "trajectory", + "logKey": "/RealOutputs/Odometry/Trajectory", + "logType": "Pose2d[]", + "visible": true, + "options": { + "color": "#ff8c00", + "size": "normal" + } + }, + { + "type": "ghost", + "logKey": "/RealOutputs/Odometry/TrajectorySetpoint", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot", + "color": "#ff8c00" + } + }, + { + "type": "robot", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "2024 KitBot" + } + }, + { + "type": "vision", + "logKey": "NT:/AdvantageKit/RealOutputs/Vision/Camera0/TagPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "vision", + "logKey": "NT:/AdvantageKit/RealOutputs/Vision/Camera1/TagPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "mechanism", + "logKey": "NT:/SmartDashboard/Intake/Mechanism", + "logType": "Mechanism2d", + "visible": true, + "options": {} + } + ], + "game": "2025 Field (Welded)", + "origin": "blue" + }, + "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 4.889792955692206, + 1.4477331817350256, + -1.5030132683871433 + ], + "cameraTarget": [ + 5.9462424992067175, + 0.5503848177969854, + 0.6337024678673298 + ] + }, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turn Calibration", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module1/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module2/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module3/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "ol8lk80m83ma3aegae849d6k1d29sp7d", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Drive Calibration", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module1/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module2/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module3/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "7jj1f83nsv59dd2ou11ls3ccz5tq1293", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turn PID Tuning", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/Measured/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "kj6tf6sxm5g04wnjrhed6mx2wpccmqtn", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Drive PID Tuning", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/Measured/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "bctxg9pmwpo31m9bv9ofr92oqqtbpqzp", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Max Speed Measurement", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "oy3vmjuhr7g36evikwnmjhfacl30mzn5", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Slip Current Measurement", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DriveCurrentAmps", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DriveVelocityRadPerSec", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "efudylbvebbv1ga2kosknov0uegmtj7h", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.1.5" +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f900e03..2ffeb88 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,8 +50,7 @@ public class RobotContainer { private final Drive drive; private final Vision vision; private final Intake intake; - private final Mechanism2d robotMechanism = new Mechanism2d(4, 4); //Intake visualization - + private final Mechanism2d robotMechanism = new Mechanism2d(4, 4); // Intake visualization private SwerveDriveSimulation driveSimulation = null; @@ -136,13 +135,13 @@ public RobotContainer() { configureButtonBindings(); } - private void setupRobotMechanism() { - // Robot base - MechanismRoot2d robotBase = robotMechanism.getRoot("RobotBase", 2, 0.2); - - // Drivetrain - robotBase.append(new MechanismLigament2d("Chassis", 0.8, 0, 10, new Color8Bit(Color.kBlue))); - } + private void setupRobotMechanism() { + // Robot base + MechanismRoot2d robotBase = robotMechanism.getRoot("RobotBase", 2, 0.2); + + // Drivetrain + robotBase.append(new MechanismLigament2d("Chassis", 0.8, 0, 10, new Color8Bit(Color.kBlue))); + } /** * Use this method to define your button->command mappings. Buttons can be created by instantiating a diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 2c802bb..6ed71d2 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -6,13 +6,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -29,36 +22,32 @@ public class Intake extends SubsystemBase { private final MechanismLigament2d armLigament; private final MechanismLigament2d rollerLigament; - public Intake(IntakeIO io) { this.io = io; // Create Mechanism2d (width, height in meters) mechanism = new Mechanism2d(3, 3); - + // Create root at robot center (x, y in meters) MechanismRoot2d root = mechanism.getRoot("IntakeRoot", 1.5, 0.5); - + // Create arm ligament (name, length in meters, angle in degrees, line width, color) armLigament = root.append(new MechanismLigament2d( - "Arm", - 0.5, // 0.5 meter arm length - Math.toDegrees(ARM_STOWED_POSITION), - 6, - new Color8Bit(Color.kOrange) - )); - + "Arm", + 0.5, // 0.5 meter arm length + Math.toDegrees(ARM_STOWED_POSITION), + 6, + new Color8Bit(Color.kOrange))); + // Create roller at end of arm rollerLigament = armLigament.append(new MechanismLigament2d( - "Roller", - 0.15, // 0.15 meter roller visualization - 90, // Perpendicular to arm - 4, - new Color8Bit(Color.kGreen) - )); + "Roller", + 0.15, // 0.15 meter roller visualization + 90, // Perpendicular to arm + 4, + new Color8Bit(Color.kGreen))); SmartDashboard.putData("Intake/Mechanism", mechanism); - } @Override @@ -68,76 +57,75 @@ public void periodic() { // Update mechanism visualization armLigament.setAngle(Math.toDegrees(inputs.armPosition)); - + // Change roller color based on game piece detection if (inputs.hasGamePiece) { rollerLigament.setColor(new Color8Bit(Color.kYellow)); - } - else { + } else { rollerLigament.setColor(new Color8Bit(Color.kGreen)); } } - - //Set roller voltage (-12 to +12 volts) + + // Set roller voltage (-12 to +12 volts) public void setRollerVoltage(double voltage) { io.setRollerVoltage(voltage); } - - //Set arm voltage (-12 to +12 volts) for manual control + + // Set arm voltage (-12 to +12 volts) for manual control public void setArmVoltage(double voltage) { io.setArmVoltage(voltage); } - - //Set arm to a specific position in radians + + // Set arm to a specific position in radians public void setArmPosition(double positionRad) { io.setArmPosition(positionRad); } - + // Deploy the intake arm to deployed position public void deployArm() { io.setArmPosition(ARM_DEPLOYED_POSITION); } - + // Stow the intake arm to stowed position public void stowArm() { io.setArmPosition(ARM_STOWED_POSITION); } - - // Reset arm encoder position to zero + + // Reset arm encoder position to zero public void resetArmPosition() { io.resetArmPosition(); } - + // Check if a game piece is detected public boolean hasGamePiece() { return inputs.hasGamePiece; } - + // Get roller position in radians public double getRollerPositionRad() { return inputs.rollerPosition; } - - //Get roller velocity in rad/s + + // Get roller velocity in rad/s public double getRollerVelocityRadPerSec() { return inputs.rollerVelocity; } - + // Get arm position in radians public double getArmPositionRad() { return inputs.armPosition; } - + // Get arm velocity in rad/s public double getArmVelocityRadPerSec() { return inputs.armVelocity; } - + // Check if arm is at target position (within tolerance) public boolean isArmAtPosition(double targetRad, double toleranceRad) { return Math.abs(inputs.armPosition - targetRad) < toleranceRad; } - + // Stop all motors public void stop() { io.stopRoller(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 641a874..a3664fb 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -122,7 +122,8 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armCurrent = armCurrent.getValueAsDouble(); // Sensor inputs - inputs.hasGamePiece = rollerMotor.getStatorCurrent().getValueAsDouble() > 50.0;// Current limits, may need to change later + inputs.hasGamePiece = + rollerMotor.getStatorCurrent().getValueAsDouble() > 50.0; // Current limits, may need to change later } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index 958bae7..2b4166d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -108,8 +108,8 @@ public void resetArmPosition() { public void stopRoller() { setRollerVoltage(0.0); } - - // For testing - manually set game piece detection + + // For testing - manually set game piece detection public void setSimulatedGamePiece(boolean hasGamePiece) { this.simulatedGamePiece = hasGamePiece; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index e69de29..8b13789 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1 @@ + diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java index 47bee02..4596445 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.shooter; -public class ShooterIOTalonFXReal { - -} +public class ShooterIOTalonFXReal {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java index 645a9c3..ebc978b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.shooter; -public class ShooterIOTalonFXSim { - -} +public class ShooterIOTalonFXSim {} From 5ee0e1e945b63cbe0482097ca52ed50c9093c989 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Mon, 24 Nov 2025 14:35:36 -0800 Subject: [PATCH 13/51] Updated simulation piece detection --- .../frc/robot/subsystems/intake/IntakeIOTalonFXSim.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index 2b4166d..f5c6b74 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -66,11 +66,14 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armVoltage = armAppliedVolts; inputs.armCurrent = armSim.getCurrentDrawAmps(); - // Simulate game piece detection based on roller speed - // If roller is spinning fast enough, assume we have a game piece - if (Math.abs(inputs.rollerVelocity) > 5.0) { // rad/s threshold + if (!simulatedGamePiece && rollerAppliedVolts > 6.0 && Math.abs(rollerSim.getAngularVelocityRadPerSec()) > 10.0) { simulatedGamePiece = true; } + + if (simulatedGamePiece && rollerAppliedVolts < -6.0) { + simulatedGamePiece = false; + } + inputs.hasGamePiece = simulatedGamePiece; } From 7a91d58e897806072da900c9762ba1f712a90775 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Mon, 24 Nov 2025 14:36:02 -0800 Subject: [PATCH 14/51] Updated current limits for intake rollers --- .../frc/robot/subsystems/intake/IntakeIOTalonFXReal.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index a3664fb..1fc54e2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -52,9 +52,9 @@ public IntakeIOTalonFXReal() { rollerConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; // Current limits for roller - rollerConfig.CurrentLimits.StatorCurrentLimit = 40.0; + rollerConfig.CurrentLimits.StatorCurrentLimit = 60.0; rollerConfig.CurrentLimits.StatorCurrentLimitEnable = true; - rollerConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + rollerConfig.CurrentLimits.SupplyCurrentLimit = 50.0; rollerConfig.CurrentLimits.SupplyCurrentLimitEnable = true; tryUntilOk(5, () -> rollerMotor.getConfigurator().apply(rollerConfig, 0.25)); @@ -122,8 +122,7 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armCurrent = armCurrent.getValueAsDouble(); // Sensor inputs - inputs.hasGamePiece = - rollerMotor.getStatorCurrent().getValueAsDouble() > 50.0; // Current limits, may need to change later + inputs.hasGamePiece = rollerMotor.getStatorCurrent().getValueAsDouble() > 40.0; //May need to change current limit and detection later } @Override From c1c6a98f8ced4a0d2aa7b6ffd3b78c90b534d6b7 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Mon, 24 Nov 2025 17:17:02 -0800 Subject: [PATCH 15/51] Added Superstructure --- src/main/java/frc/robot/RobotContainer.java | 56 +++- .../commands/SuperstructureCommands.java | 84 +++++ .../superstructure/Superstructure.java | 291 ++++++++++++++++++ .../superstructure/SuperstructureIO.java | 27 ++ .../superstructure/SuperstructureIOReal.java | 8 + .../superstructure/Superstructurestate.java | 25 ++ 6 files changed, 477 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/commands/SuperstructureCommands.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/Superstructure.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2ffeb88..cbd69e6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,9 +31,13 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; +import frc.robot.commands.SuperstructureCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.intake.*; +import frc.robot.subsystems.superstructure.Superstructure; +import frc.robot.subsystems.superstructure.SuperstructureIO; +import frc.robot.subsystems.superstructure.SuperstructureIOReal; import frc.robot.subsystems.vision.*; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; @@ -50,12 +54,14 @@ public class RobotContainer { private final Drive drive; private final Vision vision; private final Intake intake; + private final Superstructure superstructure; private final Mechanism2d robotMechanism = new Mechanism2d(4, 4); // Intake visualization private SwerveDriveSimulation driveSimulation = null; // Controller private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController operator = new CommandXboxController(1); // Dashboard inputs private final LoggedDashboardChooser autoChooser; @@ -77,6 +83,7 @@ public RobotContainer() { new VisionIOLimelight(VisionConstants.camera0Name, drive::getRotation), new VisionIOLimelight(VisionConstants.camera1Name, drive::getRotation)); intake = new Intake(new IntakeIOTalonFXReal()); + superstructure = new Superstructure(intake, new SuperstructureIOReal()); break; case SIM: // Sim robot, instantiate physics sim IO implementations @@ -102,6 +109,7 @@ public RobotContainer() { camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); intake = new Intake(new IntakeIOTalonFXSim()); setupRobotMechanism(); + superstructure = new Superstructure(intake, new SuperstructureIOReal()); break; default: @@ -115,6 +123,7 @@ public RobotContainer() { (pose) -> {}); vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); intake = new Intake(new IntakeIO() {}); + superstructure = new Superstructure(intake, new SuperstructureIOReal()); break; } @@ -149,18 +158,19 @@ private void setupRobotMechanism() { * and then passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + //============DRIVER CONTROLLER BINDINGS (DRIVE)============// // Default command, normal field-relative drive drive.setDefaultCommand(DriveCommands.joystickDrive( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); // Lock to 0° when A button is held controller - .a() + .povDown() .whileTrue(DriveCommands.joystickDriveAtAngle( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> new Rotation2d())); // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + controller.povUp().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro / odometry final Runnable resetGyro = Constants.currentMode == Constants.Mode.SIM @@ -169,18 +179,36 @@ private void configureButtonBindings() { // simulation : () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); - controller - .rightBumper() - .whileTrue(Commands.startEnd( - () -> { - intake.deployArm(); - intake.setRollerVoltage(8.0); - }, - () -> { - intake.stowArm(); - intake.stop(); - }, - intake)); + + //============DRIVER CONTROLLER BINDINGS (SYSTEM)============// + //Intake (no timeout) + controller.a().onTrue(SuperstructureCommands.intake(superstructure)); + + //Eject for 0.5 seconds + controller.b().onTrue(SuperstructureCommands.outtake(superstructure, 0.5)); + + //Stow (AKA Return to Idle) + controller.x().onTrue(SuperstructureCommands.stow(superstructure)); + + //Emergency stop + controller.y().onTrue(SuperstructureCommands.emergencyStop(superstructure)); + + //Prepare to shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) + controller.povLeft().onTrue(SuperstructureCommands.prepareToShoot(superstructure)); + + //Shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) + controller.povRight().onTrue(SuperstructureCommands.shoot(superstructure)); + + + //============OPERATOR CONTROLLER BINDINGS (SYSTEM)============// + //Manual intake + operator.a().whileTrue(SuperstructureCommands.manualIntake(superstructure)); + + //Manual eject + operator.b().whileTrue(SuperstructureCommands.manualOuttake(superstructure)); + + //Intake (with Timeout) + operator.x().onTrue(SuperstructureCommands.intakeWithTimeout(superstructure, 3.0)); } /** diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java new file mode 100644 index 0000000..ddb2962 --- /dev/null +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -0,0 +1,84 @@ +package frc.robot.commands; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.superstructure.Superstructurestate; +import frc.robot.subsystems.superstructure.Superstructure; + +public class SuperstructureCommands { + //Command to intake gamepiece with no time limits + public static Command intake(Superstructure superstructure) { + return Commands.sequence( + Commands.runOnce(() -> superstructure.requestIntake(), superstructure), + Commands.waitUntil(() -> superstructure.hasGamePiece()), + Commands.runOnce(() -> superstructure.requestIdle(), superstructure) + ).withName("Intake").alongWith(Commands.run(() -> {}, superstructure.getIntake())); + } + + //Command to intake gamepiece with timeout, stops trying to intake after timeout seconds no matter if it has gamepiece or not + public static Command intakeWithTimeout(Superstructure superstructure, double timeout) { + return Commands.sequence( + Commands.runOnce(() -> superstructure.requestIntake(), superstructure), + Commands.either( + Commands.waitUntil(() -> superstructure.hasGamePiece()), + Commands.waitSeconds(timeout), + () -> !superstructure.hasGamePiece() + ), + Commands.runOnce(() -> superstructure.requestIdle(), superstructure) + ).withName("IntakeWithTimeout").alongWith(Commands.run(() -> {}, superstructure.getIntake())); + } + + //Command to manually run intake + public static Command manualIntake(Superstructure superstructure) { + return Commands.startEnd(() -> superstructure.requestIntake(), () -> superstructure.requestIdle(), superstructure) + .withName("ManualIntake").alongWith(Commands.run(() -> {}, superstructure.getIntake())); + } + + //Command to outtake gamepiece + public static Command outtake(Superstructure superstructure, double duration){ + return Commands.sequence( + Commands.runOnce(() -> superstructure.requestOuttake(), superstructure), + Commands.waitSeconds(duration), + Commands.runOnce(() -> superstructure.requestIdle(), superstructure) + ).withName("Eject"); + } + + //Command to manually outtake + public static Command manualOuttake(Superstructure superstructure) { + return Commands.startEnd(() -> superstructure.requestOuttake(), () -> superstructure.requestIdle(), superstructure).withName("ManualEject"); + } + + //Command to stow intake + public static Command stow(Superstructure superstructure){ + return Commands.runOnce(()-> superstructure.requestIdle(), superstructure).withName("Stow"); + } + + //Command to wait until superstructure is ready + public static Command waitForReady(Superstructure superstructure){ + return Commands.waitUntil(() -> superstructure.isReady()).withName("WaitForReady"); + } + + //Command to prep for shooting + //REMMEBER TO FIX IMPLEMENTATION WHEN SHOOTER IS FULLY DONE + public static Command prepareToShoot(Superstructure superstructure){ + return Commands.sequence( + Commands.runOnce(() -> superstructure.requestShootPrepare(), superstructure), + Commands.waitUntil(() -> superstructure.getCurrentState() == Superstructurestate.PREPARING_TO_SHOOT && superstructure.isReady()) + ).withName("PrepareToShoot"); + } + + //Command to shoot + //REmember TO FIX IMPLEMENTATION WHEN SHOOTER IS FULLY DONE + public static Command shoot(Superstructure superstructure){ + return Commands.sequence( + Commands.runOnce(() -> superstructure.requestShoot(), superstructure), + Commands.waitSeconds(0.5), + Commands.runOnce(() -> superstructure.requestIdle(), superstructure) + ).withName("Shoot"); + } + + //Command to emergency stop the superstructure + public static Command emergencyStop(Superstructure superstructure){ + return Commands.runOnce(() -> superstructure.emergencyStop(), superstructure).withName("EmergencyStop"); + } +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java new file mode 100644 index 0000000..b7378b2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -0,0 +1,291 @@ +package frc.robot.subsystems.superstructure; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.intake.Intake; +import org.littletonrobotics.junction.Logger; + +public class Superstructure extends SubsystemBase { + // Time constants in seconds + private static final double StateTransitionTimeout = 2.0; + + // Position check tolerance in radians + private static final double ArmPositionTolerance = 0.1; + + private final Intake intake; + private final SuperstructureIO io; + private final SuperstructureIOInputsAutoLogged inputs = new SuperstructureIOInputsAutoLogged(); + + private Superstructurestate currentState = Superstructurestate.IDLE; + private Superstructurestate desiredState = Superstructurestate.IDLE; + private double stateStartTime = 0.0; + + public Superstructure(Intake intake, SuperstructureIO io) { + this.intake = intake; + this.io = io; + } + + @Override + public void periodic() { + // Update inputs from subsystems + io.updateInputs(inputs); + Logger.processInputs("Superstructure", inputs); + + updateStateInfo(); + runStateMachine(); + + // Log current states + Logger.recordOutput("Superstructure/CurrentState", currentState.toString()); + Logger.recordOutput("Superstructure/DesiredState", desiredState.toString()); + Logger.recordOutput("Superstructure/StateTime", Timer.getFPGATimestamp() - stateStartTime); + } + + public Intake getIntake() { + return this.intake; + } + + // Update current state info + private void updateStateInfo() { + inputs.currentState = currentState; + inputs.desiredState = desiredState; + inputs.stateStartTime = stateStartTime; + + // Intake status + inputs.intakeHasGamePiece = intake.hasGamePiece(); + inputs.intakeDeployed = intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, ArmPositionTolerance); + inputs.intakeArmPosition = intake.getArmPositionRad(); + + inputs.readyToIntake = intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, ArmPositionTolerance); + inputs.readyToShoot = false; // Placeholder for shooter + } + + // Run state machine logic + private void runStateMachine() { + if (currentState == Superstructurestate.TRANSITIONING && isStateTimedOut()) { + Logger.recordOutput("Superstructure/Warning", "State Timeout - Forcing Idle"); + transitionToIdle(); + } + + if (currentState == desiredState) { + runActiveStateLogic(); + return; + } + + switch (currentState) { + case IDLE: + handleIdleState(); + break; + case INTAKING: + handleIntakingState(); + break; + case STOWED: + handleStowedState(); + break; + case HOLDING_PIECE: + handleHoldingPieceState(); + break; + case OUTTAKING: + handleOuttakingState(); + break; + case DEPLOYED: + handleDeployedState(); + break; + case TRANSITIONING: + handleTransitioningState(); + break; + case PREPARING_TO_SHOOT: + case SHOOTING: + transitionToIdle(); + break; + } + } + + private void runActiveStateLogic() { + switch (currentState) { + case INTAKING: + intake.setRollerVoltage(8.0); + break; + case OUTTAKING: + intake.setRollerVoltage(-8.0); + break; + case HOLDING_PIECE: + intake.stop(); + break; + default: + break; + } + } + + private void handleIdleState() { + intake.stowArm(); + intake.stop(); + + switch (desiredState) { + case INTAKING: + intake.deployArm(); + changeState(Superstructurestate.TRANSITIONING); + break; + case OUTTAKING: + intake.deployArm(); + changeState(Superstructurestate.TRANSITIONING); + break; + default: + // Stay in idle + break; + } + } + + private void handleIntakingState() { + intake.setRollerVoltage(8.0); + + if (intake.hasGamePiece()) { + intake.stop(); + intake.stowArm(); + changeState(Superstructurestate.TRANSITIONING); + desiredState = Superstructurestate.HOLDING_PIECE; + } + + if (desiredState != Superstructurestate.INTAKING && desiredState != Superstructurestate.HOLDING_PIECE) { + intake.stop(); + intake.stowArm(); + changeState(Superstructurestate.TRANSITIONING); + } + } + + private void handleTransitioningState() { + switch (desiredState) { + case INTAKING: + if (inputs.readyToIntake) { + intake.setRollerVoltage(8.0); + changeState(Superstructurestate.INTAKING); + } + break; + + case OUTTAKING: + if (inputs.readyToIntake) { + intake.setRollerVoltage(-8.0); + changeState(Superstructurestate.OUTTAKING); + } + break; + + case IDLE: + // Wait for arm to stow + if (intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, ArmPositionTolerance)) { + intake.stop(); + changeState(Superstructurestate.IDLE); + } + break; + + case HOLDING_PIECE: + // Wait for arm to stow + if (intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, ArmPositionTolerance)) { + intake.stop(); + changeState(Superstructurestate.HOLDING_PIECE); + } + break; + + default: + // Go to idle if unsure + transitionToIdle(); + break; + } + } + + private void handleHoldingPieceState() { + intake.stowArm(); + intake.stop(); + + // Check for any state changes + if (desiredState == Superstructurestate.OUTTAKING) { + intake.deployArm(); + changeState(Superstructurestate.TRANSITIONING); + } + else if (desiredState == Superstructurestate.INTAKING) { + intake.deployArm(); + changeState(Superstructurestate.TRANSITIONING); + } + else if (desiredState == Superstructurestate.IDLE) { + changeState(Superstructurestate.IDLE); + } + } + + private void handleOuttakingState() { + intake.setRollerVoltage(-8.0); + + if (desiredState != Superstructurestate.OUTTAKING) { + intake.stop(); + intake.stowArm(); + changeState(Superstructurestate.TRANSITIONING); + } + } + + private void handleStowedState() { + transitionToIdle(); + } + + private void handleDeployedState() { + transitionToIdle(); + } + + private void changeState(Superstructurestate newState) { + currentState = newState; + stateStartTime = Timer.getFPGATimestamp(); + Logger.recordOutput("Superstructure/StateChange", "New State: " + newState.toString()); + } + + private void transitionToIdle() { + desiredState = Superstructurestate.IDLE; + intake.stowArm(); + intake.stop(); + changeState(Superstructurestate.TRANSITIONING); + } + + private boolean isStateTimedOut() { + double stateTime = Timer.getFPGATimestamp() - stateStartTime; + return stateTime > StateTransitionTimeout; + } + + public void requestIntake() { + desiredState = Superstructurestate.INTAKING; + } + + public void requestOuttake() { + desiredState = Superstructurestate.OUTTAKING; + } + + public void requestIdle() { + desiredState = Superstructurestate.IDLE; + } + + public void requestShoot() { + if (currentState == Superstructurestate.PREPARING_TO_SHOOT) + desiredState = Superstructurestate.SHOOTING; + } + + public void requestShootPrepare() { + if (currentState == Superstructurestate.HOLDING_PIECE) + desiredState = Superstructurestate.PREPARING_TO_SHOOT; + } + + public Superstructurestate getCurrentState() { + return currentState; + } + + public Superstructurestate getDesiredState() { + return desiredState; + } + + public boolean isReady() { + return currentState == desiredState; + } + + public boolean hasGamePiece() { + return inputs.intakeHasGamePiece || currentState.hasGamePiece(); + } + + public void emergencyStop() { + desiredState = Superstructurestate.IDLE; + currentState = Superstructurestate.IDLE; + intake.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java new file mode 100644 index 0000000..533e414 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.superstructure; + +import org.littletonrobotics.junction.AutoLog; + +public interface SuperstructureIO { + @AutoLog + class SuperstructureIOInputs { + public Superstructurestate currentState = Superstructurestate.IDLE; + public Superstructurestate desiredState = Superstructurestate.IDLE; + public double stateStartTime = 0.0; + + //Intake Status + public boolean intakeHasGamePiece = false; + public boolean intakeDeployed = false; + public double intakeArmPosition = 0.0; + + //Shooter status stuff, may need to be changed later when actually implemented + public boolean shooterAtSpeed = false; + public double shooterVelocity = 0.0; + + //Ready status + public boolean readyToIntake = false; + public boolean readyToShoot = false; + } + + default void updateInputs(SuperstructureIOInputs inputs) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java new file mode 100644 index 0000000..74ab4cd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.superstructure; + +public class SuperstructureIOReal implements SuperstructureIO { + @Override + public void updateInputs(SuperstructureIOInputs inputs) { + // No hardware to update + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java new file mode 100644 index 0000000..a505848 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.superstructure; + + public enum Superstructurestate { + IDLE, + INTAKING, + OUTTAKING, + STOWED, + DEPLOYED, + SHOOTING, + HOLDING_PIECE, + TRANSITIONING, + PREPARING_TO_SHOOT; + + public boolean isIntakeDeployed() { + return this == DEPLOYED || this == STOWED; + } + + public boolean hasGamePiece(){ + return this == HOLDING_PIECE || this == PREPARING_TO_SHOOT || this == SHOOTING; + } + + public boolean isAquiringPiece(){ + return this == INTAKING; + } + } From 5cf677aaafe03fe69d3b2b64fb1b0730ea3b0730 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Mon, 24 Nov 2025 19:16:57 -0800 Subject: [PATCH 16/51] Changed tuner constants and format --- src/main/java/frc/robot/RobotContainer.java | 26 ++--- .../commands/SuperstructureCommands.java | 106 ++++++++++-------- .../frc/robot/generated/TunerConstants.java | 63 +++++------ .../intake/IntakeIOTalonFXReal.java | 5 +- .../subsystems/intake/IntakeIOTalonFXSim.java | 8 +- .../superstructure/Superstructure.java | 22 ++-- .../superstructure/SuperstructureIO.java | 16 +-- .../superstructure/SuperstructureIOReal.java | 2 +- .../superstructure/Superstructurestate.java | 38 +++---- 9 files changed, 143 insertions(+), 143 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cbd69e6..f9fb222 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,7 +36,6 @@ import frc.robot.subsystems.drive.*; import frc.robot.subsystems.intake.*; import frc.robot.subsystems.superstructure.Superstructure; -import frc.robot.subsystems.superstructure.SuperstructureIO; import frc.robot.subsystems.superstructure.SuperstructureIOReal; import frc.robot.subsystems.vision.*; import org.ironmaple.simulation.SimulatedArena; @@ -158,7 +157,7 @@ private void setupRobotMechanism() { * and then passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - //============DRIVER CONTROLLER BINDINGS (DRIVE)============// + // ============DRIVER CONTROLLER BINDINGS (DRIVE)============// // Default command, normal field-relative drive drive.setDefaultCommand(DriveCommands.joystickDrive( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); @@ -180,34 +179,33 @@ private void configureButtonBindings() { : () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); - //============DRIVER CONTROLLER BINDINGS (SYSTEM)============// - //Intake (no timeout) + // ============DRIVER CONTROLLER BINDINGS (SYSTEM)============// + // Intake (no timeout) controller.a().onTrue(SuperstructureCommands.intake(superstructure)); - //Eject for 0.5 seconds + // Eject for 0.5 seconds controller.b().onTrue(SuperstructureCommands.outtake(superstructure, 0.5)); - //Stow (AKA Return to Idle) + // Stow (AKA Return to Idle) controller.x().onTrue(SuperstructureCommands.stow(superstructure)); - //Emergency stop + // Emergency stop controller.y().onTrue(SuperstructureCommands.emergencyStop(superstructure)); - //Prepare to shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) + // Prepare to shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) controller.povLeft().onTrue(SuperstructureCommands.prepareToShoot(superstructure)); - //Shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) + // Shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) controller.povRight().onTrue(SuperstructureCommands.shoot(superstructure)); - - //============OPERATOR CONTROLLER BINDINGS (SYSTEM)============// - //Manual intake + // ============OPERATOR CONTROLLER BINDINGS (SYSTEM)============// + // Manual intake operator.a().whileTrue(SuperstructureCommands.manualIntake(superstructure)); - //Manual eject + // Manual eject operator.b().whileTrue(SuperstructureCommands.manualOuttake(superstructure)); - //Intake (with Timeout) + // Intake (with Timeout) operator.x().onTrue(SuperstructureCommands.intakeWithTimeout(superstructure, 3.0)); } diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index ddb2962..541b400 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -1,84 +1,94 @@ package frc.robot.commands; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.superstructure.Superstructurestate; import frc.robot.subsystems.superstructure.Superstructure; +import frc.robot.subsystems.superstructure.Superstructurestate; public class SuperstructureCommands { - //Command to intake gamepiece with no time limits + // Command to intake gamepiece with no time limits public static Command intake(Superstructure superstructure) { return Commands.sequence( - Commands.runOnce(() -> superstructure.requestIntake(), superstructure), - Commands.waitUntil(() -> superstructure.hasGamePiece()), - Commands.runOnce(() -> superstructure.requestIdle(), superstructure) - ).withName("Intake").alongWith(Commands.run(() -> {}, superstructure.getIntake())); + Commands.runOnce(() -> superstructure.requestIntake(), superstructure), + Commands.waitUntil(() -> superstructure.hasGamePiece()), + Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) + .withName("Intake") + .alongWith(Commands.run(() -> {}, superstructure.getIntake())); } - //Command to intake gamepiece with timeout, stops trying to intake after timeout seconds no matter if it has gamepiece or not + // Command to intake gamepiece with timeout, stops trying to intake after timeout seconds no matter if it has + // gamepiece or not public static Command intakeWithTimeout(Superstructure superstructure, double timeout) { return Commands.sequence( - Commands.runOnce(() -> superstructure.requestIntake(), superstructure), - Commands.either( - Commands.waitUntil(() -> superstructure.hasGamePiece()), - Commands.waitSeconds(timeout), - () -> !superstructure.hasGamePiece() - ), - Commands.runOnce(() -> superstructure.requestIdle(), superstructure) - ).withName("IntakeWithTimeout").alongWith(Commands.run(() -> {}, superstructure.getIntake())); + Commands.runOnce(() -> superstructure.requestIntake(), superstructure), + Commands.either( + Commands.waitUntil(() -> superstructure.hasGamePiece()), + Commands.waitSeconds(timeout), + () -> !superstructure.hasGamePiece()), + Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) + .withName("IntakeWithTimeout") + .alongWith(Commands.run(() -> {}, superstructure.getIntake())); } - //Command to manually run intake + // Command to manually run intake public static Command manualIntake(Superstructure superstructure) { - return Commands.startEnd(() -> superstructure.requestIntake(), () -> superstructure.requestIdle(), superstructure) - .withName("ManualIntake").alongWith(Commands.run(() -> {}, superstructure.getIntake())); + return Commands.startEnd( + () -> superstructure.requestIntake(), () -> superstructure.requestIdle(), superstructure) + .withName("ManualIntake") + .alongWith(Commands.run(() -> {}, superstructure.getIntake())); } - //Command to outtake gamepiece - public static Command outtake(Superstructure superstructure, double duration){ + // Command to outtake gamepiece + public static Command outtake(Superstructure superstructure, double duration) { return Commands.sequence( - Commands.runOnce(() -> superstructure.requestOuttake(), superstructure), - Commands.waitSeconds(duration), - Commands.runOnce(() -> superstructure.requestIdle(), superstructure) - ).withName("Eject"); + Commands.runOnce(() -> superstructure.requestOuttake(), superstructure), + Commands.waitSeconds(duration), + Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) + .withName("Eject"); } - //Command to manually outtake + // Command to manually outtake public static Command manualOuttake(Superstructure superstructure) { - return Commands.startEnd(() -> superstructure.requestOuttake(), () -> superstructure.requestIdle(), superstructure).withName("ManualEject"); + return Commands.startEnd( + () -> superstructure.requestOuttake(), () -> superstructure.requestIdle(), superstructure) + .withName("ManualEject"); } - //Command to stow intake - public static Command stow(Superstructure superstructure){ - return Commands.runOnce(()-> superstructure.requestIdle(), superstructure).withName("Stow"); + // Command to stow intake + public static Command stow(Superstructure superstructure) { + return Commands.runOnce(() -> superstructure.requestIdle(), superstructure) + .withName("Stow"); } - //Command to wait until superstructure is ready - public static Command waitForReady(Superstructure superstructure){ + // Command to wait until superstructure is ready + public static Command waitForReady(Superstructure superstructure) { return Commands.waitUntil(() -> superstructure.isReady()).withName("WaitForReady"); } - //Command to prep for shooting - //REMMEBER TO FIX IMPLEMENTATION WHEN SHOOTER IS FULLY DONE - public static Command prepareToShoot(Superstructure superstructure){ + // Command to prep for shooting + // REMMEBER TO FIX IMPLEMENTATION WHEN SHOOTER IS FULLY DONE + public static Command prepareToShoot(Superstructure superstructure) { return Commands.sequence( - Commands.runOnce(() -> superstructure.requestShootPrepare(), superstructure), - Commands.waitUntil(() -> superstructure.getCurrentState() == Superstructurestate.PREPARING_TO_SHOOT && superstructure.isReady()) - ).withName("PrepareToShoot"); + Commands.runOnce(() -> superstructure.requestShootPrepare(), superstructure), + Commands.waitUntil( + () -> superstructure.getCurrentState() == Superstructurestate.PREPARING_TO_SHOOT + && superstructure.isReady())) + .withName("PrepareToShoot"); } - //Command to shoot - //REmember TO FIX IMPLEMENTATION WHEN SHOOTER IS FULLY DONE - public static Command shoot(Superstructure superstructure){ + // Command to shoot + // REmember TO FIX IMPLEMENTATION WHEN SHOOTER IS FULLY DONE + public static Command shoot(Superstructure superstructure) { return Commands.sequence( - Commands.runOnce(() -> superstructure.requestShoot(), superstructure), - Commands.waitSeconds(0.5), - Commands.runOnce(() -> superstructure.requestIdle(), superstructure) - ).withName("Shoot"); + Commands.runOnce(() -> superstructure.requestShoot(), superstructure), + Commands.waitSeconds(0.5), + Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) + .withName("Shoot"); } - //Command to emergency stop the superstructure - public static Command emergencyStop(Superstructure superstructure){ - return Commands.runOnce(() -> superstructure.emergencyStop(), superstructure).withName("EmergencyStop"); + // Command to emergency stop the superstructure + public static Command emergencyStop(Superstructure superstructure) { + return Commands.runOnce(() -> superstructure.emergencyStop(), superstructure) + .withName("EmergencyStop"); } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index b1ceb94..fbd6d5d 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -25,7 +25,7 @@ public class TunerConstants { .withKI(0) .withKD(0.5) .withKS(0.1) - .withKV(1.91) + .withKV(1.79) .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -46,7 +46,7 @@ public class TunerConstants { private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; // The stator current at which the wheels start to slip; @@ -58,8 +58,7 @@ public class TunerConstants { private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits(new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively - // low + // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true)); @@ -69,28 +68,28 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.96); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; + private static final double kCoupleRatio = 3.125; - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); + private static final double kDriveGearRatio = 5.2734375; // 5.357142857142857 + private static final double kSteerGearRatio = 18.75; + private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 1; + private static final int kPigeonId = 13; // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // Simulated voltage necessary to overcome friction private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); @@ -128,45 +127,45 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 8; private static final int kFrontLeftSteerMotorId = 4; private static final int kFrontLeftEncoderId = 12; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.198974609375); + private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); + private static final Distance kFrontLeftXPos = Inches.of(10.375); + private static final Distance kFrontLeftYPos = Inches.of(10.375); // Front Right private static final int kFrontRightDriveMotorId = 5; private static final int kFrontRightSteerMotorId = 2; private static final int kFrontRightEncoderId = 11; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.0439453125); + private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); + private static final Distance kFrontRightXPos = Inches.of(10.375); + private static final Distance kFrontRightYPos = Inches.of(-10.375); // Back Left private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 1; private static final int kBackLeftEncoderId = 10; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.487548828125); + private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); + private static final Distance kBackLeftXPos = Inches.of(-10.375); + private static final Distance kBackLeftYPos = Inches.of(10.375); // Back Right private static final int kBackRightDriveMotorId = 6; private static final int kBackRightSteerMotorId = 3; private static final int kBackRightEncoderId = 13; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.139404296875); + private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); + private static final Distance kBackRightXPos = Inches.of(-10.375); + private static final Distance kBackRightYPos = Inches.of(-10.375); public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( @@ -213,12 +212,6 @@ public class TunerConstants { kBackRightSteerMotorInverted, kBackRightEncoderInverted); - /** Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot program,. */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - // } - /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ public static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 1fc54e2..028a065 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -17,7 +17,7 @@ public class IntakeIOTalonFXReal implements IntakeIO { // Hardware configuration constants private static final int ROLLER_MOTOR_ID = 20; - private static final int ARM_MOTOR_ID = 21; + private static final int ARM_MOTOR_ID = 19; private static final double ROLLER_GEAR_RATIO = 3.0; private static final double ARM_GEAR_RATIO = 100.0; @@ -122,7 +122,8 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armCurrent = armCurrent.getValueAsDouble(); // Sensor inputs - inputs.hasGamePiece = rollerMotor.getStatorCurrent().getValueAsDouble() > 40.0; //May need to change current limit and detection later + inputs.hasGamePiece = rollerMotor.getStatorCurrent().getValueAsDouble() + > 40.0; // May need to change current limit and detection later } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index f5c6b74..6eaf1cf 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -66,14 +66,16 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armVoltage = armAppliedVolts; inputs.armCurrent = armSim.getCurrentDrawAmps(); - if (!simulatedGamePiece && rollerAppliedVolts > 6.0 && Math.abs(rollerSim.getAngularVelocityRadPerSec()) > 10.0) { + if (!simulatedGamePiece + && rollerAppliedVolts > 6.0 + && Math.abs(rollerSim.getAngularVelocityRadPerSec()) > 10.0) { simulatedGamePiece = true; } - + if (simulatedGamePiece && rollerAppliedVolts < -6.0) { simulatedGamePiece = false; } - + inputs.hasGamePiece = simulatedGamePiece; } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index b7378b2..f10ecaa 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -67,7 +67,7 @@ private void runStateMachine() { } if (currentState == desiredState) { - runActiveStateLogic(); + runActiveStateLogic(); return; } @@ -119,7 +119,7 @@ private void runActiveStateLogic() { private void handleIdleState() { intake.stowArm(); intake.stop(); - + switch (desiredState) { case INTAKING: intake.deployArm(); @@ -144,7 +144,7 @@ private void handleIntakingState() { changeState(Superstructurestate.TRANSITIONING); desiredState = Superstructurestate.HOLDING_PIECE; } - + if (desiredState != Superstructurestate.INTAKING && desiredState != Superstructurestate.HOLDING_PIECE) { intake.stop(); intake.stowArm(); @@ -156,7 +156,7 @@ private void handleTransitioningState() { switch (desiredState) { case INTAKING: if (inputs.readyToIntake) { - intake.setRollerVoltage(8.0); + intake.setRollerVoltage(8.0); changeState(Superstructurestate.INTAKING); } break; @@ -199,12 +199,10 @@ private void handleHoldingPieceState() { if (desiredState == Superstructurestate.OUTTAKING) { intake.deployArm(); changeState(Superstructurestate.TRANSITIONING); - } - else if (desiredState == Superstructurestate.INTAKING) { + } else if (desiredState == Superstructurestate.INTAKING) { intake.deployArm(); changeState(Superstructurestate.TRANSITIONING); - } - else if (desiredState == Superstructurestate.IDLE) { + } else if (desiredState == Superstructurestate.IDLE) { changeState(Superstructurestate.IDLE); } } @@ -258,13 +256,11 @@ public void requestIdle() { } public void requestShoot() { - if (currentState == Superstructurestate.PREPARING_TO_SHOOT) - desiredState = Superstructurestate.SHOOTING; + if (currentState == Superstructurestate.PREPARING_TO_SHOOT) desiredState = Superstructurestate.SHOOTING; } public void requestShootPrepare() { - if (currentState == Superstructurestate.HOLDING_PIECE) - desiredState = Superstructurestate.PREPARING_TO_SHOOT; + if (currentState == Superstructurestate.HOLDING_PIECE) desiredState = Superstructurestate.PREPARING_TO_SHOOT; } public Superstructurestate getCurrentState() { @@ -288,4 +284,4 @@ public void emergencyStop() { currentState = Superstructurestate.IDLE; intake.stop(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java index 533e414..056dfc9 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java @@ -8,20 +8,20 @@ class SuperstructureIOInputs { public Superstructurestate currentState = Superstructurestate.IDLE; public Superstructurestate desiredState = Superstructurestate.IDLE; public double stateStartTime = 0.0; - - //Intake Status + + // Intake Status public boolean intakeHasGamePiece = false; public boolean intakeDeployed = false; public double intakeArmPosition = 0.0; - - //Shooter status stuff, may need to be changed later when actually implemented + + // Shooter status stuff, may need to be changed later when actually implemented public boolean shooterAtSpeed = false; public double shooterVelocity = 0.0; - - //Ready status + + // Ready status public boolean readyToIntake = false; public boolean readyToShoot = false; } - + default void updateInputs(SuperstructureIOInputs inputs) {} -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java index 74ab4cd..5d1f103 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java @@ -5,4 +5,4 @@ public class SuperstructureIOReal implements SuperstructureIO { public void updateInputs(SuperstructureIOInputs inputs) { // No hardware to update } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java index a505848..6fc50e5 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java @@ -1,25 +1,25 @@ package frc.robot.subsystems.superstructure; - public enum Superstructurestate { - IDLE, - INTAKING, - OUTTAKING, - STOWED, - DEPLOYED, - SHOOTING, - HOLDING_PIECE, - TRANSITIONING, - PREPARING_TO_SHOOT; +public enum Superstructurestate { + IDLE, + INTAKING, + OUTTAKING, + STOWED, + DEPLOYED, + SHOOTING, + HOLDING_PIECE, + TRANSITIONING, + PREPARING_TO_SHOOT; - public boolean isIntakeDeployed() { - return this == DEPLOYED || this == STOWED; - } + public boolean isIntakeDeployed() { + return this == DEPLOYED || this == STOWED; + } - public boolean hasGamePiece(){ - return this == HOLDING_PIECE || this == PREPARING_TO_SHOOT || this == SHOOTING; - } + public boolean hasGamePiece() { + return this == HOLDING_PIECE || this == PREPARING_TO_SHOOT || this == SHOOTING; + } - public boolean isAquiringPiece(){ - return this == INTAKING; - } + public boolean isAquiringPiece() { + return this == INTAKING; } +} From cdc269c91e853e1d67230342bc44578d07d8fcbf Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Mon, 24 Nov 2025 20:07:01 -0800 Subject: [PATCH 17/51] updated tunerconstants --- src/main/java/frc/robot/generated/TunerConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index fbd6d5d..91e6e82 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -78,7 +78,7 @@ public class TunerConstants { // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.125; - private static final double kDriveGearRatio = 5.2734375; // 5.357142857142857 + private static final double kDriveGearRatio = 5.357142857142857; private static final double kSteerGearRatio = 18.75; private static final Distance kWheelRadius = Inches.of(2); @@ -127,7 +127,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 8; private static final int kFrontLeftSteerMotorId = 4; private static final int kFrontLeftEncoderId = 12; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.198974609375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.2001953125); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 5; private static final int kFrontRightSteerMotorId = 2; private static final int kFrontRightEncoderId = 11; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.0439453125); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.04638671875); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 1; private static final int kBackLeftEncoderId = 10; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.487548828125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.487060546875); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 6; private static final int kBackRightSteerMotorId = 3; private static final int kBackRightEncoderId = 13; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.139404296875); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.2255859375); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; From 132419a23ff8342b3b00c8e51dbff4fc8e0d5877 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Mon, 24 Nov 2025 21:50:36 -0800 Subject: [PATCH 18/51] changed bindings --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9fb222..d1cd180 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -180,23 +180,23 @@ private void configureButtonBindings() { controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); // ============DRIVER CONTROLLER BINDINGS (SYSTEM)============// - // Intake (no timeout) - controller.a().onTrue(SuperstructureCommands.intake(superstructure)); + // Intake (with timeout) + controller.leftBumper().onTrue(SuperstructureCommands.intakeWithTimeout(superstructure,3.0)); // Eject for 0.5 seconds - controller.b().onTrue(SuperstructureCommands.outtake(superstructure, 0.5)); + controller.rightBumper().onTrue(SuperstructureCommands.outtake(superstructure, 0.5)); // Stow (AKA Return to Idle) - controller.x().onTrue(SuperstructureCommands.stow(superstructure)); + controller.a().onTrue(SuperstructureCommands.stow(superstructure)); // Emergency stop controller.y().onTrue(SuperstructureCommands.emergencyStop(superstructure)); // Prepare to shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) - controller.povLeft().onTrue(SuperstructureCommands.prepareToShoot(superstructure)); + controller.x().onTrue(SuperstructureCommands.prepareToShoot(superstructure)); // Shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) - controller.povRight().onTrue(SuperstructureCommands.shoot(superstructure)); + controller.b().onTrue(SuperstructureCommands.shoot(superstructure)); // ============OPERATOR CONTROLLER BINDINGS (SYSTEM)============// // Manual intake @@ -205,8 +205,8 @@ private void configureButtonBindings() { // Manual eject operator.b().whileTrue(SuperstructureCommands.manualOuttake(superstructure)); - // Intake (with Timeout) - operator.x().onTrue(SuperstructureCommands.intakeWithTimeout(superstructure, 3.0)); + // Intake (without timeout) + operator.x().onTrue(SuperstructureCommands.intake(superstructure)); } /** From b7cf19229e37c781753ddc943c05323d600e6dbf Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Mon, 24 Nov 2025 21:50:50 -0800 Subject: [PATCH 19/51] removed useless function --- .../superstructure/Superstructure.java | 30 ++++--------------- 1 file changed, 5 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index f10ecaa..0d07ef1 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -66,11 +66,6 @@ private void runStateMachine() { transitionToIdle(); } - if (currentState == desiredState) { - runActiveStateLogic(); - return; - } - switch (currentState) { case IDLE: handleIdleState(); @@ -100,22 +95,6 @@ private void runStateMachine() { } } - private void runActiveStateLogic() { - switch (currentState) { - case INTAKING: - intake.setRollerVoltage(8.0); - break; - case OUTTAKING: - intake.setRollerVoltage(-8.0); - break; - case HOLDING_PIECE: - intake.stop(); - break; - default: - break; - } - } - private void handleIdleState() { intake.stowArm(); intake.stop(); @@ -144,8 +123,7 @@ private void handleIntakingState() { changeState(Superstructurestate.TRANSITIONING); desiredState = Superstructurestate.HOLDING_PIECE; } - - if (desiredState != Superstructurestate.INTAKING && desiredState != Superstructurestate.HOLDING_PIECE) { + else if (desiredState != Superstructurestate.INTAKING && desiredState != Superstructurestate.HOLDING_PIECE) { intake.stop(); intake.stowArm(); changeState(Superstructurestate.TRANSITIONING); @@ -256,11 +234,13 @@ public void requestIdle() { } public void requestShoot() { - if (currentState == Superstructurestate.PREPARING_TO_SHOOT) desiredState = Superstructurestate.SHOOTING; + if (currentState == Superstructurestate.PREPARING_TO_SHOOT) + desiredState = Superstructurestate.SHOOTING; } public void requestShootPrepare() { - if (currentState == Superstructurestate.HOLDING_PIECE) desiredState = Superstructurestate.PREPARING_TO_SHOOT; + if (currentState == Superstructurestate.HOLDING_PIECE) + desiredState = Superstructurestate.PREPARING_TO_SHOOT; } public Superstructurestate getCurrentState() { From 078f79a7f9ec409f501e16bdc0a3824ecf847c67 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 25 Nov 2025 22:23:04 -0800 Subject: [PATCH 20/51] Updated tuner constants --- src/main/java/frc/robot/generated/TunerConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 91e6e82..b7dcd15 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -78,7 +78,7 @@ public class TunerConstants { // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.125; - private static final double kDriveGearRatio = 5.357142857142857; + private static final double kDriveGearRatio = 675 / 128; private static final double kSteerGearRatio = 18.75; private static final Distance kWheelRadius = Inches.of(2); @@ -127,7 +127,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 8; private static final int kFrontLeftSteerMotorId = 4; private static final int kFrontLeftEncoderId = 12; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.2001953125); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.198486328125); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 5; private static final int kFrontRightSteerMotorId = 2; private static final int kFrontRightEncoderId = 11; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.04638671875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.04296875); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 1; private static final int kBackLeftEncoderId = 10; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.487060546875); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.488525390625); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 6; private static final int kBackRightSteerMotorId = 3; private static final int kBackRightEncoderId = 13; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.2255859375); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.10400390625); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; From fe8857f010828f35314dafe2a93b3f83ddec4af0 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 25 Nov 2025 22:24:28 -0800 Subject: [PATCH 21/51] Tuned arm deploy position and new arm function --- src/main/java/frc/robot/subsystems/intake/Intake.java | 8 ++++++-- src/main/java/frc/robot/subsystems/intake/IntakeIO.java | 2 ++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 6ed71d2..be20822 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -12,7 +12,7 @@ public class Intake extends SubsystemBase { // Arm position constants (radians) public static final double ARM_STOWED_POSITION = 0.0; - public static final double ARM_DEPLOYED_POSITION = Math.PI / 2; // 90 degrees + public static final double ARM_DEPLOYED_POSITION = -1.62; private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); @@ -129,6 +129,10 @@ public boolean isArmAtPosition(double targetRad, double toleranceRad) { // Stop all motors public void stop() { io.stopRoller(); - io.setArmVoltage(0.0); + } + + //Stop arm + public void stopArm(){ + io.stopArmMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 442b27d..98e1210 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -32,4 +32,6 @@ default void setArmPosition(double positionRad) {} default void resetArmPosition() {} default void stopRoller() {} + + default void stopArmMotor(){} } From 3a3367dd4d1d822d312d9409e66e6575d3eb363f Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 25 Nov 2025 22:24:51 -0800 Subject: [PATCH 22/51] Removed either command --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/SuperstructureCommands.java | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d1cd180..55b8d52 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -181,7 +181,7 @@ private void configureButtonBindings() { // ============DRIVER CONTROLLER BINDINGS (SYSTEM)============// // Intake (with timeout) - controller.leftBumper().onTrue(SuperstructureCommands.intakeWithTimeout(superstructure,3.0)); + controller.leftBumper().onTrue(SuperstructureCommands.intakeWithTimeout(superstructure, 3.0)); // Eject for 0.5 seconds controller.rightBumper().onTrue(SuperstructureCommands.outtake(superstructure, 0.5)); diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index 541b400..dacaded 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -21,10 +21,7 @@ public static Command intake(Superstructure superstructure) { public static Command intakeWithTimeout(Superstructure superstructure, double timeout) { return Commands.sequence( Commands.runOnce(() -> superstructure.requestIntake(), superstructure), - Commands.either( - Commands.waitUntil(() -> superstructure.hasGamePiece()), - Commands.waitSeconds(timeout), - () -> !superstructure.hasGamePiece()), + Commands.waitSeconds(timeout), Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) .withName("IntakeWithTimeout") .alongWith(Commands.run(() -> {}, superstructure.getIntake())); From 0eb3cdceb11af869a8e5f04ba88cc5cadda50eab Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 25 Nov 2025 22:25:08 -0800 Subject: [PATCH 23/51] Added indexer motor --- .../intake/IntakeIOTalonFXReal.java | 55 +++++++++++++++---- .../superstructure/Superstructure.java | 38 ++++--------- .../superstructure/SuperstructureIO.java | 2 +- 3 files changed, 54 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 028a065..27eeba8 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -5,29 +5,29 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.*; -import frc.robot.generated.TunerConstants; public class IntakeIOTalonFXReal implements IntakeIO { // Hardware configuration constants private static final int ROLLER_MOTOR_ID = 20; private static final int ARM_MOTOR_ID = 19; + private static final int INDEXER_MOTOR_ID = 17; private static final double ROLLER_GEAR_RATIO = 3.0; - private static final double ARM_GEAR_RATIO = 100.0; - + private static final double ARM_GEAR_RATIO = 1; // 248/9 // Hardware private final TalonFX rollerMotor; private final TalonFX armMotor; + private final TalonFX indexerMotor; // Control requests - private final VoltageOut voltageRequest = new VoltageOut(0.0); - private final PositionVoltage positionRequest = new PositionVoltage(0.0); + private final DutyCycleOut voltageRequest = new DutyCycleOut(0.0); + private final MotionMagicExpoTorqueCurrentFOC positionRequest = new MotionMagicExpoTorqueCurrentFOC(0.0); // Status signals - Roller private final StatusSignal rollerPosition; @@ -41,15 +41,21 @@ public class IntakeIOTalonFXReal implements IntakeIO { private final StatusSignal armAppliedVolts; private final StatusSignal armCurrent; + // Status Signals - Indexer + private final StatusSignal indexerVelocity; + private final StatusSignal indexerAppliedVolts; + private final StatusSignal indexerCurrent; + public IntakeIOTalonFXReal() { // Initialize hardware - rollerMotor = new TalonFX(ROLLER_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); - armMotor = new TalonFX(ARM_MOTOR_ID, TunerConstants.DrivetrainConstants.CANBusName); + rollerMotor = new TalonFX(ROLLER_MOTOR_ID, "rio"); // CHANGE TO SWERVE LATER + armMotor = new TalonFX(ARM_MOTOR_ID, "rio"); // CHANGE TO SWERVE LATER + indexerMotor = new TalonFX(INDEXER_MOTOR_ID, "rio"); // CHANGE LATER TO SWERVE // Configure roller motor var rollerConfig = new TalonFXConfiguration(); rollerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - rollerConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + rollerConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // Current limits for roller rollerConfig.CurrentLimits.StatorCurrentLimit = 60.0; @@ -59,6 +65,19 @@ public IntakeIOTalonFXReal() { tryUntilOk(5, () -> rollerMotor.getConfigurator().apply(rollerConfig, 0.25)); + // Configure indexer motor + var indexerConfig = new TalonFXConfiguration(); + indexerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + indexerConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + // Current limits for roller + indexerConfig.CurrentLimits.StatorCurrentLimit = 60.0; + indexerConfig.CurrentLimits.StatorCurrentLimitEnable = true; + indexerConfig.CurrentLimits.SupplyCurrentLimit = 50.0; + indexerConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + tryUntilOk(5, () -> indexerMotor.getConfigurator().apply(indexerConfig, 0.25)); + // Configure arm motor var armConfig = new TalonFXConfiguration(); armConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -69,7 +88,7 @@ public IntakeIOTalonFXReal() { armConfig.CurrentLimits.StatorCurrentLimitEnable = true; // PID for arm position control - armConfig.Slot0.kP = 10.0; + armConfig.Slot0.kP = 100.0; armConfig.Slot0.kI = 0.0; armConfig.Slot0.kD = 0.5; armConfig.Slot0.kS = 0.0; @@ -83,6 +102,11 @@ public IntakeIOTalonFXReal() { rollerAppliedVolts = rollerMotor.getMotorVoltage(); rollerCurrent = rollerMotor.getStatorCurrent(); + // Create status signals - Indexer + indexerVelocity = indexerMotor.getVelocity(); + indexerAppliedVolts = indexerMotor.getMotorVoltage(); + indexerCurrent = indexerMotor.getStatorCurrent(); + // Create status signals - Arm armPosition = armMotor.getPosition(); armVelocity = armMotor.getVelocity(); @@ -102,6 +126,7 @@ public IntakeIOTalonFXReal() { armCurrent); rollerMotor.optimizeBusUtilization(); armMotor.optimizeBusUtilization(); + indexerMotor.optimizeBusUtilization(); } @Override @@ -123,12 +148,13 @@ public void updateInputs(IntakeIOInputs inputs) { // Sensor inputs inputs.hasGamePiece = rollerMotor.getStatorCurrent().getValueAsDouble() - > 40.0; // May need to change current limit and detection later + > 100.0; // May need to change current limit and detection later } @Override public void setRollerVoltage(double voltage) { rollerMotor.setControl(voltageRequest.withOutput(voltage)); + indexerMotor.setControl(voltageRequest.withOutput(voltage)); } @Override @@ -150,5 +176,10 @@ public void resetArmPosition() { @Override public void stopRoller() { rollerMotor.stopMotor(); + indexerMotor.stopMotor(); + } + + public void stopArmMotor(){ + armMotor.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index 0d07ef1..80a1859 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -55,7 +55,7 @@ private void updateStateInfo() { inputs.intakeDeployed = intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, ArmPositionTolerance); inputs.intakeArmPosition = intake.getArmPositionRad(); - inputs.readyToIntake = intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, ArmPositionTolerance); + inputs.readyToIntake = true; // intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, ArmPositionTolerance); inputs.readyToShoot = false; // Placeholder for shooter } @@ -96,8 +96,6 @@ private void runStateMachine() { } private void handleIdleState() { - intake.stowArm(); - intake.stop(); switch (desiredState) { case INTAKING: @@ -115,43 +113,29 @@ private void handleIdleState() { } private void handleIntakingState() { - intake.setRollerVoltage(8.0); - - if (intake.hasGamePiece()) { - intake.stop(); - intake.stowArm(); - changeState(Superstructurestate.TRANSITIONING); - desiredState = Superstructurestate.HOLDING_PIECE; - } - else if (desiredState != Superstructurestate.INTAKING && desiredState != Superstructurestate.HOLDING_PIECE) { - intake.stop(); - intake.stowArm(); - changeState(Superstructurestate.TRANSITIONING); - } + intake.setRollerVoltage(0.8); + intake.setArmPosition(Intake.ARM_DEPLOYED_POSITION); } private void handleTransitioningState() { switch (desiredState) { case INTAKING: if (inputs.readyToIntake) { - intake.setRollerVoltage(8.0); + intake.setRollerVoltage(0.5); changeState(Superstructurestate.INTAKING); } break; case OUTTAKING: if (inputs.readyToIntake) { - intake.setRollerVoltage(-8.0); + intake.setRollerVoltage(-0.5); changeState(Superstructurestate.OUTTAKING); } break; case IDLE: - // Wait for arm to stow - if (intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, ArmPositionTolerance)) { - intake.stop(); - changeState(Superstructurestate.IDLE); - } + intake.stop(); + changeState(Superstructurestate.IDLE); break; case HOLDING_PIECE: @@ -186,7 +170,7 @@ private void handleHoldingPieceState() { } private void handleOuttakingState() { - intake.setRollerVoltage(-8.0); + intake.setRollerVoltage(-0.5); if (desiredState != Superstructurestate.OUTTAKING) { intake.stop(); @@ -234,13 +218,11 @@ public void requestIdle() { } public void requestShoot() { - if (currentState == Superstructurestate.PREPARING_TO_SHOOT) - desiredState = Superstructurestate.SHOOTING; + if (currentState == Superstructurestate.PREPARING_TO_SHOOT) desiredState = Superstructurestate.SHOOTING; } public void requestShootPrepare() { - if (currentState == Superstructurestate.HOLDING_PIECE) - desiredState = Superstructurestate.PREPARING_TO_SHOOT; + if (currentState == Superstructurestate.HOLDING_PIECE) desiredState = Superstructurestate.PREPARING_TO_SHOOT; } public Superstructurestate getCurrentState() { diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java index 056dfc9..02d839d 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java @@ -19,7 +19,7 @@ class SuperstructureIOInputs { public double shooterVelocity = 0.0; // Ready status - public boolean readyToIntake = false; + public boolean readyToIntake = true; public boolean readyToShoot = false; } From 3ff3ebc1331199cf7f2df6caf961ea395f18ebd0 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Tue, 25 Nov 2025 23:16:46 -0800 Subject: [PATCH 24/51] Updated advantage scope layouts --- AdvantageScope Swerve Calibration.json | 134 ++++++++++++++++++++++--- 1 file changed, 119 insertions(+), 15 deletions(-) diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json index f412ed2..513c508 100644 --- a/AdvantageScope Swerve Calibration.json +++ b/AdvantageScope Swerve Calibration.json @@ -1,10 +1,10 @@ { "hubs": [ { - "x": 139, - "y": 95, - "width": 1100, - "height": 650, + "x": -8, + "y": -8, + "width": 1936, + "height": 1048, "state": { "sidebar": { "width": 300, @@ -13,11 +13,19 @@ "/RealOutputs", "/RealOutputs/SwerveStates", "/RealOutputs/SwerveChassisSpeeds", - "/RealOutputs/Odometry" + "/RealOutputs/Odometry", + "/AdvantageKit/RealOutputs", + "/AdvantageKit/RealOutputs/Odometry/Robot", + "/AdvantageKit/RealOutputs/Vision/Camera1", + "/SmartDashboard", + "/AdvantageKit/RealOutputs/Superstructure", + "/AdvantageKit", + "/AdvantageKit/Intake", + "/AdvantageKit/Superstructure" ] }, "tabs": { - "selected": 1, + "selected": 2, "tabs": [ { "type": 0, @@ -76,6 +84,51 @@ "logType": "Rotation2d", "visible": true, "options": {} + }, + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ff00", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ffff00" + } + }, + { + "type": "chassisSpeeds", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Setpoints", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff8c00" + } + }, + { + "type": "rotation", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} } ], "maxSpeed": 5, @@ -120,9 +173,45 @@ "model": "Crab Bot", "color": "#ff8c00" } + }, + { + "type": "robot", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "2024 KitBot" + } + }, + { + "type": "vision", + "logKey": "NT:/AdvantageKit/RealOutputs/Vision/Camera0/TagPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "vision", + "logKey": "NT:/AdvantageKit/RealOutputs/Vision/Camera1/TagPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "mechanism", + "logKey": "NT:/SmartDashboard/Intake/Mechanism", + "logType": "Mechanism2d", + "visible": true, + "options": {} } ], - "game": "2025 Field", + "game": "2025 Field (Welded)", "origin": "blue" }, "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", @@ -130,17 +219,32 @@ "cameraIndex": -1, "orbitFov": 50, "cameraPosition": [ - 1.4695761589768238e-15, - 5.999999999999999, - -12 + 5.19113927039247, + 5.88432583191309, + -3.632652004468072 ], "cameraTarget": [ - 0, - 0.5, - 0 + 5.830818917232525, + 0.5503848177969854, + 0.6221527170045741 ] }, - "controlsHeight": 200 + "controlsHeight": 100 + }, + { + "type": 8, + "title": "Joysticks", + "controller": [ + "Xbox Controller (White)", + "None", + "None", + "None", + "None", + "None" + ], + "controllerUUID": "txhox0ps7gozefkq4lb225n2hyhj2s3h", + "renderer": null, + "controlsHeight": 85 }, { "type": 1, @@ -459,5 +563,5 @@ } ], "satellites": [], - "version": "4.0.0-beta-3" + "version": "4.1.5" } From 0d5c4a709a816f6986dbf2c8884396557a8f2983 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Tue, 25 Nov 2025 23:17:30 -0800 Subject: [PATCH 25/51] Changed commands used --- src/main/java/frc/robot/RobotContainer.java | 38 ++++++--------------- 1 file changed, 11 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 55b8d52..0bdd5e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -162,14 +162,14 @@ private void configureButtonBindings() { drive.setDefaultCommand(DriveCommands.joystickDrive( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); - // Lock to 0° when A button is held - controller - .povDown() - .whileTrue(DriveCommands.joystickDriveAtAngle( - drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> new Rotation2d())); + // // Lock to 0° when A button is held + // controller + // .povDown() + // .whileTrue(DriveCommands.joystickDriveAtAngle( + // drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> new Rotation2d())); - // Switch to X pattern when X button is pressed - controller.povUp().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // // Switch to X pattern when X button is pressed + // controller.povUp().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro / odometry final Runnable resetGyro = Constants.currentMode == Constants.Mode.SIM @@ -180,33 +180,17 @@ private void configureButtonBindings() { controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); // ============DRIVER CONTROLLER BINDINGS (SYSTEM)============// - // Intake (with timeout) - controller.leftBumper().onTrue(SuperstructureCommands.intakeWithTimeout(superstructure, 3.0)); + // Intake + controller.leftBumper().whileTrue(SuperstructureCommands.intake(superstructure)); // left bumper // Eject for 0.5 seconds - controller.rightBumper().onTrue(SuperstructureCommands.outtake(superstructure, 0.5)); + controller.rightBumper().whileTrue(SuperstructureCommands.outtake(superstructure)); // right bumper // Stow (AKA Return to Idle) - controller.a().onTrue(SuperstructureCommands.stow(superstructure)); + controller.a().onTrue(SuperstructureCommands.returnToIdle(superstructure)); // a // Emergency stop controller.y().onTrue(SuperstructureCommands.emergencyStop(superstructure)); - - // Prepare to shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) - controller.x().onTrue(SuperstructureCommands.prepareToShoot(superstructure)); - - // Shoot (WONT DO ANYTHING UNTIL SHOOTER IMPLEMENTED) - controller.b().onTrue(SuperstructureCommands.shoot(superstructure)); - - // ============OPERATOR CONTROLLER BINDINGS (SYSTEM)============// - // Manual intake - operator.a().whileTrue(SuperstructureCommands.manualIntake(superstructure)); - - // Manual eject - operator.b().whileTrue(SuperstructureCommands.manualOuttake(superstructure)); - - // Intake (without timeout) - operator.x().onTrue(SuperstructureCommands.intake(superstructure)); } /** From 772b0f1cc3c6a9469e03f2ed13e4f4d74eb7236e Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Tue, 25 Nov 2025 23:18:10 -0800 Subject: [PATCH 26/51] Removed extra outtake and intake commands, replaced with startEnd commands --- .../commands/SuperstructureCommands.java | 76 ++++--------------- 1 file changed, 14 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index dacaded..436cf22 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -3,58 +3,31 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.superstructure.Superstructure; -import frc.robot.subsystems.superstructure.Superstructurestate; public class SuperstructureCommands { - // Command to intake gamepiece with no time limits - public static Command intake(Superstructure superstructure) { - return Commands.sequence( - Commands.runOnce(() -> superstructure.requestIntake(), superstructure), - Commands.waitUntil(() -> superstructure.hasGamePiece()), - Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) - .withName("Intake") - .alongWith(Commands.run(() -> {}, superstructure.getIntake())); - } - // Command to intake gamepiece with timeout, stops trying to intake after timeout seconds no matter if it has - // gamepiece or not - public static Command intakeWithTimeout(Superstructure superstructure, double timeout) { - return Commands.sequence( - Commands.runOnce(() -> superstructure.requestIntake(), superstructure), - Commands.waitSeconds(timeout), - Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) - .withName("IntakeWithTimeout") - .alongWith(Commands.run(() -> {}, superstructure.getIntake())); + // Command to return to idle state + public static Command returnToIdle(Superstructure superstructure) { + return Commands.runOnce(() -> superstructure.requestIdle(), superstructure) + .withName("ReturnToIdle"); } - // Command to manually run intake - public static Command manualIntake(Superstructure superstructure) { + public static Command intake(Superstructure superstructure) { return Commands.startEnd( - () -> superstructure.requestIntake(), () -> superstructure.requestIdle(), superstructure) - .withName("ManualIntake") + () -> superstructure.requestIntake(), + () -> superstructure.requestIdle(), + superstructure) + .withName("intake") .alongWith(Commands.run(() -> {}, superstructure.getIntake())); } - // Command to outtake gamepiece - public static Command outtake(Superstructure superstructure, double duration) { - return Commands.sequence( - Commands.runOnce(() -> superstructure.requestOuttake(), superstructure), - Commands.waitSeconds(duration), - Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) - .withName("Eject"); - } - // Command to manually outtake - public static Command manualOuttake(Superstructure superstructure) { + public static Command outtake(Superstructure superstructure) { return Commands.startEnd( - () -> superstructure.requestOuttake(), () -> superstructure.requestIdle(), superstructure) - .withName("ManualEject"); - } - - // Command to stow intake - public static Command stow(Superstructure superstructure) { - return Commands.runOnce(() -> superstructure.requestIdle(), superstructure) - .withName("Stow"); + () -> superstructure.requestOuttake(), + () -> superstructure.requestIdle(), + superstructure) + .withName("outtake"); } // Command to wait until superstructure is ready @@ -62,27 +35,6 @@ public static Command waitForReady(Superstructure superstructure) { return Commands.waitUntil(() -> superstructure.isReady()).withName("WaitForReady"); } - // Command to prep for shooting - // REMMEBER TO FIX IMPLEMENTATION WHEN SHOOTER IS FULLY DONE - public static Command prepareToShoot(Superstructure superstructure) { - return Commands.sequence( - Commands.runOnce(() -> superstructure.requestShootPrepare(), superstructure), - Commands.waitUntil( - () -> superstructure.getCurrentState() == Superstructurestate.PREPARING_TO_SHOOT - && superstructure.isReady())) - .withName("PrepareToShoot"); - } - - // Command to shoot - // REmember TO FIX IMPLEMENTATION WHEN SHOOTER IS FULLY DONE - public static Command shoot(Superstructure superstructure) { - return Commands.sequence( - Commands.runOnce(() -> superstructure.requestShoot(), superstructure), - Commands.waitSeconds(0.5), - Commands.runOnce(() -> superstructure.requestIdle(), superstructure)) - .withName("Shoot"); - } - // Command to emergency stop the superstructure public static Command emergencyStop(Superstructure superstructure) { return Commands.runOnce(() -> superstructure.emergencyStop(), superstructure) From f4c56ec8eabc93e856f4cc09f0890d23098bd2d3 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Tue, 25 Nov 2025 23:18:59 -0800 Subject: [PATCH 27/51] Removed piece detection --- .../frc/robot/subsystems/intake/Intake.java | 17 +++-------------- .../frc/robot/subsystems/intake/IntakeIO.java | 5 +---- .../subsystems/intake/IntakeIOTalonFXSim.java | 6 ------ 3 files changed, 4 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index be20822..6317d16 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -57,13 +57,7 @@ public void periodic() { // Update mechanism visualization armLigament.setAngle(Math.toDegrees(inputs.armPosition)); - - // Change roller color based on game piece detection - if (inputs.hasGamePiece) { - rollerLigament.setColor(new Color8Bit(Color.kYellow)); - } else { - rollerLigament.setColor(new Color8Bit(Color.kGreen)); - } + rollerLigament.setColor(new Color8Bit(Color.kGreen)); } // Set roller voltage (-12 to +12 volts) @@ -96,11 +90,6 @@ public void resetArmPosition() { io.resetArmPosition(); } - // Check if a game piece is detected - public boolean hasGamePiece() { - return inputs.hasGamePiece; - } - // Get roller position in radians public double getRollerPositionRad() { return inputs.rollerPosition; @@ -131,8 +120,8 @@ public void stop() { io.stopRoller(); } - //Stop arm - public void stopArm(){ + // Stop arm + public void stopArm() { io.stopArmMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 98e1210..89c77a7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -16,9 +16,6 @@ class IntakeIOInputs { public double armVoltage = 0.0; public double armPosition = 0.0; public double armVelocity = 0.0; - - // Sensing - public boolean hasGamePiece = false; } default void updateInputs(IntakeIOInputs inputs) {} @@ -33,5 +30,5 @@ default void resetArmPosition() {} default void stopRoller() {} - default void stopArmMotor(){} + default void stopArmMotor() {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index 6eaf1cf..eb49b04 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -76,7 +76,6 @@ public void updateInputs(IntakeIOInputs inputs) { simulatedGamePiece = false; } - inputs.hasGamePiece = simulatedGamePiece; } @Override @@ -113,9 +112,4 @@ public void resetArmPosition() { public void stopRoller() { setRollerVoltage(0.0); } - - // For testing - manually set game piece detection - public void setSimulatedGamePiece(boolean hasGamePiece) { - this.simulatedGamePiece = hasGamePiece; - } } From b043c21230a3a8e52e14755ef8eb192748e812ce Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Tue, 25 Nov 2025 23:19:24 -0800 Subject: [PATCH 28/51] Partially added indexer logging --- .../subsystems/intake/IntakeIOTalonFXReal.java | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 27eeba8..68cea3f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -20,6 +20,7 @@ public class IntakeIOTalonFXReal implements IntakeIO { private static final int INDEXER_MOTOR_ID = 17; private static final double ROLLER_GEAR_RATIO = 3.0; private static final double ARM_GEAR_RATIO = 1; // 248/9 + private static final double INDEXER_GEAR_RATIO = 3.0; // Hardware private final TalonFX rollerMotor; private final TalonFX armMotor; @@ -123,7 +124,10 @@ public IntakeIOTalonFXReal() { armPosition, armVelocity, armAppliedVolts, - armCurrent); + armCurrent, + indexerVelocity, + indexerAppliedVolts, + indexerCurrent); rollerMotor.optimizeBusUtilization(); armMotor.optimizeBusUtilization(); indexerMotor.optimizeBusUtilization(); @@ -133,6 +137,7 @@ public IntakeIOTalonFXReal() { public void updateInputs(IntakeIOInputs inputs) { BaseStatusSignal.refreshAll(rollerPosition, rollerVelocity, rollerAppliedVolts, rollerCurrent); BaseStatusSignal.refreshAll(armPosition, armVelocity, armAppliedVolts, armCurrent); + BaseStatusSignal.refreshAll(indexerVelocity, indexerAppliedVolts, indexerCurrent); // Roller inputs inputs.rollerPosition = Units.rotationsToRadians(rollerPosition.getValueAsDouble()) / ROLLER_GEAR_RATIO; @@ -146,9 +151,10 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armVoltage = armAppliedVolts.getValueAsDouble(); inputs.armCurrent = armCurrent.getValueAsDouble(); - // Sensor inputs - inputs.hasGamePiece = rollerMotor.getStatorCurrent().getValueAsDouble() - > 100.0; // May need to change current limit and detection later + // // Indexer inputs + // inputs.indexerVelocity = Units.rotationsToRadians(indexerVelocity.getValueAsDouble()) / INDEXER_GEAR_RATIO; + // inputs.indexerVoltage = indexerAppliedVolts.getValueAsDouble(); + // inputs.indexerCurrent = indexerCurrent.getValueAsDouble(); } @Override @@ -179,7 +185,7 @@ public void stopRoller() { indexerMotor.stopMotor(); } - public void stopArmMotor(){ + public void stopArmMotor() { armMotor.stopMotor(); } } From efcf057e1cfab7e213473fb722d20c8961e43c3d Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Tue, 25 Nov 2025 23:19:50 -0800 Subject: [PATCH 29/51] Cleaned up intaking logic, removed piece detection --- .../superstructure/Superstructure.java | 61 ++++--------------- .../superstructure/SuperstructureIO.java | 5 -- .../superstructure/Superstructurestate.java | 5 -- 3 files changed, 12 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index 80a1859..c5170fb 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -51,12 +51,8 @@ private void updateStateInfo() { inputs.stateStartTime = stateStartTime; // Intake status - inputs.intakeHasGamePiece = intake.hasGamePiece(); inputs.intakeDeployed = intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, ArmPositionTolerance); inputs.intakeArmPosition = intake.getArmPositionRad(); - - inputs.readyToIntake = true; // intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, ArmPositionTolerance); - inputs.readyToShoot = false; // Placeholder for shooter } // Run state machine logic @@ -76,9 +72,6 @@ private void runStateMachine() { case STOWED: handleStowedState(); break; - case HOLDING_PIECE: - handleHoldingPieceState(); - break; case OUTTAKING: handleOuttakingState(); break; @@ -96,7 +89,6 @@ private void runStateMachine() { } private void handleIdleState() { - switch (desiredState) { case INTAKING: intake.deployArm(); @@ -107,7 +99,9 @@ private void handleIdleState() { changeState(Superstructurestate.TRANSITIONING); break; default: - // Stay in idle + if (intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, ArmPositionTolerance)) { + intake.stopArm(); // Stop arm if at stowed position + } break; } } @@ -115,22 +109,23 @@ private void handleIdleState() { private void handleIntakingState() { intake.setRollerVoltage(0.8); intake.setArmPosition(Intake.ARM_DEPLOYED_POSITION); + if(desiredState != Superstructurestate.INTAKING) { + intake.stop(); + intake.stowArm(); + changeState(Superstructurestate.TRANSITIONING); + } } private void handleTransitioningState() { switch (desiredState) { case INTAKING: - if (inputs.readyToIntake) { - intake.setRollerVoltage(0.5); - changeState(Superstructurestate.INTAKING); - } + intake.setRollerVoltage(0.5); + changeState(Superstructurestate.INTAKING); break; case OUTTAKING: - if (inputs.readyToIntake) { - intake.setRollerVoltage(-0.5); - changeState(Superstructurestate.OUTTAKING); - } + intake.setRollerVoltage(-0.5); + changeState(Superstructurestate.OUTTAKING); break; case IDLE: @@ -138,14 +133,6 @@ private void handleTransitioningState() { changeState(Superstructurestate.IDLE); break; - case HOLDING_PIECE: - // Wait for arm to stow - if (intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, ArmPositionTolerance)) { - intake.stop(); - changeState(Superstructurestate.HOLDING_PIECE); - } - break; - default: // Go to idle if unsure transitionToIdle(); @@ -153,22 +140,6 @@ private void handleTransitioningState() { } } - private void handleHoldingPieceState() { - intake.stowArm(); - intake.stop(); - - // Check for any state changes - if (desiredState == Superstructurestate.OUTTAKING) { - intake.deployArm(); - changeState(Superstructurestate.TRANSITIONING); - } else if (desiredState == Superstructurestate.INTAKING) { - intake.deployArm(); - changeState(Superstructurestate.TRANSITIONING); - } else if (desiredState == Superstructurestate.IDLE) { - changeState(Superstructurestate.IDLE); - } - } - private void handleOuttakingState() { intake.setRollerVoltage(-0.5); @@ -221,10 +192,6 @@ public void requestShoot() { if (currentState == Superstructurestate.PREPARING_TO_SHOOT) desiredState = Superstructurestate.SHOOTING; } - public void requestShootPrepare() { - if (currentState == Superstructurestate.HOLDING_PIECE) desiredState = Superstructurestate.PREPARING_TO_SHOOT; - } - public Superstructurestate getCurrentState() { return currentState; } @@ -237,10 +204,6 @@ public boolean isReady() { return currentState == desiredState; } - public boolean hasGamePiece() { - return inputs.intakeHasGamePiece || currentState.hasGamePiece(); - } - public void emergencyStop() { desiredState = Superstructurestate.IDLE; currentState = Superstructurestate.IDLE; diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java index 02d839d..3723747 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java @@ -10,17 +10,12 @@ class SuperstructureIOInputs { public double stateStartTime = 0.0; // Intake Status - public boolean intakeHasGamePiece = false; public boolean intakeDeployed = false; public double intakeArmPosition = 0.0; // Shooter status stuff, may need to be changed later when actually implemented public boolean shooterAtSpeed = false; public double shooterVelocity = 0.0; - - // Ready status - public boolean readyToIntake = true; - public boolean readyToShoot = false; } default void updateInputs(SuperstructureIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java index 6fc50e5..2341377 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java @@ -7,7 +7,6 @@ public enum Superstructurestate { STOWED, DEPLOYED, SHOOTING, - HOLDING_PIECE, TRANSITIONING, PREPARING_TO_SHOOT; @@ -15,10 +14,6 @@ public boolean isIntakeDeployed() { return this == DEPLOYED || this == STOWED; } - public boolean hasGamePiece() { - return this == HOLDING_PIECE || this == PREPARING_TO_SHOOT || this == SHOOTING; - } - public boolean isAquiringPiece() { return this == INTAKING; } From 6745733747a7e1a45a075b44afd1f513f4a8031b Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Wed, 26 Nov 2025 16:26:28 -0800 Subject: [PATCH 30/51] Removed IO layers of superstructure --- src/main/java/frc/robot/RobotContainer.java | 15 ++++--------- .../superstructure/SuperstructureIO.java | 22 ------------------- .../superstructure/SuperstructureIOReal.java | 8 ------- 3 files changed, 4 insertions(+), 41 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java delete mode 100644 src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0bdd5e3..13c770b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,7 +36,6 @@ import frc.robot.subsystems.drive.*; import frc.robot.subsystems.intake.*; import frc.robot.subsystems.superstructure.Superstructure; -import frc.robot.subsystems.superstructure.SuperstructureIOReal; import frc.robot.subsystems.vision.*; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; @@ -82,7 +81,7 @@ public RobotContainer() { new VisionIOLimelight(VisionConstants.camera0Name, drive::getRotation), new VisionIOLimelight(VisionConstants.camera1Name, drive::getRotation)); intake = new Intake(new IntakeIOTalonFXReal()); - superstructure = new Superstructure(intake, new SuperstructureIOReal()); + superstructure = new Superstructure(intake); break; case SIM: // Sim robot, instantiate physics sim IO implementations @@ -108,7 +107,7 @@ public RobotContainer() { camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); intake = new Intake(new IntakeIOTalonFXSim()); setupRobotMechanism(); - superstructure = new Superstructure(intake, new SuperstructureIOReal()); + superstructure = new Superstructure(intake); break; default: @@ -122,7 +121,7 @@ public RobotContainer() { (pose) -> {}); vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); intake = new Intake(new IntakeIO() {}); - superstructure = new Superstructure(intake, new SuperstructureIOReal()); + superstructure = new Superstructure(intake); break; } @@ -183,14 +182,8 @@ private void configureButtonBindings() { // Intake controller.leftBumper().whileTrue(SuperstructureCommands.intake(superstructure)); // left bumper - // Eject for 0.5 seconds + // Eject controller.rightBumper().whileTrue(SuperstructureCommands.outtake(superstructure)); // right bumper - - // Stow (AKA Return to Idle) - controller.a().onTrue(SuperstructureCommands.returnToIdle(superstructure)); // a - - // Emergency stop - controller.y().onTrue(SuperstructureCommands.emergencyStop(superstructure)); } /** diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java deleted file mode 100644 index 3723747..0000000 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.subsystems.superstructure; - -import org.littletonrobotics.junction.AutoLog; - -public interface SuperstructureIO { - @AutoLog - class SuperstructureIOInputs { - public Superstructurestate currentState = Superstructurestate.IDLE; - public Superstructurestate desiredState = Superstructurestate.IDLE; - public double stateStartTime = 0.0; - - // Intake Status - public boolean intakeDeployed = false; - public double intakeArmPosition = 0.0; - - // Shooter status stuff, may need to be changed later when actually implemented - public boolean shooterAtSpeed = false; - public double shooterVelocity = 0.0; - } - - default void updateInputs(SuperstructureIOInputs inputs) {} -} diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java deleted file mode 100644 index 5d1f103..0000000 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java +++ /dev/null @@ -1,8 +0,0 @@ -package frc.robot.subsystems.superstructure; - -public class SuperstructureIOReal implements SuperstructureIO { - @Override - public void updateInputs(SuperstructureIOInputs inputs) { - // No hardware to update - } -} From f51be2919d5b99127bb8d4b7b44200da315f02d1 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Wed, 26 Nov 2025 16:27:27 -0800 Subject: [PATCH 31/51] Switched to Goals instead of enums, simplified statemachine and removed most states, and combined the state and superstructure files --- .../superstructure/Superstructure.java | 192 ++---------------- .../superstructure/Superstructurestate.java | 20 -- 2 files changed, 18 insertions(+), 194 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index c5170fb..8195a68 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -6,207 +6,51 @@ import org.littletonrobotics.junction.Logger; public class Superstructure extends SubsystemBase { - // Time constants in seconds - private static final double StateTransitionTimeout = 2.0; - // Position check tolerance in radians - private static final double ArmPositionTolerance = 0.1; + public enum Goal{ + IDLE, + INTAKING, + OUTTAKING; + } private final Intake intake; - private final SuperstructureIO io; private final SuperstructureIOInputsAutoLogged inputs = new SuperstructureIOInputsAutoLogged(); - private Superstructurestate currentState = Superstructurestate.IDLE; - private Superstructurestate desiredState = Superstructurestate.IDLE; - private double stateStartTime = 0.0; + private Goal currentGoal = Goal.IDLE; + private Goal desiredGoal = Goal.IDLE; - public Superstructure(Intake intake, SuperstructureIO io) { + public Superstructure(Intake intake) { this.intake = intake; - this.io = io; } @Override public void periodic() { // Update inputs from subsystems - io.updateInputs(inputs); Logger.processInputs("Superstructure", inputs); - updateStateInfo(); - runStateMachine(); - // Log current states - Logger.recordOutput("Superstructure/CurrentState", currentState.toString()); - Logger.recordOutput("Superstructure/DesiredState", desiredState.toString()); - Logger.recordOutput("Superstructure/StateTime", Timer.getFPGATimestamp() - stateStartTime); - } - - public Intake getIntake() { - return this.intake; - } - - // Update current state info - private void updateStateInfo() { - inputs.currentState = currentState; - inputs.desiredState = desiredState; - inputs.stateStartTime = stateStartTime; - - // Intake status - inputs.intakeDeployed = intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, ArmPositionTolerance); - inputs.intakeArmPosition = intake.getArmPositionRad(); - } + Logger.recordOutput("Superstructure/CurrentState", currentGoal.toString()); + Logger.recordOutput("Superstructure/DesiredState", desiredGoal.toString()); - // Run state machine logic - private void runStateMachine() { - if (currentState == Superstructurestate.TRANSITIONING && isStateTimedOut()) { - Logger.recordOutput("Superstructure/Warning", "State Timeout - Forcing Idle"); - transitionToIdle(); - } + currentGoal = desiredGoal; - switch (currentState) { + switch (currentGoal) { case IDLE: - handleIdleState(); + intake.stowArm(); + intake.setRollerVoltage(0.0); break; - case INTAKING: - handleIntakingState(); - break; - case STOWED: - handleStowedState(); - break; - case OUTTAKING: - handleOuttakingState(); - break; - case DEPLOYED: - handleDeployedState(); - break; - case TRANSITIONING: - handleTransitioningState(); - break; - case PREPARING_TO_SHOOT: - case SHOOTING: - transitionToIdle(); - break; - } - } - - private void handleIdleState() { - switch (desiredState) { case INTAKING: intake.deployArm(); - changeState(Superstructurestate.TRANSITIONING); + intake.setRollerVoltage(6); break; case OUTTAKING: intake.deployArm(); - changeState(Superstructurestate.TRANSITIONING); - break; - default: - if (intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, ArmPositionTolerance)) { - intake.stopArm(); // Stop arm if at stowed position - } + intake.setRollerVoltage(-6); break; } } - private void handleIntakingState() { - intake.setRollerVoltage(0.8); - intake.setArmPosition(Intake.ARM_DEPLOYED_POSITION); - if(desiredState != Superstructurestate.INTAKING) { - intake.stop(); - intake.stowArm(); - changeState(Superstructurestate.TRANSITIONING); - } - } - - private void handleTransitioningState() { - switch (desiredState) { - case INTAKING: - intake.setRollerVoltage(0.5); - changeState(Superstructurestate.INTAKING); - break; - - case OUTTAKING: - intake.setRollerVoltage(-0.5); - changeState(Superstructurestate.OUTTAKING); - break; - - case IDLE: - intake.stop(); - changeState(Superstructurestate.IDLE); - break; - - default: - // Go to idle if unsure - transitionToIdle(); - break; - } - } - - private void handleOuttakingState() { - intake.setRollerVoltage(-0.5); - - if (desiredState != Superstructurestate.OUTTAKING) { - intake.stop(); - intake.stowArm(); - changeState(Superstructurestate.TRANSITIONING); - } - } - - private void handleStowedState() { - transitionToIdle(); - } - - private void handleDeployedState() { - transitionToIdle(); - } - - private void changeState(Superstructurestate newState) { - currentState = newState; - stateStartTime = Timer.getFPGATimestamp(); - Logger.recordOutput("Superstructure/StateChange", "New State: " + newState.toString()); - } - - private void transitionToIdle() { - desiredState = Superstructurestate.IDLE; - intake.stowArm(); - intake.stop(); - changeState(Superstructurestate.TRANSITIONING); - } - - private boolean isStateTimedOut() { - double stateTime = Timer.getFPGATimestamp() - stateStartTime; - return stateTime > StateTransitionTimeout; - } - - public void requestIntake() { - desiredState = Superstructurestate.INTAKING; - } - - public void requestOuttake() { - desiredState = Superstructurestate.OUTTAKING; - } - - public void requestIdle() { - desiredState = Superstructurestate.IDLE; - } - - public void requestShoot() { - if (currentState == Superstructurestate.PREPARING_TO_SHOOT) desiredState = Superstructurestate.SHOOTING; - } - - public Superstructurestate getCurrentState() { - return currentState; - } - - public Superstructurestate getDesiredState() { - return desiredState; - } - - public boolean isReady() { - return currentState == desiredState; - } - - public void emergencyStop() { - desiredState = Superstructurestate.IDLE; - currentState = Superstructurestate.IDLE; - intake.stop(); + public Intake getIntake() { + return this.intake; } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java deleted file mode 100644 index 2341377..0000000 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructurestate.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.subsystems.superstructure; - -public enum Superstructurestate { - IDLE, - INTAKING, - OUTTAKING, - STOWED, - DEPLOYED, - SHOOTING, - TRANSITIONING, - PREPARING_TO_SHOOT; - - public boolean isIntakeDeployed() { - return this == DEPLOYED || this == STOWED; - } - - public boolean isAquiringPiece() { - return this == INTAKING; - } -} From 716e180a69094ff82b4efb523e897de06e2b280b Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Wed, 26 Nov 2025 16:28:12 -0800 Subject: [PATCH 32/51] Rephrased commmands to work with new superstructure code --- .../commands/SuperstructureCommands.java | 42 ++++++++----------- 1 file changed, 17 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index 436cf22..2e44bfe 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -3,41 +3,33 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.superstructure.Superstructure; +import frc.robot.subsystems.superstructure.Superstructure.Goal; public class SuperstructureCommands { - // Command to return to idle state - public static Command returnToIdle(Superstructure superstructure) { - return Commands.runOnce(() -> superstructure.requestIdle(), superstructure) - .withName("ReturnToIdle"); - } - + // Standard goal command - returns to STOW when released public static Command intake(Superstructure superstructure) { return Commands.startEnd( - () -> superstructure.requestIntake(), - () -> superstructure.requestIdle(), - superstructure) - .withName("intake") - .alongWith(Commands.run(() -> {}, superstructure.getIntake())); + () -> superstructure.setGoal(Goal.INTAKING), + () -> superstructure.setGoal(Goal.IDLE), + superstructure) + .withName("Intake"); } - // Command to manually outtake public static Command outtake(Superstructure superstructure) { return Commands.startEnd( - () -> superstructure.requestOuttake(), - () -> superstructure.requestIdle(), - superstructure) - .withName("outtake"); - } - - // Command to wait until superstructure is ready - public static Command waitForReady(Superstructure superstructure) { - return Commands.waitUntil(() -> superstructure.isReady()).withName("WaitForReady"); + () -> superstructure.setGoal(Goal.OUTTAKING), + () -> superstructure.setGoal(Goal.IDLE), + superstructure) + .withName("Outtake"); } - // Command to emergency stop the superstructure - public static Command emergencyStop(Superstructure superstructure) { - return Commands.runOnce(() -> superstructure.emergencyStop(), superstructure) - .withName("EmergencyStop"); + // Wait until goal is reached (for autonomous sequencing) + public static Command intakeUntilReady(Superstructure superstructure) { + return Commands.sequence( + Commands.runOnce(() -> superstructure.setGoal(Goal.INTAKING)), + Commands.waitUntil(Superstructure::atGoal) + ).finallyDo(() -> superstructure.setGoal(Goal.IDLE)) + .withName("IntakeUntilReady"); } } From 9984f7dd4192b350d8d99b8881b37d4f6edb39fe Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Wed, 26 Nov 2025 16:36:33 -0800 Subject: [PATCH 33/51] Added functions for goal setting, and goal checking --- .../superstructure/Superstructure.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index 8195a68..25eec7e 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -5,6 +5,8 @@ import frc.robot.subsystems.intake.Intake; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.Idle; + public class Superstructure extends SubsystemBase { public enum Goal{ @@ -53,4 +55,19 @@ public void periodic() { public Intake getIntake() { return this.intake; } + + public void setGoal (Goal goal){ + desiredGoal = goal; + } + + public boolean atGoal(){ + if (currentGoal != desiredGoal){ + return false; + } + + return switch (currentGoal){ + case IDLE -> intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, 0.1); + case INTAKING, OUTTAKING -> intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, 0.1); + }; + } } From da75931eca87b2419f18716476189c459cb29878 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Wed, 26 Nov 2025 16:41:19 -0800 Subject: [PATCH 34/51] Deactivated auto command for now --- .../commands/SuperstructureCommands.java | 19 ++++++++++--------- .../superstructure/Superstructure.java | 2 +- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index 2e44bfe..54608b0 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -7,7 +7,7 @@ public class SuperstructureCommands { - // Standard goal command - returns to STOW when released + //Intake command that intakes while held, and then returns to idle when released public static Command intake(Superstructure superstructure) { return Commands.startEnd( () -> superstructure.setGoal(Goal.INTAKING), @@ -16,6 +16,7 @@ public static Command intake(Superstructure superstructure) { .withName("Intake"); } + //Outtake command that intakes while held, and then returns to idle when released public static Command outtake(Superstructure superstructure) { return Commands.startEnd( () -> superstructure.setGoal(Goal.OUTTAKING), @@ -24,12 +25,12 @@ public static Command outtake(Superstructure superstructure) { .withName("Outtake"); } - // Wait until goal is reached (for autonomous sequencing) - public static Command intakeUntilReady(Superstructure superstructure) { - return Commands.sequence( - Commands.runOnce(() -> superstructure.setGoal(Goal.INTAKING)), - Commands.waitUntil(Superstructure::atGoal) - ).finallyDo(() -> superstructure.setGoal(Goal.IDLE)) - .withName("IntakeUntilReady"); - } + //Intake command for autos, waits until the intake has reached its goal + // public static Command intakeUntilReady(Superstructure superstructure) { + // return Commands.sequence( + // Commands.runOnce(() -> superstructure.setGoal(Goal.INTAKING)), + // Commands.waitUntil(superstructure::isAtGoal) + // ).finallyDo(() -> superstructure.setGoal(Goal.IDLE)) + // .withName("IntakeUntilReady"); + // } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index 25eec7e..e69e707 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -59,7 +59,7 @@ public Intake getIntake() { public void setGoal (Goal goal){ desiredGoal = goal; } - + public boolean atGoal(){ if (currentGoal != desiredGoal){ return false; From dbad3419ce13ba2d4b76e6f62df86e8d5f0df9fc Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Wed, 26 Nov 2025 16:43:18 -0800 Subject: [PATCH 35/51] Reenabled auto command --- .../frc/robot/commands/SuperstructureCommands.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index 54608b0..56262c2 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -26,11 +26,11 @@ public static Command outtake(Superstructure superstructure) { } //Intake command for autos, waits until the intake has reached its goal - // public static Command intakeUntilReady(Superstructure superstructure) { - // return Commands.sequence( - // Commands.runOnce(() -> superstructure.setGoal(Goal.INTAKING)), - // Commands.waitUntil(superstructure::isAtGoal) - // ).finallyDo(() -> superstructure.setGoal(Goal.IDLE)) - // .withName("IntakeUntilReady"); - // } + public static Command intakeUntilReady(Superstructure superstructure) { + return Commands.sequence( + Commands.runOnce(() -> superstructure.setGoal(Goal.INTAKING)), + Commands.waitUntil(superstructure::atGoal) + ).finallyDo(() -> superstructure.setGoal(Goal.IDLE)) + .withName("IntakeUntilReady"); + } } From 29801c457df52b4bcf0be602f5be5b3d2d144149 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Wed, 26 Nov 2025 17:36:39 -0800 Subject: [PATCH 36/51] Added IOs back for logging purposes --- src/main/java/frc/robot/RobotContainer.java | 12 +++--- .../commands/SuperstructureCommands.java | 30 +++++++-------- .../subsystems/intake/IntakeIOTalonFXSim.java | 1 - .../superstructure/Superstructure.java | 38 ++++++++++++------- .../superstructure/SuperstructureIO.java | 14 +++++++ .../superstructure/SuperstructureIOReal.java | 6 +++ 6 files changed, 66 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13c770b..1fb4c15 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,8 @@ import frc.robot.subsystems.drive.*; import frc.robot.subsystems.intake.*; import frc.robot.subsystems.superstructure.Superstructure; +import frc.robot.subsystems.superstructure.SuperstructureIO; +import frc.robot.subsystems.superstructure.SuperstructureIOReal; import frc.robot.subsystems.vision.*; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; @@ -81,7 +83,7 @@ public RobotContainer() { new VisionIOLimelight(VisionConstants.camera0Name, drive::getRotation), new VisionIOLimelight(VisionConstants.camera1Name, drive::getRotation)); intake = new Intake(new IntakeIOTalonFXReal()); - superstructure = new Superstructure(intake); + superstructure = new Superstructure(intake, new SuperstructureIOReal() {}); break; case SIM: // Sim robot, instantiate physics sim IO implementations @@ -107,7 +109,7 @@ public RobotContainer() { camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); intake = new Intake(new IntakeIOTalonFXSim()); setupRobotMechanism(); - superstructure = new Superstructure(intake); + superstructure = new Superstructure(intake, new SuperstructureIOReal() {}); break; default: @@ -121,7 +123,7 @@ public RobotContainer() { (pose) -> {}); vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); intake = new Intake(new IntakeIO() {}); - superstructure = new Superstructure(intake); + superstructure = new Superstructure(intake, new SuperstructureIO() {}); break; } @@ -180,10 +182,10 @@ private void configureButtonBindings() { // ============DRIVER CONTROLLER BINDINGS (SYSTEM)============// // Intake - controller.leftBumper().whileTrue(SuperstructureCommands.intake(superstructure)); // left bumper + controller.povUp().whileTrue(SuperstructureCommands.intake(superstructure)); // left bumper // Eject - controller.rightBumper().whileTrue(SuperstructureCommands.outtake(superstructure)); // right bumper + controller.povDown().whileTrue(SuperstructureCommands.outtake(superstructure)); // right bumper } /** diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index 56262c2..caffe7c 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -7,30 +7,30 @@ public class SuperstructureCommands { - //Intake command that intakes while held, and then returns to idle when released + // Intake command that intakes while held, and then returns to idle when released public static Command intake(Superstructure superstructure) { return Commands.startEnd( - () -> superstructure.setGoal(Goal.INTAKING), - () -> superstructure.setGoal(Goal.IDLE), - superstructure) - .withName("Intake"); + () -> superstructure.setGoal(Goal.INTAKING), + () -> superstructure.setGoal(Goal.IDLE), + superstructure) + .withName("Intake"); } - //Outtake command that intakes while held, and then returns to idle when released + // Outtake command that intakes while held, and then returns to idle when released public static Command outtake(Superstructure superstructure) { return Commands.startEnd( - () -> superstructure.setGoal(Goal.OUTTAKING), - () -> superstructure.setGoal(Goal.IDLE), - superstructure) - .withName("Outtake"); + () -> superstructure.setGoal(Goal.OUTTAKING), + () -> superstructure.setGoal(Goal.IDLE), + superstructure) + .withName("Outtake"); } - //Intake command for autos, waits until the intake has reached its goal + // Intake command for autos, waits until the intake has reached its goal public static Command intakeUntilReady(Superstructure superstructure) { return Commands.sequence( - Commands.runOnce(() -> superstructure.setGoal(Goal.INTAKING)), - Commands.waitUntil(superstructure::atGoal) - ).finallyDo(() -> superstructure.setGoal(Goal.IDLE)) - .withName("IntakeUntilReady"); + Commands.runOnce(() -> superstructure.setGoal(Goal.INTAKING)), + Commands.waitUntil(superstructure::atGoal)) + .finallyDo(() -> superstructure.setGoal(Goal.IDLE)) + .withName("IntakeUntilReady"); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index eb49b04..222dd5d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -75,7 +75,6 @@ public void updateInputs(IntakeIOInputs inputs) { if (simulatedGamePiece && rollerAppliedVolts < -6.0) { simulatedGamePiece = false; } - } @Override diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index e69e707..ccb26fc 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -1,38 +1,42 @@ package frc.robot.subsystems.superstructure; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.intake.Intake; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.Idle; - public class Superstructure extends SubsystemBase { - public enum Goal{ + public enum Goal { IDLE, INTAKING, OUTTAKING; } - private final Intake intake; private final SuperstructureIOInputsAutoLogged inputs = new SuperstructureIOInputsAutoLogged(); + private final Intake intake; + private final SuperstructureIO io; + private Goal currentGoal = Goal.IDLE; private Goal desiredGoal = Goal.IDLE; - public Superstructure(Intake intake) { + public Superstructure(Intake intake, SuperstructureIO io) { this.intake = intake; + this.io = io; } @Override public void periodic() { - // Update inputs from subsystems + // Process Inputs + io.updateInputs(inputs); Logger.processInputs("Superstructure", inputs); + updateGoalInfo(); + // Log current states - Logger.recordOutput("Superstructure/CurrentState", currentGoal.toString()); - Logger.recordOutput("Superstructure/DesiredState", desiredGoal.toString()); + Logger.recordOutput("Superstructure/CurrentGoal", currentGoal.toString()); + Logger.recordOutput("Superstructure/DesiredGoal", desiredGoal.toString()); + Logger.recordOutput("Superstructure/AtGoal", atGoal()); currentGoal = desiredGoal; @@ -52,20 +56,26 @@ public void periodic() { } } + private void updateGoalInfo() { + inputs.currentGoal = currentGoal; + inputs.desiredGoal = desiredGoal; + inputs.atGoal = atGoal(); + } + public Intake getIntake() { return this.intake; } - public void setGoal (Goal goal){ + public void setGoal(Goal goal) { desiredGoal = goal; } - - public boolean atGoal(){ - if (currentGoal != desiredGoal){ + + public boolean atGoal() { + if (currentGoal != desiredGoal) { return false; } - return switch (currentGoal){ + return switch (currentGoal) { case IDLE -> intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, 0.1); case INTAKING, OUTTAKING -> intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, 0.1); }; diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java new file mode 100644 index 0000000..6e8f6a5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.superstructure; + +import org.littletonrobotics.junction.AutoLog; + +public interface SuperstructureIO { + @AutoLog + class SuperstructureIOInputs { + public Superstructure.Goal currentGoal = Superstructure.Goal.IDLE; + public Superstructure.Goal desiredGoal = Superstructure.Goal.IDLE; + public boolean atGoal = true; + } + + default void updateInputs(SuperstructureIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java new file mode 100644 index 0000000..af390fa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java @@ -0,0 +1,6 @@ +package frc.robot.subsystems.superstructure; + +public class SuperstructureIOReal implements SuperstructureIO { + @Override + public void updateInputs(SuperstructureIOInputs inputs) {} +} From d401190b488dfd165a71f15a9d9226a7a07fe041 Mon Sep 17 00:00:00 2001 From: Dominic Nguyen Date: Wed, 26 Nov 2025 17:43:55 -0800 Subject: [PATCH 37/51] Added Indexer logging --- .../frc/robot/subsystems/intake/IntakeIO.java | 6 ++++++ .../subsystems/intake/IntakeIOTalonFXReal.java | 14 +++++++++----- .../subsystems/intake/IntakeIOTalonFXSim.java | 18 ++++++++++++++++++ 3 files changed, 33 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 89c77a7..29ac7f2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -16,6 +16,12 @@ class IntakeIOInputs { public double armVoltage = 0.0; public double armPosition = 0.0; public double armVelocity = 0.0; + + // Indexer Motor + public double indexerCurrent = 0.0; + public double indexerVoltage = 0.0; + public double indexerPosition = 0.0; + public double indexerVelocity = 0.0; } default void updateInputs(IntakeIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 68cea3f..772f305 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -43,6 +43,7 @@ public class IntakeIOTalonFXReal implements IntakeIO { private final StatusSignal armCurrent; // Status Signals - Indexer + private final StatusSignal indexerPosition; private final StatusSignal indexerVelocity; private final StatusSignal indexerAppliedVolts; private final StatusSignal indexerCurrent; @@ -104,6 +105,7 @@ public IntakeIOTalonFXReal() { rollerCurrent = rollerMotor.getStatorCurrent(); // Create status signals - Indexer + indexerPosition = indexerMotor.getPosition(); indexerVelocity = indexerMotor.getVelocity(); indexerAppliedVolts = indexerMotor.getMotorVoltage(); indexerCurrent = indexerMotor.getStatorCurrent(); @@ -127,7 +129,8 @@ public IntakeIOTalonFXReal() { armCurrent, indexerVelocity, indexerAppliedVolts, - indexerCurrent); + indexerCurrent, + indexerPosition); rollerMotor.optimizeBusUtilization(); armMotor.optimizeBusUtilization(); indexerMotor.optimizeBusUtilization(); @@ -151,10 +154,11 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armVoltage = armAppliedVolts.getValueAsDouble(); inputs.armCurrent = armCurrent.getValueAsDouble(); - // // Indexer inputs - // inputs.indexerVelocity = Units.rotationsToRadians(indexerVelocity.getValueAsDouble()) / INDEXER_GEAR_RATIO; - // inputs.indexerVoltage = indexerAppliedVolts.getValueAsDouble(); - // inputs.indexerCurrent = indexerCurrent.getValueAsDouble(); + // Indexer inputs + inputs.indexerPosition = Units.rotationsToRadians(indexerPosition.getValueAsDouble()) / INDEXER_GEAR_RATIO; + inputs.indexerVelocity = Units.rotationsToRadians(indexerVelocity.getValueAsDouble()) / INDEXER_GEAR_RATIO; + inputs.indexerVoltage = indexerAppliedVolts.getValueAsDouble(); + inputs.indexerCurrent = indexerCurrent.getValueAsDouble(); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index 222dd5d..f7eeabe 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -8,9 +8,11 @@ public class IntakeIOTalonFXSim implements IntakeIO { // Hardware configuration constants private static final double ROLLER_GEAR_RATIO = 3.0; private static final double ARM_GEAR_RATIO = 100.0; + private static final double INDEXER_GEAR_RATIO = 3.0; // Simulation parameters private static final double ROLLER_MOI = 0.001; // kg*m^2 + private static final double INDEXER_MOI = 0.001; // kg*m^2 private static final double ARM_MOI = 0.1; // kg*m^2 // Arm position constants (radians) @@ -20,9 +22,11 @@ public class IntakeIOTalonFXSim implements IntakeIO { // Motor simulations private final DCMotorSim rollerSim; private final DCMotorSim armSim; + private final DCMotorSim indexerSim; // Applied voltages private double rollerAppliedVolts = 0.0; + private double indexerAppliedVolts = 0.0; private double armAppliedVolts = 0.0; // Simulated game piece detection @@ -34,6 +38,11 @@ public IntakeIOTalonFXSim() { rollerSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ROLLER_MOI, ROLLER_GEAR_RATIO), DCMotor.getKrakenX60(1)); + + // Kraken X60 for indexer (1 motor, geared) + indexerSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), INDEXER_MOI, INDEXER_GEAR_RATIO), + DCMotor.getKrakenX60(1)); // Kraken X60 for arm (1 motor, highly geared) armSim = new DCMotorSim( @@ -46,6 +55,7 @@ public void updateInputs(IntakeIOInputs inputs) { // Update simulations rollerSim.update(0.02); armSim.update(0.02); + indexerSim.update(0.02); // Clamp arm position to realistic bounds if (armSim.getAngularPositionRad() < ARM_MIN_ANGLE) { @@ -66,6 +76,12 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.armVoltage = armAppliedVolts; inputs.armCurrent = armSim.getCurrentDrawAmps(); + // Update indexer inputs + inputs.indexerPosition = indexerSim.getAngularPositionRad(); + inputs.indexerVelocity = indexerSim.getAngularVelocityRadPerSec(); + inputs.indexerVoltage = indexerAppliedVolts; + inputs.indexerCurrent = indexerSim.getCurrentDrawAmps(); + if (!simulatedGamePiece && rollerAppliedVolts > 6.0 && Math.abs(rollerSim.getAngularVelocityRadPerSec()) > 10.0) { @@ -81,6 +97,8 @@ public void updateInputs(IntakeIOInputs inputs) { public void setRollerVoltage(double voltage) { rollerAppliedVolts = voltage; rollerSim.setInputVoltage(voltage); + indexerAppliedVolts = voltage; + indexerSim.setInputVoltage(voltage); } @Override From 742f79016384364d4338e3b46b389c96c2281b82 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 2 Dec 2025 10:52:27 -0800 Subject: [PATCH 38/51] Created test autopath --- .../pathplanner/paths/Example Path.path | 28 ++++--------------- 1 file changed, 6 insertions(+), 22 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 303dbb2..9a7e5fb 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -8,36 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.013282910175676, - "y": 6.5274984191074985 + "x": 3.133203374840485, + "y": 6.9703859889647495 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.166769560390973, - "y": 5.0964860911203305 + "x": 4.0255714246368255, + "y": 6.940640387304872 }, "prevControl": { - "x": 4.166769560390973, - "y": 6.0964860911203305 - }, - "nextControl": { - "x": 6.166769560390973, - "y": 4.0964860911203305 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.0, - "y": 1.0 - }, - "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 2.00287051176512, + "y": 7.000131590624627 }, "nextControl": null, "isLocked": false, From 3d6188cd20fc50bc6094e4b51acfb230d3a222a5 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 2 Dec 2025 10:52:40 -0800 Subject: [PATCH 39/51] updated controls --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1fb4c15..6a9829b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -178,14 +178,14 @@ private void configureButtonBindings() { driveSimulation.getSimulatedDriveTrainPose()) // reset odometry to actual robot pose during // simulation : () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro - controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); + controller.povDown().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); // ============DRIVER CONTROLLER BINDINGS (SYSTEM)============// // Intake - controller.povUp().whileTrue(SuperstructureCommands.intake(superstructure)); // left bumper + controller.leftBumper().whileTrue(SuperstructureCommands.intake(superstructure)); // left bumper // Eject - controller.povDown().whileTrue(SuperstructureCommands.outtake(superstructure)); // right bumper + controller.rightBumper().whileTrue(SuperstructureCommands.outtake(superstructure)); // right bumper } /** From 18d22e6b30add0cf243cc5b6feb315bdfdb176c5 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 2 Dec 2025 10:53:11 -0800 Subject: [PATCH 40/51] Tuned arm deploy position --- src/main/java/frc/robot/subsystems/intake/Intake.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 6317d16..9816048 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -12,7 +12,7 @@ public class Intake extends SubsystemBase { // Arm position constants (radians) public static final double ARM_STOWED_POSITION = 0.0; - public static final double ARM_DEPLOYED_POSITION = -1.62; + public static final double ARM_DEPLOYED_POSITION = -8; // -1.62 private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); @@ -61,8 +61,8 @@ public void periodic() { } // Set roller voltage (-12 to +12 volts) - public void setRollerVoltage(double voltage) { - io.setRollerVoltage(voltage); + public void setRollerDutyCycle(double value) { + io.setRollerDutyCycle(value); } // Set arm voltage (-12 to +12 volts) for manual control From c3e85ef01d892f1d786b9f647cceac877c56b1ab Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 2 Dec 2025 10:53:41 -0800 Subject: [PATCH 41/51] Renamed setRollerVoltage functions to setDutyCycle --- src/main/java/frc/robot/subsystems/intake/IntakeIO.java | 2 +- .../frc/robot/subsystems/intake/IntakeIOTalonFXReal.java | 8 +++++--- .../frc/robot/subsystems/intake/IntakeIOTalonFXSim.java | 6 +++--- .../robot/subsystems/superstructure/Superstructure.java | 6 +++--- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 29ac7f2..1c44e74 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -26,7 +26,7 @@ class IntakeIOInputs { default void updateInputs(IntakeIOInputs inputs) {} - default void setRollerVoltage(double voltage) {} + default void setRollerDutyCycle(double value) {} default void setArmVoltage(double voltage) {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index 772f305..e51df11 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -98,6 +98,8 @@ public IntakeIOTalonFXReal() { tryUntilOk(5, () -> armMotor.getConfigurator().apply(armConfig, 0.25)); + resetArmPosition(); + // Create status signals - Roller rollerPosition = rollerMotor.getPosition(); rollerVelocity = rollerMotor.getVelocity(); @@ -162,9 +164,9 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setRollerVoltage(double voltage) { - rollerMotor.setControl(voltageRequest.withOutput(voltage)); - indexerMotor.setControl(voltageRequest.withOutput(voltage)); + public void setRollerDutyCycle(double value) { + rollerMotor.setControl(voltageRequest.withOutput(value)); + indexerMotor.setControl(voltageRequest.withOutput(value)); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java index f7eeabe..06f68cf 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java @@ -38,7 +38,7 @@ public IntakeIOTalonFXSim() { rollerSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ROLLER_MOI, ROLLER_GEAR_RATIO), DCMotor.getKrakenX60(1)); - + // Kraken X60 for indexer (1 motor, geared) indexerSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), INDEXER_MOI, INDEXER_GEAR_RATIO), @@ -94,7 +94,7 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setRollerVoltage(double voltage) { + public void setRollerDutyCycle(double voltage) { rollerAppliedVolts = voltage; rollerSim.setInputVoltage(voltage); indexerAppliedVolts = voltage; @@ -127,6 +127,6 @@ public void resetArmPosition() { @Override public void stopRoller() { - setRollerVoltage(0.0); + setRollerDutyCycle(0.0); } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index ccb26fc..15d9dca 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -43,15 +43,15 @@ public void periodic() { switch (currentGoal) { case IDLE: intake.stowArm(); - intake.setRollerVoltage(0.0); + intake.setRollerDutyCycle(0.0); break; case INTAKING: intake.deployArm(); - intake.setRollerVoltage(6); + intake.setRollerDutyCycle(0.5); break; case OUTTAKING: intake.deployArm(); - intake.setRollerVoltage(-6); + intake.setRollerDutyCycle(-0.5); break; } } From 889c76dd83edd2ebd298efbeb5a9c4e903951206 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Tue, 2 Dec 2025 12:36:00 -0800 Subject: [PATCH 42/51] Added elastic dashboard logging --- elastic-layout.json | 279 ++++++++++++++++++ src/main/java/frc/robot/Robot.java | 9 +- src/main/java/frc/robot/RobotContainer.java | 46 +-- .../java/frc/robot/util/ElasticDashboard.java | 239 +++++++++++++++ 4 files changed, 545 insertions(+), 28 deletions(-) create mode 100644 elastic-layout.json create mode 100644 src/main/java/frc/robot/util/ElasticDashboard.java diff --git a/elastic-layout.json b/elastic-layout.json new file mode 100644 index 0000000..9cb9b3a --- /dev/null +++ b/elastic-layout.json @@ -0,0 +1,279 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Main", + "grid_layout": { + "layouts": [ + { + "title": "Robot Status", + "x": 768.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Match Time", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Main/Robot Status/Match Time", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Battery Voltage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Voltage View", + "properties": { + "topic": "/Shuffleboard/Main/Robot Status/Battery Voltage", + "period": 0.06, + "data_type": "double", + "min_value": 4.0, + "max_value": 13.0, + "divisions": 5, + "inverted": false, + "orientation": "horizontal" + } + }, + { + "title": "Alliance", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Main/Robot Status/Alliance", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + }, + { + "title": "Superstructure", + "x": 1024.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Current State", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Main/Superstructure/Current State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Desired State", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Main/Superstructure/Desired State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "At Goal", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Main/Superstructure/At Goal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + }, + { + "title": "Drive Telemetry", + "x": 1280.0, + "y": 0.0, + "width": 256.0, + "height": 512.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "X Position (m)", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Main/Drive Telemetry/X Position (m)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Y Position (m)", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Main/Drive Telemetry/Y Position (m)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Rotation (deg)", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Main/Drive Telemetry/Rotation (deg)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Gyro Yaw (deg)", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Gyro", + "properties": { + "topic": "/Shuffleboard/Main/Drive Telemetry/Gyro Yaw (deg)", + "period": 0.06, + "counter_clockwise_positive": false + } + } + ] + } + ], + "containers": [ + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 768.0, + "height": 512.0, + "type": "Field", + "properties": { + "topic": "/Shuffleboard/Main/Field", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295 + } + }, + { + "title": "FMSInfo", + "x": 0.0, + "y": 512.0, + "width": 384.0, + "height": 128.0, + "type": "FMSInfo", + "properties": { + "topic": "/FMSInfo", + "period": 0.06 + } + } + ] + } + }, + { + "name": "Vision", + "grid_layout": { + "layouts": [ + { + "title": "Vision Status", + "x": 1280.0, + "y": 0.0, + "width": 256.0, + "height": 512.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [] + } + ], + "containers": [ + { + "title": "Camera 0", + "x": 0.0, + "y": 0.0, + "width": 640.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/Shuffleboard/Vision/Camera 0", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Camera 1", + "x": 640.0, + "y": 0.0, + "width": 640.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/Shuffleboard/Vision/Camera 1", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8b62b38..6613b03 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -94,6 +94,9 @@ public void robotPeriodic() { // the Command-based framework to work. CommandScheduler.getInstance().run(); + // Update Elastic Dashboard + robotContainer.updateDashboard(); + // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); } @@ -126,10 +129,6 @@ public void autonomousPeriodic() {} /** This function is called once when teleop is enabled. */ @Override public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. if (autonomousCommand != null) { autonomousCommand.cancel(); } @@ -159,4 +158,4 @@ public void simulationInit() {} public void simulationPeriodic() { robotContainer.updateSimulation(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a9829b..a1eda86 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,11 +21,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -39,6 +34,7 @@ import frc.robot.subsystems.superstructure.SuperstructureIO; import frc.robot.subsystems.superstructure.SuperstructureIOReal; import frc.robot.subsystems.vision.*; +import frc.robot.util.ElasticDashboard; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.littletonrobotics.junction.Logger; @@ -55,7 +51,9 @@ public class RobotContainer { private final Vision vision; private final Intake intake; private final Superstructure superstructure; - private final Mechanism2d robotMechanism = new Mechanism2d(4, 4); // Intake visualization + + // Elastic Dashboard + private final ElasticDashboard elasticDashboard; private SwerveDriveSimulation driveSimulation = null; @@ -108,7 +106,6 @@ public RobotContainer() { new VisionIOPhotonVisionSim( camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); intake = new Intake(new IntakeIOTalonFXSim()); - setupRobotMechanism(); superstructure = new Superstructure(intake, new SuperstructureIOReal() {}); break; @@ -127,6 +124,9 @@ public RobotContainer() { break; } + // Initialize Elastic Dashboard + elasticDashboard = new ElasticDashboard(); + // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -142,14 +142,13 @@ public RobotContainer() { // Configure the button bindings configureButtonBindings(); - } - private void setupRobotMechanism() { - // Robot base - MechanismRoot2d robotBase = robotMechanism.getRoot("RobotBase", 2, 0.2); - - // Drivetrain - robotBase.append(new MechanismLigament2d("Chassis", 0.8, 0, 10, new Color8Bit(Color.kBlue))); + // Configure camera streams (adjust URLs if needed) + if (Constants.currentMode == Constants.Mode.REAL) { + elasticDashboard.setCameraStreams( + "mjpg:http://limelight-" + camera0Name + ".local:5800", + "mjpg:http://limelight-" + camera1Name + ".local:5800"); + } } /** @@ -163,15 +162,6 @@ private void configureButtonBindings() { drive.setDefaultCommand(DriveCommands.joystickDrive( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); - // // Lock to 0° when A button is held - // controller - // .povDown() - // .whileTrue(DriveCommands.joystickDriveAtAngle( - // drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> new Rotation2d())); - - // // Switch to X pattern when X button is pressed - // controller.povUp().onTrue(Commands.runOnce(drive::stopWithX, drive)); - // Reset gyro / odometry final Runnable resetGyro = Constants.currentMode == Constants.Mode.SIM ? () -> drive.setPose( @@ -214,4 +204,14 @@ public void updateSimulation() { Logger.recordOutput( "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae")); } + + /** Updates the Elastic Dashboard. Should be called from Robot.robotPeriodic(). */ + public void updateDashboard() { + elasticDashboard.update(drive, superstructure); + } + + /** Gets the field widget from Elastic Dashboard for trajectory visualization. */ + public ElasticDashboard getElasticDashboard() { + return elasticDashboard; + } } diff --git a/src/main/java/frc/robot/util/ElasticDashboard.java b/src/main/java/frc/robot/util/ElasticDashboard.java new file mode 100644 index 0000000..6e2e5a7 --- /dev/null +++ b/src/main/java/frc/robot/util/ElasticDashboard.java @@ -0,0 +1,239 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.superstructure.Superstructure; +import java.util.Map; + +/** + * Utility class for managing Elastic Dashboard layouts and widgets. Provides a centralized interface for displaying + * robot telemetry. + */ +public class ElasticDashboard { + // Tabs + private final ShuffleboardTab mainTab; + private final ShuffleboardTab visionTab; + + // Field widget + private final Field2d field; + + // Main tab widgets + private GenericEntry currentStateEntry; + private GenericEntry desiredStateEntry; + private GenericEntry atGoalEntry; + private GenericEntry matchTimeEntry; + private GenericEntry batteryVoltageEntry; + private GenericEntry allianceEntry; + + // Drive tab widgets + private GenericEntry driveXEntry; + private GenericEntry driveYEntry; + private GenericEntry driveRotationEntry; + private GenericEntry gyroYawEntry; + + // Vision tab widgets (placeholders for camera streams) + private GenericEntry camera0StreamEntry; + private GenericEntry camera1StreamEntry; + + // Auto chooser + private SendableChooser autoChooser; + + public ElasticDashboard() { + // Create tabs + mainTab = Shuffleboard.getTab("Main"); + visionTab = Shuffleboard.getTab("Vision"); + + // Initialize field + field = new Field2d(); + + setupMainTab(); + setupVisionTab(); + } + + /** Sets up the main competition tab with essential robot information. */ + private void setupMainTab() { + // Field widget - large visualization + mainTab.add("Field", field) + .withWidget(BuiltInWidgets.kField) + .withSize(6, 4) + .withPosition(0, 0); + + // Robot Status layout + ShuffleboardLayout robotStatus = mainTab.getLayout("Robot Status", BuiltInLayouts.kList) + .withSize(2, 3) + .withPosition(6, 0); + + matchTimeEntry = robotStatus + .add("Match Time", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + batteryVoltageEntry = robotStatus + .add("Battery Voltage", 0.0) + .withWidget(BuiltInWidgets.kVoltageView) + .withProperties(Map.of("min", 0, "max", 13)) + .getEntry(); + + allianceEntry = robotStatus + .add("Alliance", "Unknown") + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + // Superstructure Status layout + ShuffleboardLayout superstructureStatus = mainTab.getLayout("Superstructure", BuiltInLayouts.kList) + .withSize(2, 3) + .withPosition(8, 0); + + currentStateEntry = superstructureStatus + .add("Current State", "IDLE") + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + desiredStateEntry = superstructureStatus + .add("Desired State", "IDLE") + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + atGoalEntry = superstructureStatus + .add("At Goal", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .withProperties(Map.of("colorWhenTrue", "green", "colorWhenFalse", "red")) + .getEntry(); + + // Drive Telemetry layout + ShuffleboardLayout driveTelemetry = mainTab.getLayout("Drive Telemetry", BuiltInLayouts.kList) + .withSize(2, 4) + .withPosition(10, 0); + + driveXEntry = driveTelemetry + .add("X Position (m)", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + driveYEntry = driveTelemetry + .add("Y Position (m)", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + driveRotationEntry = driveTelemetry + .add("Rotation (deg)", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + gyroYawEntry = driveTelemetry + .add("Gyro Yaw (deg)", 0.0) + .withWidget(BuiltInWidgets.kGyro) + .getEntry(); + + // Swerve module visualization + ShuffleboardLayout moduleLayout = mainTab.getLayout("Swerve Modules", BuiltInLayouts.kGrid) + .withSize(4, 2) + .withPosition(6, 3) + .withProperties(Map.of("Number of columns", 2, "Number of rows", 2)); + } + + /** Sets up the vision tab with camera streams. */ + private void setupVisionTab() { + // Camera 0 stream + camera0StreamEntry = visionTab + .add("Camera 0", "mjpg:http://limelight-camera0.local:5800") + .withWidget(BuiltInWidgets.kCameraStream) + .withSize(5, 4) + .withPosition(0, 0) + .getEntry(); + + // Camera 1 stream + camera1StreamEntry = visionTab + .add("Camera 1", "mjpg:http://limelight-camera1.local:5800") + .withWidget(BuiltInWidgets.kCameraStream) + .withSize(5, 4) + .withPosition(5, 0) + .getEntry(); + + // Vision status + ShuffleboardLayout visionStatus = visionTab + .getLayout("Vision Status", BuiltInLayouts.kList) + .withSize(2, 4) + .withPosition(10, 0); + } + + // Sets the auto chooser for the dashboard. Should be called from RobotContainer after AutoBuilder is configured. + public void setAutoChooser(SendableChooser chooser) { + this.autoChooser = chooser; + mainTab.add("Auto Chooser", chooser) + .withWidget(BuiltInWidgets.kComboBoxChooser) + .withSize(2, 1) + .withPosition(0, 4); + } + + // Updates all dashboard values + public void update(Drive drive, Superstructure superstructure) { + // Update match time + double matchTime = DriverStation.getMatchTime(); + matchTimeEntry.setDouble(matchTime); + + // Update battery voltage + double batteryVoltage = RobotController.getBatteryVoltage(); + batteryVoltageEntry.setDouble(batteryVoltage); + + // Update alliance + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + allianceEntry.setString(alliance.get().toString()); + } else { + allianceEntry.setString("Unknown"); + } + + // Update superstructure state + updateSuperstructureState(superstructure); + + // Update drive telemetry + updateDriveTelemetry(drive); + + // Update field + updateField(drive); + } + + /** Updates superstructure state information. */ + private void updateSuperstructureState(Superstructure superstructure) {} + + // Updates drive info + private void updateDriveTelemetry(Drive drive) { + Pose2d pose = drive.getPose(); + Rotation2d rotation = drive.getRotation(); + + // Update position + driveXEntry.setDouble(pose.getX()); + driveYEntry.setDouble(pose.getY()); + driveRotationEntry.setDouble(rotation.getDegrees()); + gyroYawEntry.setDouble(rotation.getDegrees()); + } + + // Updates the field widget with current robot pose + private void updateField(Drive drive) { + field.setRobotPose(drive.getPose()); + } + + // gets field for external use + public Field2d getField() { + return field; + } + + // Set camera stream URLS + public void setCameraStreams(String camera0Url, String camera1Url) { + camera0StreamEntry.setString(camera0Url); + camera1StreamEntry.setString(camera1Url); + } +} From 1ea4836d18ac1a16a7f8ba4928e78f3007d71ece Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Wed, 3 Dec 2025 12:36:09 -0800 Subject: [PATCH 43/51] Added Elastic compatibility and logging --- elastic-layout.json | 2 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../superstructure/Superstructure.java | 8 ++ .../java/frc/robot/util/ElasticDashboard.java | 132 +++++++++++++++++- 5 files changed, 139 insertions(+), 7 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 9cb9b3a..7da7976 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -276,4 +276,4 @@ } } ] -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6613b03..8867cbe 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -158,4 +158,4 @@ public void simulationInit() {} public void simulationPeriodic() { robotContainer.updateSimulation(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a1eda86..1ce4229 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -207,7 +207,7 @@ public void updateSimulation() { /** Updates the Elastic Dashboard. Should be called from Robot.robotPeriodic(). */ public void updateDashboard() { - elasticDashboard.update(drive, superstructure); + elasticDashboard.update(drive, superstructure, intake); } /** Gets the field widget from Elastic Dashboard for trajectory visualization. */ diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index 15d9dca..57a3f8d 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -66,6 +66,14 @@ public Intake getIntake() { return this.intake; } + public Goal getCurrentGoal() { + return currentGoal; + } + + public Goal getDesiredGoal() { + return desiredGoal; + } + public void setGoal(Goal goal) { desiredGoal = goal; } diff --git a/src/main/java/frc/robot/util/ElasticDashboard.java b/src/main/java/frc/robot/util/ElasticDashboard.java index 6e2e5a7..43e5656 100644 --- a/src/main/java/frc/robot/util/ElasticDashboard.java +++ b/src/main/java/frc/robot/util/ElasticDashboard.java @@ -14,6 +14,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.intake.IntakeIO; +import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.superstructure.Superstructure; import java.util.Map; @@ -25,6 +27,7 @@ public class ElasticDashboard { // Tabs private final ShuffleboardTab mainTab; private final ShuffleboardTab visionTab; + private final ShuffleboardTab mechanismTab; // Field widget private final Field2d field; @@ -43,6 +46,21 @@ public class ElasticDashboard { private GenericEntry driveRotationEntry; private GenericEntry gyroYawEntry; + // Mechanism tab widgets + private GenericEntry armCurrentEntry; + private GenericEntry armVoltageEntry; + private GenericEntry armPositionEntry; + private GenericEntry armVelocityEntry; + private GenericEntry rollerCurrentEntry; + private GenericEntry rollerVoltageEntry; + private GenericEntry rollerPositionEntry; + private GenericEntry rollerVelocityEntry; + private GenericEntry indexerCurrentEntry; + private GenericEntry indexerVoltageEntry; + private GenericEntry indexerPositionEntry; + private GenericEntry indexerVelocityEntry; + + // Vision tab widgets (placeholders for camera streams) private GenericEntry camera0StreamEntry; private GenericEntry camera1StreamEntry; @@ -54,12 +72,14 @@ public ElasticDashboard() { // Create tabs mainTab = Shuffleboard.getTab("Main"); visionTab = Shuffleboard.getTab("Vision"); + mechanismTab = Shuffleboard.getTab("Mechanisms"); // Initialize field field = new Field2d(); setupMainTab(); setupVisionTab(); + setupMechTab(); } /** Sets up the main competition tab with essential robot information. */ @@ -144,7 +164,7 @@ private void setupMainTab() { .withProperties(Map.of("Number of columns", 2, "Number of rows", 2)); } - /** Sets up the vision tab with camera streams. */ + // Sets up the vision tab with camera streams private void setupVisionTab() { // Camera 0 stream camera0StreamEntry = visionTab @@ -178,8 +198,82 @@ public void setAutoChooser(SendableChooser chooser) { .withPosition(0, 4); } + private void setupMechTab() { + ShuffleboardLayout IntakeArmValues = mechanismTab.getLayout("Intake Arm", BuiltInLayouts.kList) + .withSize(2, 4) + .withPosition(0, 0); + + armCurrentEntry = IntakeArmValues + .add("Arm Current", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + armVoltageEntry = IntakeArmValues + .add("Arm Voltage", 0.0) + .withWidget(BuiltInWidgets.kVoltageView) + .getEntry(); + + armPositionEntry = IntakeArmValues + .add("Arm Position", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + armVelocityEntry = IntakeArmValues + .add("Arm Velocity", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + ShuffleboardLayout IntakeRollerValues = mechanismTab.getLayout("Intake Roller", BuiltInLayouts.kList) + .withSize(2, 4) + .withPosition(2, 0); + + rollerCurrentEntry = IntakeRollerValues + .add("Roller Current", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + rollerVoltageEntry = IntakeRollerValues + .add("Roller Voltage", 0.0) + .withWidget(BuiltInWidgets.kVoltageView) + .getEntry(); + + rollerPositionEntry = IntakeRollerValues + .add("Roller Position", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + rollerVelocityEntry = IntakeRollerValues + .add("Roller Velocity", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + ShuffleboardLayout IndexerValues = mechanismTab.getLayout("Indexer", BuiltInLayouts.kList) + .withSize(2, 4) + .withPosition(4, 0); + + indexerCurrentEntry = IndexerValues + .add("Indexer Current", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + indexerVoltageEntry = IndexerValues + .add("Indexer Voltage", 0.0) + .withWidget(BuiltInWidgets.kVoltageView) + .getEntry(); + + indexerPositionEntry = IndexerValues + .add("Indexer Position", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + indexerVelocityEntry = IndexerValues + .add("Indexer Velocity", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + } + // Updates all dashboard values - public void update(Drive drive, Superstructure superstructure) { + public void update(Drive drive, Superstructure superstructure, Intake intake) { // Update match time double matchTime = DriverStation.getMatchTime(); matchTimeEntry.setDouble(matchTime); @@ -204,10 +298,40 @@ public void update(Drive drive, Superstructure superstructure) { // Update field updateField(drive); + + //Update Mechanism values + updateMechs(intake); } - /** Updates superstructure state information. */ - private void updateSuperstructureState(Superstructure superstructure) {} + // Updates superstructure state info + private void updateSuperstructureState(Superstructure superstructure) { + atGoalEntry.setBoolean(superstructure.atGoal()); + currentStateEntry.setString(superstructure.getCurrentGoal().toString()); + desiredStateEntry.setString(superstructure.getDesiredGoal().toString()); + } + + private void updateMechs(Intake intake){ + IntakeIO.IntakeIOInputs inputs = new IntakeIO.IntakeIOInputs(); + intake.updateInputs(inputs); + + // Update Intake Arm values + armCurrentEntry.setDouble(inputs.armCurrent); + armVoltageEntry.setDouble(inputs.armVoltage); + armPositionEntry.setDouble(inputs.armPosition); + armVelocityEntry.setDouble(inputs.armVelocity); + + // Update Intake Roller values + rollerCurrentEntry.setDouble(inputs.rollerCurrent); + rollerVoltageEntry.setDouble(inputs.rollerVoltage); + rollerPositionEntry.setDouble(inputs.rollerPosition); + rollerVelocityEntry.setDouble(inputs.rollerVelocity); + + // Update Indexer values + indexerCurrentEntry.setDouble(inputs.indexerCurrent); + indexerVoltageEntry.setDouble(inputs.indexerVoltage); + indexerPositionEntry.setDouble(inputs.indexerPosition); + indexerVelocityEntry.setDouble(inputs.indexerVelocity); + } // Updates drive info private void updateDriveTelemetry(Drive drive) { From 5e754e08ca3afa44a8ebe62bec9df61779d22bc1 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Wed, 3 Dec 2025 12:36:29 -0800 Subject: [PATCH 44/51] Tuned drive constants --- .SysId/sysid.json | 1 + src/main/java/frc/robot/generated/TunerConstants.java | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) create mode 100644 .SysId/sysid.json diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index b7dcd15..51be837 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -51,7 +51,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(104.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -72,7 +72,7 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.96); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(6.14); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot @@ -80,7 +80,7 @@ public class TunerConstants { private static final double kDriveGearRatio = 675 / 128; private static final double kSteerGearRatio = 18.75; - private static final Distance kWheelRadius = Inches.of(2); + private static final Distance kWheelRadius = Inches.of(1.841); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 2c1619eb17c44fcb8ac39bb13b3327d10591dd88 Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Wed, 3 Dec 2025 12:36:38 -0800 Subject: [PATCH 45/51] Changed canbus --- .../frc/robot/subsystems/intake/IntakeIOTalonFXReal.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java index e51df11..0d4b6f3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java @@ -50,9 +50,9 @@ public class IntakeIOTalonFXReal implements IntakeIO { public IntakeIOTalonFXReal() { // Initialize hardware - rollerMotor = new TalonFX(ROLLER_MOTOR_ID, "rio"); // CHANGE TO SWERVE LATER - armMotor = new TalonFX(ARM_MOTOR_ID, "rio"); // CHANGE TO SWERVE LATER - indexerMotor = new TalonFX(INDEXER_MOTOR_ID, "rio"); // CHANGE LATER TO SWERVE + rollerMotor = new TalonFX(ROLLER_MOTOR_ID, "swerve"); // CHANGE TO SWERVE LATER + armMotor = new TalonFX(ARM_MOTOR_ID, "swerve"); // CHANGE TO SWERVE LATER + indexerMotor = new TalonFX(INDEXER_MOTOR_ID, "swerve"); // CHANGE LATER TO SWERVE // Configure roller motor var rollerConfig = new TalonFXConfiguration(); From c77b7f3d9ce0f61ac7f0c19d24e6f0c7d6fcba8b Mon Sep 17 00:00:00 2001 From: 527dj <157839604+527dj@users.noreply.github.com> Date: Wed, 3 Dec 2025 13:03:37 -0800 Subject: [PATCH 46/51] Completed intake elastic logging --- src/main/java/frc/robot/RobotContainer.java | 2 - .../frc/robot/subsystems/intake/Intake.java | 46 ++++++- .../java/frc/robot/util/ElasticDashboard.java | 130 +++++++++--------- 3 files changed, 108 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1ce4229..a88e488 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -205,12 +205,10 @@ public void updateSimulation() { "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae")); } - /** Updates the Elastic Dashboard. Should be called from Robot.robotPeriodic(). */ public void updateDashboard() { elasticDashboard.update(drive, superstructure, intake); } - /** Gets the field widget from Elastic Dashboard for trajectory visualization. */ public ElasticDashboard getElasticDashboard() { return elasticDashboard; } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 9816048..968e234 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -90,31 +90,69 @@ public void resetArmPosition() { io.resetArmPosition(); } - // Get roller position in radians + // GETTERS + + // Roller telemetry public double getRollerPositionRad() { return inputs.rollerPosition; } - // Get roller velocity in rad/s public double getRollerVelocityRadPerSec() { return inputs.rollerVelocity; } - // Get arm position in radians + public double getRollerCurrent() { + return inputs.rollerCurrent; + } + + public double getRollerVoltage() { + return inputs.rollerVoltage; + } + + // Arm telemetry public double getArmPositionRad() { return inputs.armPosition; } - // Get arm velocity in rad/s public double getArmVelocityRadPerSec() { return inputs.armVelocity; } + public double getArmCurrent() { + return inputs.armCurrent; + } + + public double getArmVoltage() { + return inputs.armVoltage; + } + + // Indexer telemetry + public double getIndexerPositionRad() { + return inputs.indexerPosition; + } + + public double getIndexerVelocityRadPerSec() { + return inputs.indexerVelocity; + } + + public double getIndexerCurrent() { + return inputs.indexerCurrent; + } + + public double getIndexerVoltage() { + return inputs.indexerVoltage; + } + // Check if arm is at target position (within tolerance) public boolean isArmAtPosition(double targetRad, double toleranceRad) { return Math.abs(inputs.armPosition - targetRad) < toleranceRad; } + // Get the Mechanism2d for dashboard display + public Mechanism2d getMechanism() { + return mechanism; + } + // Stop all motors public void stop() { io.stopRoller(); diff --git a/src/main/java/frc/robot/util/ElasticDashboard.java b/src/main/java/frc/robot/util/ElasticDashboard.java index 43e5656..a13f11d 100644 --- a/src/main/java/frc/robot/util/ElasticDashboard.java +++ b/src/main/java/frc/robot/util/ElasticDashboard.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.superstructure.Superstructure; import java.util.Map; @@ -46,20 +45,19 @@ public class ElasticDashboard { private GenericEntry driveRotationEntry; private GenericEntry gyroYawEntry; - // Mechanism tab widgets - private GenericEntry armCurrentEntry; - private GenericEntry armVoltageEntry; - private GenericEntry armPositionEntry; - private GenericEntry armVelocityEntry; - private GenericEntry rollerCurrentEntry; - private GenericEntry rollerVoltageEntry; - private GenericEntry rollerPositionEntry; - private GenericEntry rollerVelocityEntry; - private GenericEntry indexerCurrentEntry; - private GenericEntry indexerVoltageEntry; - private GenericEntry indexerPositionEntry; - private GenericEntry indexerVelocityEntry; - + // Mechanism tab widgets + private GenericEntry armCurrentEntry; + private GenericEntry armVoltageEntry; + private GenericEntry armPositionEntry; + private GenericEntry armVelocityEntry; + private GenericEntry rollerCurrentEntry; + private GenericEntry rollerVoltageEntry; + private GenericEntry rollerPositionEntry; + private GenericEntry rollerVelocityEntry; + private GenericEntry indexerCurrentEntry; + private GenericEntry indexerVoltageEntry; + private GenericEntry indexerPositionEntry; + private GenericEntry indexerVelocityEntry; // Vision tab widgets (placeholders for camera streams) private GenericEntry camera0StreamEntry; @@ -87,7 +85,7 @@ private void setupMainTab() { // Field widget - large visualization mainTab.add("Field", field) .withWidget(BuiltInWidgets.kField) - .withSize(6, 4) + .withSize(6, 3) .withPosition(0, 0); // Robot Status layout @@ -157,6 +155,21 @@ private void setupMainTab() { .withWidget(BuiltInWidgets.kGyro) .getEntry(); + // Intake value summary + ShuffleboardLayout intakeSummary = mainTab.getLayout("Intake Summary", BuiltInLayouts.kList) + .withSize(2, 3) + .withPosition(6, 3); + + intakeSummary + .add("Arm Position", 0.0) + .withWidget(BuiltInWidgets.kNumberBar) + .withProperties(Map.of("min", -10, "max", 1)); + + intakeSummary + .add("Roller Speed", 0.0) + .withWidget(BuiltInWidgets.kNumberBar) + .withProperties(Map.of("min", -100, "max", 100)); + // Swerve module visualization ShuffleboardLayout moduleLayout = mainTab.getLayout("Swerve Modules", BuiltInLayouts.kGrid) .withSize(4, 2) @@ -199,75 +212,66 @@ public void setAutoChooser(SendableChooser chooser) { } private void setupMechTab() { - ShuffleboardLayout IntakeArmValues = mechanismTab.getLayout("Intake Arm", BuiltInLayouts.kList) + ShuffleboardLayout IntakeArmValues = mechanismTab + .getLayout("Intake Arm", BuiltInLayouts.kList) .withSize(2, 4) .withPosition(0, 0); - - armCurrentEntry = IntakeArmValues - .add("Arm Current", 0.0) + + armCurrentEntry = IntakeArmValues.add("Arm Current", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - - armVoltageEntry = IntakeArmValues - .add("Arm Voltage", 0.0) + + armVoltageEntry = IntakeArmValues.add("Arm Voltage", 0.0) .withWidget(BuiltInWidgets.kVoltageView) .getEntry(); - - armPositionEntry = IntakeArmValues - .add("Arm Position", 0.0) + + armPositionEntry = IntakeArmValues.add("Arm Position (Rad)", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - - armVelocityEntry = IntakeArmValues - .add("Arm Velocity", 0.0) + + armVelocityEntry = IntakeArmValues.add("Arm Velocity (Rad/s)", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - ShuffleboardLayout IntakeRollerValues = mechanismTab.getLayout("Intake Roller", BuiltInLayouts.kList) + ShuffleboardLayout IntakeRollerValues = mechanismTab + .getLayout("Intake Roller", BuiltInLayouts.kList) .withSize(2, 4) .withPosition(2, 0); - rollerCurrentEntry = IntakeRollerValues - .add("Roller Current", 0.0) + rollerCurrentEntry = IntakeRollerValues.add("Roller Current", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - rollerVoltageEntry = IntakeRollerValues - .add("Roller Voltage", 0.0) + rollerVoltageEntry = IntakeRollerValues.add("Roller Voltage", 0.0) .withWidget(BuiltInWidgets.kVoltageView) .getEntry(); - rollerPositionEntry = IntakeRollerValues - .add("Roller Position", 0.0) + rollerPositionEntry = IntakeRollerValues.add("Roller Position (Rad)", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - rollerVelocityEntry = IntakeRollerValues - .add("Roller Velocity", 0.0) + rollerVelocityEntry = IntakeRollerValues.add("Roller Velocity (Rad/s)", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - ShuffleboardLayout IndexerValues = mechanismTab.getLayout("Indexer", BuiltInLayouts.kList) + ShuffleboardLayout IndexerValues = mechanismTab + .getLayout("Indexer", BuiltInLayouts.kList) .withSize(2, 4) .withPosition(4, 0); - indexerCurrentEntry = IndexerValues - .add("Indexer Current", 0.0) + indexerCurrentEntry = IndexerValues.add("Indexer Current", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - - indexerVoltageEntry = IndexerValues - .add("Indexer Voltage", 0.0) + + indexerVoltageEntry = IndexerValues.add("Indexer Voltage", 0.0) .withWidget(BuiltInWidgets.kVoltageView) .getEntry(); - indexerPositionEntry = IndexerValues - .add("Indexer Position", 0.0) + indexerPositionEntry = IndexerValues.add("Indexer Position (Rad)", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - indexerVelocityEntry = IndexerValues - .add("Indexer Velocity", 0.0) + indexerVelocityEntry = IndexerValues.add("Indexer Velocity (Rad/s)", 0.0) .withWidget(BuiltInWidgets.kTextView) .getEntry(); } @@ -299,7 +303,7 @@ public void update(Drive drive, Superstructure superstructure, Intake intake) { // Update field updateField(drive); - //Update Mechanism values + // Update Mechanism values updateMechs(intake); } @@ -310,27 +314,25 @@ private void updateSuperstructureState(Superstructure superstructure) { desiredStateEntry.setString(superstructure.getDesiredGoal().toString()); } - private void updateMechs(Intake intake){ - IntakeIO.IntakeIOInputs inputs = new IntakeIO.IntakeIOInputs(); - intake.updateInputs(inputs); + private void updateMechs(Intake intake) { // Update Intake Arm values - armCurrentEntry.setDouble(inputs.armCurrent); - armVoltageEntry.setDouble(inputs.armVoltage); - armPositionEntry.setDouble(inputs.armPosition); - armVelocityEntry.setDouble(inputs.armVelocity); + armCurrentEntry.setDouble(intake.getArmCurrent()); + armVoltageEntry.setDouble(intake.getArmVoltage()); + armPositionEntry.setDouble(intake.getArmPositionRad()); + armVelocityEntry.setDouble(intake.getArmVelocityRadPerSec()); // Update Intake Roller values - rollerCurrentEntry.setDouble(inputs.rollerCurrent); - rollerVoltageEntry.setDouble(inputs.rollerVoltage); - rollerPositionEntry.setDouble(inputs.rollerPosition); - rollerVelocityEntry.setDouble(inputs.rollerVelocity); + rollerCurrentEntry.setDouble(intake.getRollerCurrent()); + rollerVoltageEntry.setDouble(intake.getRollerVoltage()); + rollerPositionEntry.setDouble(intake.getRollerPositionRad()); + rollerVelocityEntry.setDouble(intake.getRollerVelocityRadPerSec()); // Update Indexer values - indexerCurrentEntry.setDouble(inputs.indexerCurrent); - indexerVoltageEntry.setDouble(inputs.indexerVoltage); - indexerPositionEntry.setDouble(inputs.indexerPosition); - indexerVelocityEntry.setDouble(inputs.indexerVelocity); + indexerCurrentEntry.setDouble(intake.getIndexerCurrent()); + indexerVoltageEntry.setDouble(intake.getIndexerVoltage()); + indexerPositionEntry.setDouble(intake.getIndexerPositionRad()); + indexerVelocityEntry.setDouble(intake.getIndexerVelocityRadPerSec()); } // Updates drive info From 6a805cca420166f9915690ebf3ccf584dc0c26de Mon Sep 17 00:00:00 2001 From: Pramit Mazumder Date: Fri, 5 Dec 2025 03:43:00 -0800 Subject: [PATCH 47/51] update mk5n constants --- src/main/java/frc/robot/generated/TunerConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 51be837..3ceb45b 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -76,10 +76,10 @@ public class TunerConstants { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.125; + private static final double kCoupleRatio = 3.375; - private static final double kDriveGearRatio = 675 / 128; - private static final double kSteerGearRatio = 18.75; + private static final double kDriveGearRatio = 675.0 / 128.0; + private static final double kSteerGearRatio = 26.09; private static final Distance kWheelRadius = Inches.of(1.841); private static final boolean kInvertLeftSide = false; From d18b7cf0c4c9c4aae72998a66d5ec1fae780fbb9 Mon Sep 17 00:00:00 2001 From: Pramit Mazumder Date: Fri, 5 Dec 2025 03:50:32 -0800 Subject: [PATCH 48/51] use higher freq gyro updates --- src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index f10767f..0734f23 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -41,7 +41,7 @@ public GyroIOPigeon2() { yawVelocity.setUpdateFrequency(50.0); pigeon.optimizeBusUtilization(); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw()); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone()); } @Override From df7c4f0c60a00e6602110041027bf1482e6ecbdd Mon Sep 17 00:00:00 2001 From: Pramit Mazumder Date: Fri, 5 Dec 2025 03:50:59 -0800 Subject: [PATCH 49/51] user sensortomech ratio --- .../robot/subsystems/drive/ModuleIOTalonFX.java | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index cfa3ff8..e077742 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -70,6 +70,7 @@ protected ModuleIOTalonFX( var driveConfig = constants.DriveMotorInitialConfigs; driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.Slot0 = constants.DriveMotorGains; + driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; @@ -142,10 +143,8 @@ public void updateInputs(ModuleIOInputs inputs) { // Update drive inputs inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); - inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / constants.DriveMotorGearRatio; - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / constants.DriveMotorGearRatio; + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); @@ -177,13 +176,12 @@ public void setTurnOpenLoop(double output) { } @Override - public void setDriveVelocity(double wheelVelocityRadPerSec) { - double motorVelocityRotPerSec = - Units.radiansToRotations(wheelVelocityRadPerSec) * constants.DriveMotorGearRatio; + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); driveTalon.setControl( switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> velocityVoltageRequest.withVelocity(motorVelocityRotPerSec); - case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(motorVelocityRotPerSec); + case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); }); } From ab59b7424b917044ac84bb3f025836a6889e09ac Mon Sep 17 00:00:00 2001 From: Pramit Mazumder Date: Fri, 5 Dec 2025 04:33:36 -0800 Subject: [PATCH 50/51] coupling ratio position/vel compensation --- .../subsystems/drive/ModuleIOTalonFX.java | 7 +++++ .../subsystems/drive/ModuleIOTalonFXReal.java | 30 +++++++++++++++---- 2 files changed, 32 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index e077742..fda2c65 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -178,6 +178,13 @@ public void setTurnOpenLoop(double output) { @Override public void setDriveVelocity(double velocityRadPerSec) { double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + + // Apply coupling compensation (254 style): add back the drive velocity caused by azimuth rotation + // so the wheel achieves the actual requested velocity + double azimuthVelocityRps = turnVelocity.getValueAsDouble() / constants.SteerMotorGearRatio; + double driveBackoutRps = azimuthVelocityRps * constants.CouplingGearRatio / constants.DriveMotorGearRatio; + velocityRotPerSec += driveBackoutRps; + driveTalon.setControl( switch (constants.DriveMotorClosedLoopOutput) { case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXReal.java index 7e27a56..0da142a 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXReal.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXReal.java @@ -42,14 +42,34 @@ public ModuleIOTalonFXReal(SwerveModuleConstants constants) { public void updateInputs(ModuleIOInputs inputs) { super.updateInputs(inputs); + // Apply coupling compensation to main drive position + // When azimuth rotates, the drive encoder sees phantom movement + double couplingRatio = constants.CouplingGearRatio; + double driveGearRatio = constants.DriveMotorGearRatio; + double turnRotations = inputs.turnAbsolutePosition.getRotations(); + double couplingCorrectionRad = Units.rotationsToRadians(turnRotations * couplingRatio / driveGearRatio); + inputs.drivePositionRad -= couplingCorrectionRad; + // Update odometry inputs inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = drivePositionQueue.stream() - .mapToDouble(Units::rotationsToRadians) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new); + + // Get raw positions from queues + double[] rawDrivePositions = drivePositionQueue.stream().mapToDouble(Double::doubleValue).toArray(); + double[] rawTurnPositions = turnPositionQueue.stream().mapToDouble(Double::doubleValue).toArray(); + + // Apply coupling compensation to odometry samples + inputs.odometryDrivePositionsRad = new double[rawDrivePositions.length]; + for (int i = 0; i < rawDrivePositions.length; i++) { + double couplingCorrection = rawTurnPositions[i] * couplingRatio / driveGearRatio; + inputs.odometryDrivePositionsRad[i] = Units.rotationsToRadians(rawDrivePositions[i] - couplingCorrection); + } + + inputs.odometryTurnPositions = new Rotation2d[rawTurnPositions.length]; + for (int i = 0; i < rawTurnPositions.length; i++) { + inputs.odometryTurnPositions[i] = Rotation2d.fromRotations(rawTurnPositions[i]); + } + timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); From dc8e7bbbb6edc332903c730da6b9946321688187 Mon Sep 17 00:00:00 2001 From: Pramit Mazumder Date: Fri, 5 Dec 2025 04:41:58 -0800 Subject: [PATCH 51/51] swerve setpoint generator --- .../frc/robot/subsystems/drive/Drive.java | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 02bd1d6..eb3ff70 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -22,7 +22,10 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.util.swerve.SwerveSetpoint; +import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; @@ -76,6 +79,7 @@ public class Drive extends SubsystemBase implements Vision.VisionConsumer { private static final double ROBOT_MASS_KG = 74.088; private static final double ROBOT_MOI = 6.883; private static final double WHEEL_COF = 1.2; + private static final double MAX_STEER_VELOCITY_RAD_PER_SEC = 13.0; // ~2 rotations per second private static final RobotConfig PP_CONFIG = new RobotConfig( ROBOT_MASS_KG, ROBOT_MOI, @@ -125,6 +129,10 @@ public class Drive extends SubsystemBase implements Vision.VisionConsumer { private final Consumer resetSimulationPoseCallBack; + // Swerve setpoint generator (Team 254 style anti-slip) + private final SwerveSetpointGenerator setpointGenerator; + private SwerveSetpoint previousSetpoint; + public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -168,6 +176,10 @@ public Drive( new SysIdRoutine.Config( null, null, null, (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), new SysIdRoutine.Mechanism((voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + + // Configure swerve setpoint generator (Team 254 style) + setpointGenerator = new SwerveSetpointGenerator(PP_CONFIG, MAX_STEER_VELOCITY_RAD_PER_SEC); + previousSetpoint = new SwerveSetpoint(getChassisSpeeds(), getModuleStates(), DriveFeedforwards.zeros(4)); } @Override @@ -232,10 +244,10 @@ public void periodic() { * @param speeds Speeds in meters/sec */ public void runVelocity(ChassisSpeeds speeds) { - // Calculate module setpoints - speeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts); + // Use Team 254's setpoint generator for carefully controlled module states + // Note: Do NOT discretize speeds - the setpoint generator handles this internally + previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, speeds, 0.02); + SwerveModuleState[] setpointStates = previousSetpoint.moduleStates(); // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates);