From 38249bbbba6fbb8a67a147eb0cf618d8ab4e135c Mon Sep 17 00:00:00 2001 From: Alex Resnick Date: Thu, 3 Oct 2024 16:43:39 -0500 Subject: [PATCH] Add Triggers/Sim --- .../frc/lib/util/swerve/SwerveModule.java | 29 ++-- .../frc/lib/util/swerve/SwerveModuleIO.java | 7 +- .../frc/lib/util/swerve/SwerveModuleReal.java | 21 ++- .../frc/lib/util/swerve/SwerveModuleSim.java | 91 +++++++++++ src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/OperatorState.java | 10 ++ src/main/java/frc/robot/RobotContainer.java | 148 ++++++++++-------- src/main/java/frc/robot/autos/JustShoot1.java | 12 +- src/main/java/frc/robot/autos/P123.java | 57 ++++--- .../robot/autos/{P32.java => P32.java.txt} | 0 src/main/java/frc/robot/autos/P321.java | 58 ++++--- .../autos/{P3675.java => P3675.java.txt} | 0 .../robot/autos/{P675.java => P675.java.txt} | 0 src/main/java/frc/robot/autos/P8765.java | 64 ++++---- .../{Resnick3.java => Resnick3.java.txt} | 0 .../{Resnick4.java => Resnick4.java.txt} | 0 .../{Resnick5.java => Resnick5.java.txt} | 0 .../frc/robot/commands/CommandFactory.java | 85 ++++++---- .../customproxy/ProxyConditionalCommand.java | 59 +++++++ .../customproxy/ProxyParallelCommand.java | 79 ++++++++++ .../customproxy/ProxySequentialCommand.java | 82 ++++++++++ .../elevator_wrist/ElevatorWrist.java | 3 + .../frc/robot/subsystems/indexer/Indexer.java | 73 +++++++++ .../robot/subsystems/indexer/IndexerIO.java | 22 +++ .../IndexerIOFalcon.java} | 39 +---- .../subsystems/indexer/IndexerIOSim.java | 38 +++++ .../frc/robot/subsystems/intake/Intake.java | 88 +---------- .../frc/robot/subsystems/intake/IntakeIO.java | 12 +- .../robot/subsystems/intake/IntakeIORev.java | 53 +++++++ .../robot/subsystems/intake/IntakeIOSim.java | 39 +++++ .../frc/robot/subsystems/shooter/Shooter.java | 3 + .../robot/subsystems/shooter/ShooterSim.java | 45 ++++++ .../frc/robot/subsystems/swerve/Swerve.java | 40 ++++- .../frc/robot/subsystems/swerve/SwerveIO.java | 1 + .../robot/subsystems/swerve/SwerveReal.java | 1 + .../robot/subsystems/swerve/SwerveSim.java | 29 ++++ 36 files changed, 937 insertions(+), 352 deletions(-) create mode 100644 src/main/java/frc/lib/util/swerve/SwerveModuleSim.java rename src/main/java/frc/robot/autos/{P32.java => P32.java.txt} (100%) rename src/main/java/frc/robot/autos/{P3675.java => P3675.java.txt} (100%) rename src/main/java/frc/robot/autos/{P675.java => P675.java.txt} (100%) rename src/main/java/frc/robot/autos/{Resnick3.java => Resnick3.java.txt} (100%) rename src/main/java/frc/robot/autos/{Resnick4.java => Resnick4.java.txt} (100%) rename src/main/java/frc/robot/autos/{Resnick5.java => Resnick5.java.txt} (100%) create mode 100644 src/main/java/frc/robot/commands/customproxy/ProxyConditionalCommand.java create mode 100644 src/main/java/frc/robot/commands/customproxy/ProxyParallelCommand.java create mode 100644 src/main/java/frc/robot/commands/customproxy/ProxySequentialCommand.java create mode 100644 src/main/java/frc/robot/subsystems/indexer/Indexer.java create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerIO.java rename src/main/java/frc/robot/subsystems/{intake/IntakeIOFalcon.java => indexer/IndexerIOFalcon.java} (52%) create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIORev.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterSim.java create mode 100644 src/main/java/frc/robot/subsystems/swerve/SwerveSim.java diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 0378b215..e7cea05c 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,13 +1,11 @@ package frc.lib.util.swerve; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.math.Conversions; import frc.robot.Constants; import frc.robot.Robot; @@ -26,12 +24,6 @@ public class SwerveModule { private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward( Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); - /* drive motor control requests */ - private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0); - private final VelocityVoltage driveVelocity = new VelocityVoltage(0); - - /* angle motor control requests */ - private final PositionVoltage anglePosition = new PositionVoltage(0); /** * Swerve Module @@ -75,8 +67,12 @@ public void periodic() { */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { desiredState = SwerveModuleState.optimize(desiredState, getState().angle); - io.setAngleMotor(anglePosition.withPosition(desiredState.angle.getRotations())); + io.setAngleMotor(desiredState.angle.getRotations()); setSpeed(desiredState, isOpenLoop); + SmartDashboard.putNumber("desired state speed/" + moduleNumber, + desiredState.speedMetersPerSecond); + SmartDashboard.putNumber("desired state angle/" + moduleNumber, + desiredState.angle.getDegrees()); } /** @@ -87,16 +83,13 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) */ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { if (isOpenLoop) { - driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; - io.setDriveMotor(driveDutyCycle); - inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output; + double power = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; + io.setDriveMotorPower(power); } else { - driveVelocity.Velocity = Conversions.metersPerSecondToRotationPerSecond( + double driveRPM = Conversions.metersPerSecondToRotationPerSecond( desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); - driveVelocity.FeedForward = - driveFeedForward.calculate(desiredState.speedMetersPerSecond); - io.setDriveMotor(driveVelocity); - inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output; + double driveFF = driveFeedForward.calculate(desiredState.speedMetersPerSecond); + io.setDriveMotor(driveRPM, driveFF); } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java index cbfb1530..9f823135 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java @@ -1,7 +1,6 @@ package frc.lib.util.swerve; import org.littletonrobotics.junction.AutoLog; -import com.ctre.phoenix6.controls.ControlRequest; /** IO Class for SwerveModule */ public interface SwerveModuleIO { @@ -18,9 +17,11 @@ public static class SwerveModuleInputs { public default void updateInputs(SwerveModuleInputs inputs) {} - public default void setDriveMotor(ControlRequest request) {} + public default void setDriveMotor(double rpm, double feedforward) {} - public default void setAngleMotor(ControlRequest request) {} + public default void setDriveMotorPower(double power) {} + + public default void setAngleMotor(double angle) {} public default void setAngleSelectedSensorPosition(double angle) {} diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 34538eb1..bccbc008 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -4,7 +4,9 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,6 +29,13 @@ public class SwerveModuleReal implements SwerveModuleIO { private StatusSignal angleMotorSelectedPosition; private StatusSignal absolutePositionAngleEncoder; + /* drive motor control requests */ + private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0); + private final VelocityVoltage driveVelocity = new VelocityVoltage(0); + + /* angle motor control requests */ + private final PositionVoltage anglePosition = new PositionVoltage(0); + /** Instantiating motors and Encoders */ public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID, Rotation2d angleOffset) { @@ -117,13 +126,15 @@ private void configAngleEncoder() { } @Override - public void setAngleMotor(ControlRequest request) { - mAngleMotor.setControl(request); + public void setAngleMotor(double angle) { + mAngleMotor.setControl(anglePosition.withPosition(angle)); } @Override - public void setDriveMotor(ControlRequest request) { - mDriveMotor.setControl(request); + public void setDriveMotor(double rpm, double feedforward) { + driveVelocity.FeedForward = feedforward; + driveVelocity.Velocity = rpm; + mDriveMotor.setControl(driveVelocity); } @Override diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java new file mode 100644 index 00000000..78c94227 --- /dev/null +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java @@ -0,0 +1,91 @@ +package frc.lib.util.swerve; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; + +public class SwerveModuleSim implements SwerveModuleIO { + public int moduleNumber; + private FlywheelSim driveSim = + new FlywheelSim(DCMotor.getFalcon500(1), Constants.Swerve.driveGearRatio, 0.025); + private FlywheelSim turnSim = + new FlywheelSim(DCMotor.getFalcon500(1), Constants.Swerve.angleGearRatio, 0.004); + + private final Rotation2d turnAbsoluteInitPosition = + new Rotation2d(Math.random() * 2.0 * Math.PI); + + private double turnRelativePositionRad = 0.0; + private double turnAbsolutePositionRad = Math.random() * 2.0 * Math.PI; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + private final PIDController driveFeedback; + private final PIDController turnFeedback; + + public SwerveModuleSim(int moduleNumber) { + this.moduleNumber = moduleNumber; + System.out.println("[Init] Creating ServeModuleSim"); + driveFeedback = new PIDController(Constants.Swerve.driveKP, Constants.Swerve.driveKI, + Constants.Swerve.driveKD); + turnFeedback = new PIDController(Constants.Swerve.angleKP, Constants.Swerve.angleKI, + Constants.Swerve.angleKD); + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + } + + public void updateInputs(SwerveModuleInputs inputs) { + driveSim.update(Constants.loopPeriodSecs); + turnSim.update(Constants.loopPeriodSecs); + + double angleDiffRad = turnSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs; + turnRelativePositionRad += angleDiffRad; + turnAbsolutePositionRad += angleDiffRad; + while (turnAbsolutePositionRad < 0) { + turnAbsolutePositionRad += 2.0 * Math.PI; + } + while (turnAbsolutePositionRad > 2.0 * Math.PI) { + turnAbsolutePositionRad -= 2.0 * Math.PI; + } + + inputs.driveMotorSelectedPosition = Rotation2d + .fromRadians(inputs.driveMotorSelectedPosition + + (driveSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs)) + .getRotations(); + inputs.driveMotorSelectedSensorVelocity = + Rotation2d.fromRadians(driveSim.getAngularVelocityRadPerSec()).getRotations(); + + inputs.angleMotorSelectedPosition = + Rotation2d.fromRadians(turnAbsolutePositionRad).getRotations(); + inputs.absolutePositionAngleEncoder = 0; + } + + public void setDriveMotor(double rpm, double feedforward) { + driveFeedback.setSetpoint(rpm); + double volts = + driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec() / (2 * Math.PI)) + + feedforward; + SmartDashboard.putNumber("rpm/" + moduleNumber, rpm); + SmartDashboard.putNumber("ff/" + moduleNumber, feedforward); + SmartDashboard.putNumber("volts/" + moduleNumber, volts); + setDriveVoltage(volts); + } + + public void setAngleMotor(double angle) { + turnFeedback.setSetpoint(angle); + double volts = turnFeedback.calculate(turnAbsolutePositionRad / (2 * Math.PI)); + setTurnVoltage(volts); + } + + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(driveAppliedVolts); + } + + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + turnSim.setInputVoltage(turnAppliedVolts); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 69736909..ac3887fa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -21,6 +21,7 @@ * Constants file. */ public final class Constants { + public static final double loopPeriodSecs = 0.02; /** * Stick Deadband diff --git a/src/main/java/frc/robot/OperatorState.java b/src/main/java/frc/robot/OperatorState.java index c41118df..628ab911 100755 --- a/src/main/java/frc/robot/OperatorState.java +++ b/src/main/java/frc/robot/OperatorState.java @@ -1,5 +1,7 @@ package frc.robot; +import edu.wpi.first.wpilibj2.command.button.Trigger; + /** Singleton tracker for operator state. */ public class OperatorState { @@ -38,6 +40,14 @@ public State decrement() { private static State currentState = State.kShootWhileMove; private static boolean manualMode = false; + public static Trigger isManualMode = new Trigger(() -> manualModeEnabled()); + public static Trigger isAmpMode = new Trigger(() -> getCurrentState() == State.kAmp); + public static Trigger isSpeakerMode = new Trigger(() -> getCurrentState() == State.kSpeaker); + public static Trigger isShootWhileMoveMode = + new Trigger(() -> getCurrentState() == State.kShootWhileMove); + public static Trigger isPostMode = new Trigger(() -> getCurrentState() == State.kPost); + public static Trigger isClimbMode = new Trigger(() -> getCurrentState() == State.kClimb); + /** Get whether or not operator should be able to control wrist and elevator manually. */ public static boolean manualModeEnabled() { return manualMode; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 520ee01e..61d05015 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,7 +1,6 @@ package frc.robot; import java.util.Map; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.GenericHID; @@ -16,9 +15,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SelectCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.lib.util.FieldConstants; import frc.lib.util.photon.PhotonCameraWrapper; import frc.lib.util.photon.PhotonReal; @@ -37,11 +35,17 @@ import frc.robot.subsystems.elevator_wrist.ElevatorWrist; import frc.robot.subsystems.elevator_wrist.ElevatorWristIO; import frc.robot.subsystems.elevator_wrist.ElevatorWristReal; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.indexer.IndexerIO; +import frc.robot.subsystems.indexer.IndexerIOFalcon; +import frc.robot.subsystems.indexer.IndexerIOSim; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIO; -import frc.robot.subsystems.intake.IntakeIOFalcon; +import frc.robot.subsystems.intake.IntakeIORev; +import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIO; +import frc.robot.subsystems.shooter.ShooterSim; import frc.robot.subsystems.shooter.ShooterVortex; import frc.robot.subsystems.swerve.Swerve; import frc.robot.subsystems.swerve.SwerveIO; @@ -99,6 +103,11 @@ public class RobotContainer { public static SimpleWidget dumpNotes = RobotContainer.mainDriverTab.add("Auto - Dump Notes", false).withWidget("Toggle Switch") .withProperties(Map.of()).withPosition(7, 7).withSize(3, 1); + + private String noNote = Color.kBlack.toHexString(); + private GenericEntry haveNote = RobotContainer.mainDriverTab.add("Have Note", noNote) + .withWidget("Single Color View").withPosition(9, 4).withSize(3, 2).getEntry(); + /* Controllers */ public final CommandXboxController driver = new CommandXboxController(Constants.DRIVER_ID); private final CommandXboxController operator = new CommandXboxController(Constants.OPERATOR_ID); @@ -107,17 +116,12 @@ public class RobotContainer { private Swerve s_Swerve; private Shooter shooter; private Intake intake; + private Indexer indexer; private PhotonCameraWrapper[] cameras; private ElevatorWrist elevatorWrist; private LEDs leds = new LEDs(Constants.LEDConstants.LED_COUNT, Constants.LEDConstants.PWM_PORT); // private PhotonCamera backLeftCamera = new PhotonCamera("back-left"); - - private Trigger noteInIndexer = new Trigger(() -> this.intake.getIndexerBeamBrakeStatus()) - .debounce(0.25, Debouncer.DebounceType.kRising); - private Trigger noteInIntake = new Trigger(() -> this.intake.getintakeBeamBrakeStatus()) - .debounce(0.25, Debouncer.DebounceType.kRising); - /** */ public RobotContainer(RobotRunType runtimeType) { @@ -150,29 +154,33 @@ public RobotContainer(RobotRunType runtimeType) { switch (runtimeType) { case kReal: shooter = new Shooter(new ShooterVortex()); - intake = new Intake(new IntakeIOFalcon()); + intake = new Intake(new IntakeIORev()); + indexer = new Indexer(new IndexerIOFalcon()); s_Swerve = new Swerve(new SwerveReal(), cameras); elevatorWrist = new ElevatorWrist(new ElevatorWristReal(), operator); break; case kSimulation: s_Swerve = new Swerve(new SwerveIO() {}, cameras); - shooter = new Shooter(new ShooterIO() {}); - intake = new Intake(new IntakeIO() {}); + shooter = new Shooter(new ShooterSim()); + intake = new Intake(new IntakeIOSim()); + indexer = new Indexer(new IndexerIOSim()); elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator); break; default: s_Swerve = new Swerve(new SwerveIO() {}, cameras); shooter = new Shooter(new ShooterIO() {}); intake = new Intake(new IntakeIO() {}); + indexer = new Indexer(new IndexerIO() {}); elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator); } autoChooser.setDefaultOption("Nothing", Commands.none()); - autoChooser.addOption("P123", new P123(s_Swerve, elevatorWrist, intake, shooter)); - autoChooser.addOption("P321", new P321(s_Swerve, elevatorWrist, intake, shooter)); - autoChooser.addOption("P8765", new P8765(s_Swerve, elevatorWrist, intake, shooter)); + autoChooser.addOption("P123", new P123(s_Swerve, elevatorWrist, intake, indexer, shooter)); + autoChooser.addOption("P321", new P321(s_Swerve, elevatorWrist, intake, indexer, shooter)); + autoChooser.addOption("P8765", + new P8765(s_Swerve, elevatorWrist, intake, indexer, shooter)); autoChooser.addOption("Just Shoot 1", - new JustShoot1(s_Swerve, elevatorWrist, intake, shooter)); + new JustShoot1(s_Swerve, elevatorWrist, indexer, shooter)); // autoChooser.addOption("P32", new P32(s_Swerve, elevatorWrist, intake, shooter)); // autoChooser.addOption("P675", new P675(s_Swerve, elevatorWrist, intake, shooter)); // autoChooser.addOption("P3675", new P3675(s_Swerve, elevatorWrist, intake, shooter)); @@ -182,6 +190,44 @@ public RobotContainer(RobotRunType runtimeType) { leds.setDefaultCommand(new MovingColorLEDs(leds, Color.kRed, 4, false)); // Configure the button bindings configureButtonBindings(); + configureTiggerBindings(); + } + + /* + * Configure Trigger Bindings + */ + private void configureTiggerBindings() { + this.indexer.noteInIndexer.negate().and(this.intake.noteInIntake.negate()) + .onTrue(Commands.runOnce(() -> this.haveNote.setString(noNote)).ignoringDisable(true)); + // Flash LEDs Purple when note in indexer + this.indexer.noteInIndexer.and(this.intake.noteInIntake.negate()) + .onTrue(new FlashingLEDColor(leds, Constants.LEDConstants.INDEXER_COLOR).withTimeout(3)) + .onTrue(Commands.runOnce(() -> { + this.haveNote.setString(Constants.LEDConstants.INDEXER_COLOR.toHexString()); + }).ignoringDisable(true)); + // Flash LEDs Green when note in Intake + this.intake.noteInIntake.and(this.indexer.noteInIndexer.negate()) + .onTrue(new FlashingLEDColor(leds, Constants.LEDConstants.INTAKE_COLOR).withTimeout(3)) + .onTrue(Commands.runOnce(() -> { + this.haveNote.setString(Constants.LEDConstants.INTAKE_COLOR.toHexString()); + }).ignoringDisable(true)); + // Flash LEDs White when note in indexer AND intake at the same time + this.intake.noteInIntake.and(this.indexer.noteInIndexer) + .whileTrue(new FlashingLEDColor(leds, Constants.LEDConstants.ALERT_COLOR)) + .onTrue(Commands.runOnce(() -> { + this.haveNote.setString(Constants.LEDConstants.ALERT_COLOR.toHexString()); + }).ignoringDisable(true)); + // Automatically move elevator and wrist to home position after note is spit in AMP mode + OperatorState.isAmpMode.and(OperatorState.isManualMode.negate()) + .and(this.indexer.noteInIndexer.negate().debounce(1)) + .onTrue(elevatorWrist.homePosition()); + // Automatically move elevator and wrist to home position when intaking and don't have a + // note + this.intake.intakeActive.and(this.indexer.noteInIndexer.negate()) + .and(this.elevatorWrist.elevatorAtHome.negate()).onTrue(elevatorWrist.homePosition()); + + RobotModeTriggers.autonomous().and(indexer.noteInIndexer.negate()) + .and(shooter.isShooting.negate()).whileTrue(CommandFactory.intakeNote(intake, indexer)); } /** @@ -191,27 +237,20 @@ public RobotContainer(RobotRunType runtimeType) { * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - noteInIndexer.and(noteInIntake.negate()).onTrue( - new FlashingLEDColor(leds, Constants.LEDConstants.INDEXER_COLOR).withTimeout(3)); - noteInIntake.and(noteInIndexer.negate()) - .onTrue(new FlashingLEDColor(leds, Constants.LEDConstants.INTAKE_COLOR).withTimeout(3)); - noteInIntake.and(noteInIndexer) - .whileTrue(new FlashingLEDColor(leds, Constants.LEDConstants.ALERT_COLOR)); - - /* Driver Buttons */ driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset())); driver.start().onTrue( new InstantCommand(() -> s_Swerve.resetPvInitialization()).ignoringDisable(true)); // intake forward - driver.rightTrigger().whileTrue(CommandFactory.newIntakeCommand(intake, elevatorWrist)); + driver.rightTrigger() + .whileTrue(CommandFactory.newIntakeCommand(intake, indexer, elevatorWrist)); // intake backward - driver.leftTrigger().and(() -> elevatorWrist.getWristAngle().getDegrees() <= 24.0) - .whileTrue(intake.runIntakeMotorNonStop(-1, -.20)); + driver.leftTrigger().and(elevatorWrist.wristReverseOutakeLimit) + .whileTrue(CommandFactory.runIntakeMotorNonStop(intake, indexer, -1, -.20)); /* Operator Buttons */ // spit note currently in robot through shooter - operator.x().whileTrue(CommandFactory.spit(shooter, intake)); + operator.x().whileTrue(CommandFactory.spit(shooter, indexer)); // reset apriltag vision operator.b().onTrue(new InstantCommand(() -> s_Swerve.resetPvInitialization())); // spin up shooter @@ -226,13 +265,13 @@ private void configureButtonBindings() { // Constants.ElevatorWristConstants.SetPoints.HOME_ANGLE))); // shoot note to speaker after being intaked operator.rightTrigger().and(operator.leftTrigger().negate()) - .whileTrue(CommandFactory.shootSpeaker(shooter, intake)); - operator.rightTrigger().and(operator.leftTrigger()).whileTrue(intake.runIndexerMotor(1)); + .whileTrue(CommandFactory.shootSpeaker(shooter, indexer)); + operator.rightTrigger().and(operator.leftTrigger()).whileTrue(indexer.runIndexerMotor(1)); // set shooter to home preset position operator.y().onTrue(elevatorWrist.homePosition()); - operator.y().and(elevatorWrist.elevatorAtAmp).and(noteInIndexer) - .onTrue(intake.runIntakeMotorNonStop(0, -0.2).withTimeout(2.0) - .until(new Trigger(() -> !this.intake.getIndexerBeamBrakeStatus()).debounce(.5))); + operator.y().and(elevatorWrist.elevatorAtAmp).and(indexer.noteInIndexer) + .onTrue(CommandFactory.runIntakeMotorNonStop(intake, indexer, 0, -0.2).withTimeout(2.0) + .until(indexer.noteNotInIndexer.debounce(.5))); // increment once through states list to next state operator.povRight().onTrue(Commands.runOnce(() -> { @@ -242,43 +281,26 @@ private void configureButtonBindings() { operator.povLeft().onTrue(Commands.runOnce(() -> { OperatorState.decrement(); }).ignoringDisable(true)); - new Trigger(() -> OperatorState.getCurrentState() == OperatorState.State.kAmp) - .and(new Trigger(() -> !this.intake.getIndexerBeamBrakeStatus()).debounce(1.0)) + OperatorState.isAmpMode.and(indexer.noteNotInIndexer.debounce(1.0)) .onTrue(elevatorWrist.homePosition()); // run action based on current state as incremented through operator states list - operator.a().whileTrue(new SelectCommand(Map.of( - // - OperatorState.State.kSpeaker, - elevatorWrist.speakerPreset() - .alongWith(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, - Constants.Swerve.isOpenLoop)), - // - OperatorState.State.kAmp, - Commands.either(elevatorWrist.ampPosition(), Commands.none(), noteInIndexer) - .alongWith(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, - Constants.Swerve.isOpenLoop)), - // - OperatorState.State.kShootWhileMove, - new ShootWhileMoving(s_Swerve, driver, () -> s_Swerve.getPose(), + operator.a().and(OperatorState.isSpeakerMode).whileTrue(elevatorWrist.speakerPreset()); + operator.a().and(OperatorState.isAmpMode).and(this.indexer.noteInIndexer) + .whileTrue(elevatorWrist.ampPosition()); + operator.a().and(OperatorState.isShootWhileMoveMode).and(s_Swerve.seeAprilTag) + .whileTrue(new ShootWhileMoving(s_Swerve, driver, () -> s_Swerve.getPose(), () -> FieldConstants.allianceFlip(FieldConstants.Speaker.centerSpeakerOpening) .getTranslation()) .alongWith(elevatorWrist.followPosition( () -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, () -> elevatorWrist.getAngleFromDistance(s_Swerve.getPose()) - .plus(Rotation2d.fromDegrees(0.0)))), - // - OperatorState.State.kPost, - new TurnToAngle(s_Swerve, Rotation2d.fromDegrees(25)).alongWith(elevatorWrist + .plus(Rotation2d.fromDegrees(0.0))))); + operator.a().and(OperatorState.isPostMode) + .whileTrue(new TurnToAngle(s_Swerve, Rotation2d.fromDegrees(25)).alongWith(elevatorWrist .followPosition(() -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, - () -> Constants.ElevatorWristConstants.SetPoints.PODIUM_ANGLE)), - // - OperatorState.State.kClimb, - Commands - .sequence(elevatorWrist.ampPosition(), - Commands.runOnce(() -> OperatorState.enableManualMode())) - .alongWith(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, - Constants.Swerve.isOpenLoop))), - OperatorState::getCurrentState)); + () -> Constants.ElevatorWristConstants.SetPoints.PODIUM_ANGLE))); + operator.a().and(OperatorState.isClimbMode).whileTrue(Commands.sequence( + elevatorWrist.ampPosition(), Commands.runOnce(() -> OperatorState.enableManualMode()))); // Toggle manual mode operator.start().onTrue(Commands.runOnce(() -> { diff --git a/src/main/java/frc/robot/autos/JustShoot1.java b/src/main/java/frc/robot/autos/JustShoot1.java index 3325ae53..943eafd9 100644 --- a/src/main/java/frc/robot/autos/JustShoot1.java +++ b/src/main/java/frc/robot/autos/JustShoot1.java @@ -12,7 +12,7 @@ import frc.robot.Constants; import frc.robot.commands.CommandFactory; import frc.robot.subsystems.elevator_wrist.ElevatorWrist; -import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.swerve.Swerve; @@ -23,7 +23,7 @@ public class JustShoot1 extends SequentialCommandGroup { Swerve swerveDrive; ElevatorWrist elevatorWrist; - Intake intake; + Indexer indexer; Shooter shooter; /** @@ -31,14 +31,14 @@ public class JustShoot1 extends SequentialCommandGroup { * * @param swerveDrive Swerve Drive Subsystem * @param elevatorWrist Elevator Wrist Subsystem - * @param intake Intake Subsystem + * @param indexer INdexer Subsystem * @param shooter Shooter Subsystem */ - public JustShoot1(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, + public JustShoot1(Swerve swerveDrive, ElevatorWrist elevatorWrist, Indexer indexer, Shooter shooter) { this.swerveDrive = swerveDrive; this.elevatorWrist = elevatorWrist; - this.intake = intake; + this.indexer = indexer; this.shooter = shooter; PathPlannerPath path1 = PathPlannerPath.fromChoreoTrajectory("just shoot 1"); @@ -52,7 +52,7 @@ public JustShoot1(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake .plus(new Transform2d(0, 0, Rotation2d.fromDegrees(180)))); swerveDrive.resetOdometry(initialState); }); - SequentialCommandGroup part1 = followPath1.andThen(CommandFactory.Auto.runIndexer(intake)); + SequentialCommandGroup part1 = followPath1.andThen(CommandFactory.Auto.runIndexer(indexer)); SequentialCommandGroup followPaths = part1; diff --git a/src/main/java/frc/robot/autos/P123.java b/src/main/java/frc/robot/autos/P123.java index 4281b140..d93bede1 100644 --- a/src/main/java/frc/robot/autos/P123.java +++ b/src/main/java/frc/robot/autos/P123.java @@ -3,18 +3,17 @@ import java.util.function.BooleanSupplier; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.FieldConstants; import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.commands.CommandFactory; import frc.robot.subsystems.elevator_wrist.ElevatorWrist; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.swerve.Swerve; @@ -27,6 +26,7 @@ public class P123 extends SequentialCommandGroup { Swerve swerveDrive; ElevatorWrist elevatorWrist; Intake intake; + Indexer indexer; Shooter shooter; /** @@ -35,12 +35,15 @@ public class P123 extends SequentialCommandGroup { * @param swerveDrive Swerve Drive Subsystem * @param elevatorWrist Elevator Wrist Subsystem * @param intake Intake Subsystem + * @param indexer Indexer Subsystem * @param shooter Shooter Subsystem */ - public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shooter shooter) { + public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Indexer indexer, + Shooter shooter) { this.swerveDrive = swerveDrive; this.elevatorWrist = elevatorWrist; this.intake = intake; + this.indexer = indexer; this.shooter = shooter; // addRequirements(swerveDrive); // SmartDashboard.putBoolean("Auto Status", false); @@ -61,11 +64,8 @@ public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo Command followPath5 = AutoBuilder.followPath(path5); Command followPath6 = AutoBuilder.followPath(path6); - Trigger noteInIndexer = new Trigger(() -> this.intake.getIndexerBeamBrakeStatus()) - .debounce(0.25, Debouncer.DebounceType.kRising); - Trigger noteInIntake = new Trigger(() -> this.intake.getintakeBeamBrakeStatus()) - .debounce(0.25, Debouncer.DebounceType.kRising); - BooleanSupplier abort = () -> !noteInIndexer.getAsBoolean() && !noteInIntake.getAsBoolean(); + BooleanSupplier abort = indexer.noteInIndexer.negate().and(intake.noteInIntake.negate()); + BooleanSupplier goToCenter = () -> RobotContainer.goToCenter.getEntry().getBoolean(false); Command resetPosition = Commands.runOnce(() -> { Pose2d initialState = @@ -78,29 +78,28 @@ public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo .alongWith( elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, Rotation2d.fromDegrees(36.5)).withTimeout(1.0)) - .andThen(CommandFactory.Auto.runIndexer(intake)); - SequentialCommandGroup part2 = followPath2 - .alongWith(CommandFactory.intakeNote(intake), elevatorWrist - .goToPosition(elevatorHeight, Rotation2d.fromDegrees(38.5)).withTimeout(.5)) - .andThen(CommandFactory.Auto.runIndexer(intake)); - SequentialCommandGroup part3 = followPath3 - .alongWith(CommandFactory.intakeNote(intake), elevatorWrist - .goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.5)).withTimeout(.5)) - .andThen(CommandFactory.Auto.runIndexer(intake)); + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()); + SequentialCommandGroup part2 = followPath2.alongWith(elevatorWrist + .goToPosition(elevatorHeight, Rotation2d.fromDegrees(38.5)).withTimeout(.5)) + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()); + SequentialCommandGroup part3 = followPath3.alongWith(elevatorWrist + .goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.5)).withTimeout(.5)) + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()); SequentialCommandGroup part4 = followPath4 - .alongWith(CommandFactory.intakeNote(intake), - elevatorWrist.goToPosition(elevatorHeight, Rotation2d.fromDegrees(33.0)) - .withTimeout(.5)) - .andThen(CommandFactory.Auto.runIndexer(intake)) + .alongWith(elevatorWrist.goToPosition(elevatorHeight, Rotation2d.fromDegrees(33.0)) + .withTimeout(.5)) + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()) .andThen(elevatorWrist.homePosition().withTimeout(.5)); - Command part5 = followPath5.deadlineWith(CommandFactory.intakeNote(intake)).andThen( - Commands.either(Commands.none(), Commands.sequence(CommandFactory.intakeNote(intake), - CommandFactory.Auto.runIndexer(intake)), abort)); - Command part6 = followPath6.deadlineWith(CommandFactory.intakeNote(intake)).andThen( - Commands.either(Commands.none(), Commands.sequence(CommandFactory.intakeNote(intake), - CommandFactory.Auto.runIndexer(intake)), abort)); - Command midline = Commands.either(Commands.sequence(part5, part6), Commands.none(), - () -> RobotContainer.goToCenter.getEntry().getBoolean(false)); + Command part5 = followPath5.andThen(Commands.either(Commands.none(), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer).asProxy()), + abort)); + Command part6 = followPath6.andThen(Commands.either(Commands.none(), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer).asProxy()), + abort)); + Command midline = + Commands.either(Commands.sequence(part5, part6), Commands.none(), goToCenter); Command followPaths = Commands.sequence(part1, part2, part3, part4, midline); diff --git a/src/main/java/frc/robot/autos/P32.java b/src/main/java/frc/robot/autos/P32.java.txt similarity index 100% rename from src/main/java/frc/robot/autos/P32.java rename to src/main/java/frc/robot/autos/P32.java.txt diff --git a/src/main/java/frc/robot/autos/P321.java b/src/main/java/frc/robot/autos/P321.java index 96d72b57..e35eb966 100644 --- a/src/main/java/frc/robot/autos/P321.java +++ b/src/main/java/frc/robot/autos/P321.java @@ -3,18 +3,17 @@ import java.util.function.BooleanSupplier; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.FieldConstants; import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.commands.CommandFactory; import frc.robot.subsystems.elevator_wrist.ElevatorWrist; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.swerve.Swerve; @@ -27,6 +26,7 @@ public class P321 extends SequentialCommandGroup { Swerve swerveDrive; ElevatorWrist elevatorWrist; Intake intake; + Indexer indexer; Shooter shooter; /** @@ -35,12 +35,15 @@ public class P321 extends SequentialCommandGroup { * @param swerveDrive Swerve Drive Subsystem * @param elevatorWrist Elevator Wrist Subsystem * @param intake Intake Subsystem + * @param indexer Indexer SUbsystem * @param shooter Shooter Subsystem */ - public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shooter shooter) { + public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Indexer indexer, + Shooter shooter) { this.swerveDrive = swerveDrive; this.elevatorWrist = elevatorWrist; this.intake = intake; + this.indexer = indexer; this.shooter = shooter; PathPlannerPath path1 = PathPlannerPath.fromPathFile("1 - Resnick 2 Shoot Initial Note"); @@ -58,12 +61,8 @@ public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo Command followPath5 = AutoBuilder.followPath(path5); Command followPath6 = AutoBuilder.followPath(path6); - Trigger noteInIndexer = new Trigger(() -> this.intake.getIndexerBeamBrakeStatus()) - .debounce(0.25, Debouncer.DebounceType.kRising); - Trigger noteInIntake = new Trigger(() -> this.intake.getintakeBeamBrakeStatus()) - .debounce(0.25, Debouncer.DebounceType.kRising); - BooleanSupplier abort = () -> !noteInIndexer.getAsBoolean() && !noteInIntake.getAsBoolean(); - + BooleanSupplier abort = indexer.noteInIndexer.negate().and(intake.noteInIntake.negate()); + BooleanSupplier goToCenter = () -> RobotContainer.goToCenter.getEntry().getBoolean(false); Command resetPosition = Commands.runOnce(() -> { Pose2d initialState = @@ -76,29 +75,28 @@ public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo .alongWith( elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, Rotation2d.fromDegrees(39.0)).withTimeout(1.0)) - .andThen(CommandFactory.Auto.runIndexer(intake)); - SequentialCommandGroup part2 = followPath2 - .alongWith(CommandFactory.intakeNote(intake), elevatorWrist - .goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.0)).withTimeout(.5)) - .andThen(CommandFactory.Auto.runIndexer(intake)); - SequentialCommandGroup part3 = followPath3 - .alongWith(CommandFactory.intakeNote(intake), elevatorWrist - .goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.5)).withTimeout(.5)) - .andThen(CommandFactory.Auto.runIndexer(intake)); + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()); + SequentialCommandGroup part2 = followPath2.alongWith(elevatorWrist + .goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.0)).withTimeout(.5)) + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()); + SequentialCommandGroup part3 = followPath3.alongWith(elevatorWrist + .goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.5)).withTimeout(.5)) + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()); SequentialCommandGroup part4 = followPath4 - .alongWith(CommandFactory.intakeNote(intake), - elevatorWrist.goToPosition(elevatorHeight, Rotation2d.fromDegrees(36.0)) - .withTimeout(.5)) - .andThen(CommandFactory.Auto.runIndexer(intake)) + .alongWith(elevatorWrist.goToPosition(elevatorHeight, Rotation2d.fromDegrees(36.0)) + .withTimeout(.5)) + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()) .andThen(elevatorWrist.homePosition().withTimeout(.5)); - Command part5 = followPath5.deadlineWith(CommandFactory.intakeNote(intake)).andThen( - Commands.either(Commands.none(), Commands.sequence(CommandFactory.intakeNote(intake), - CommandFactory.Auto.runIndexer(intake)), abort)); - Command part6 = followPath6.deadlineWith(CommandFactory.intakeNote(intake)).andThen( - Commands.either(Commands.none(), Commands.sequence(CommandFactory.intakeNote(intake), - CommandFactory.Auto.runIndexer(intake)), abort)); - Command midline = Commands.either(Commands.sequence(part5, part6), Commands.none(), - () -> RobotContainer.goToCenter.getEntry().getBoolean(false)); + Command part5 = followPath5.andThen(Commands.either(Commands.none(), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer).asProxy()), + abort)); + Command part6 = followPath6.andThen(Commands.either(Commands.none(), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer).asProxy()), + abort)); + Command midline = + Commands.either(Commands.sequence(part5, part6), Commands.none(), goToCenter); Command followPaths = Commands.sequence(part1, part2, part3, part4, midline); diff --git a/src/main/java/frc/robot/autos/P3675.java b/src/main/java/frc/robot/autos/P3675.java.txt similarity index 100% rename from src/main/java/frc/robot/autos/P3675.java rename to src/main/java/frc/robot/autos/P3675.java.txt diff --git a/src/main/java/frc/robot/autos/P675.java b/src/main/java/frc/robot/autos/P675.java.txt similarity index 100% rename from src/main/java/frc/robot/autos/P675.java rename to src/main/java/frc/robot/autos/P675.java.txt diff --git a/src/main/java/frc/robot/autos/P8765.java b/src/main/java/frc/robot/autos/P8765.java index 02a0452a..690f32ad 100644 --- a/src/main/java/frc/robot/autos/P8765.java +++ b/src/main/java/frc/robot/autos/P8765.java @@ -3,19 +3,18 @@ import java.util.function.BooleanSupplier; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.FieldConstants; import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.commands.CommandFactory; import frc.robot.subsystems.elevator_wrist.ElevatorWrist; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.swerve.Swerve; @@ -28,6 +27,7 @@ public class P8765 extends SequentialCommandGroup { Swerve swerveDrive; ElevatorWrist elevatorWrist; Intake intake; + Indexer indexer; Shooter shooter; /** @@ -36,12 +36,15 @@ public class P8765 extends SequentialCommandGroup { * @param swerveDrive Swerve Drive Subsystem * @param elevatorWrist Elevator Wrist Subsystem * @param intake Intake Subsystem + * @param indexer Indexer Subsystem * @param shooter Shooter Subsystem */ - public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shooter shooter) { + public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Indexer indexer, + Shooter shooter) { this.swerveDrive = swerveDrive; this.elevatorWrist = elevatorWrist; this.intake = intake; + this.indexer = indexer; this.shooter = shooter; PathPlannerPath path0 = PathPlannerPath.fromChoreoTrajectory("P8765-initial"); @@ -65,65 +68,56 @@ public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Sho Command followPath3_dump = AutoBuilder.followPath(path3_dump); Command followPath4_dump = AutoBuilder.followPath(path4_dump); - - - Trigger noteInIndexer = new Trigger(() -> this.intake.getIndexerBeamBrakeStatus()) - .debounce(0.25, Debouncer.DebounceType.kRising); - Trigger noteInIntake = new Trigger(() -> this.intake.getintakeBeamBrakeStatus()) - .debounce(0.25, Debouncer.DebounceType.kRising); - BooleanSupplier abort = () -> !noteInIndexer.getAsBoolean() && !noteInIntake.getAsBoolean(); + BooleanSupplier abort = indexer.noteInIndexer.negate().and(intake.noteInIntake.negate()); BooleanSupplier dumpOrNot = () -> RobotContainer.dumpNotes.getEntry().getBoolean(false); double elevatorHeight = 28.0; Command part0 = followPath0 .alongWith( elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, Rotation2d.fromDegrees(31.0)).withTimeout(1.5)) - .andThen(Commands.waitSeconds(.1)).andThen(CommandFactory.Auto.runIndexer(intake)); + .andThen(Commands.waitSeconds(.1)) + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()); // .andThen(Commands.either(elevatorWrist.homePosition().withTimeout(.5), Commands.none(), // dumpOrNot)); - Command part1 = followPath1.deadlineWith(CommandFactory.intakeNote(intake), - elevatorWrist.homePosition().withTimeout(1.0)) - .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake).alongWith(elevatorWrist + Command part1 = + followPath1.deadlineWith(elevatorWrist.homePosition().withTimeout(1.0)) + .andThen(Commands.either(Commands.none(), Commands.sequence(elevatorWrist .followPosition(() -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, () -> elevatorWrist.getAngleFromDistance(swerveDrive.getPose())) - .withTimeout(1.5)), CommandFactory.Auto.runIndexer(intake)), - abort)); - Command part2 = followPath2.deadlineWith(CommandFactory.intakeNote(intake), - elevatorWrist.homePosition().withTimeout(1.0)) - .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake).alongWith(elevatorWrist + .withTimeout(1.5), CommandFactory.Auto.runIndexer(indexer).asProxy()), abort)); + Command part2 = + followPath2.deadlineWith(elevatorWrist.homePosition().withTimeout(1.0)) + .andThen(Commands.either(Commands.none(), Commands.sequence(elevatorWrist .followPosition(() -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, () -> elevatorWrist.getAngleFromDistance(swerveDrive.getPose())) - .withTimeout(1.5)), CommandFactory.Auto.runIndexer(intake)), - abort)); - Command part3 = followPath3.alongWith(CommandFactory.intakeNote(intake)) + .withTimeout(1.5), CommandFactory.Auto.runIndexer(indexer).asProxy()), abort)); + Command part3 = followPath3 .andThen( elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, Rotation2d.fromDegrees(32.5)).withTimeout(1.5)) - .andThen(CommandFactory.Auto.runIndexer(intake)) + .andThen(CommandFactory.Auto.runIndexer(indexer).asProxy()) .andThen(elevatorWrist.homePosition().withTimeout(.5)); - Command part1_dump = followPath1_dump.deadlineWith(CommandFactory.intakeNote(intake)) + Command part1_dump = followPath1_dump .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake), - CommandFactory.Auto.runIndexer(intake)), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer).asProxy()), abort)) .andThen(elevatorWrist.homePosition().withTimeout(.1)); - Command part2_dump = followPath2_dump.deadlineWith(CommandFactory.intakeNote(intake)) + Command part2_dump = followPath2_dump .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake), - CommandFactory.Auto.runIndexer(intake)), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer).asProxy()), abort)) .andThen(elevatorWrist.homePosition().withTimeout(.1)); - Command part3_dump = followPath3_dump.deadlineWith(CommandFactory.intakeNote(intake)) + Command part3_dump = followPath3_dump .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake), - CommandFactory.Auto.runIndexer(intake)), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer).asProxy()), abort)) .andThen(elevatorWrist.homePosition().withTimeout(.1)); - Command part4_dump = followPath4_dump.alongWith(CommandFactory.intakeNote(intake)); + Command part4_dump = followPath4_dump; Command wait = Commands.waitSeconds(.01); Command resetPosition = Commands.runOnce(() -> { diff --git a/src/main/java/frc/robot/autos/Resnick3.java b/src/main/java/frc/robot/autos/Resnick3.java.txt similarity index 100% rename from src/main/java/frc/robot/autos/Resnick3.java rename to src/main/java/frc/robot/autos/Resnick3.java.txt diff --git a/src/main/java/frc/robot/autos/Resnick4.java b/src/main/java/frc/robot/autos/Resnick4.java.txt similarity index 100% rename from src/main/java/frc/robot/autos/Resnick4.java rename to src/main/java/frc/robot/autos/Resnick4.java.txt diff --git a/src/main/java/frc/robot/autos/Resnick5.java b/src/main/java/frc/robot/autos/Resnick5.java.txt similarity index 100% rename from src/main/java/frc/robot/autos/Resnick5.java rename to src/main/java/frc/robot/autos/Resnick5.java.txt diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 0dc41442..1538dadc 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -1,12 +1,11 @@ package frc.robot.commands; import java.util.function.BooleanSupplier; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; import frc.robot.subsystems.elevator_wrist.ElevatorWrist; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.swerve.Swerve; @@ -23,13 +22,13 @@ public class CommandFactory { * @param elevatorWrist Elevator and Wrist subsystem * @return Returns a command */ - public static Command runIntake(Intake intake, ElevatorWrist elevatorWrist) { - BooleanSupplier sensor = () -> intake.getIndexerBeamBrakeStatus(); + public static Command runIntake(Intake intake, Indexer indexer, ElevatorWrist elevatorWrist) { + BooleanSupplier sensor = () -> indexer.getIndexerBeamBrakeStatus(); Command moveElevatorWrist = elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, Constants.ElevatorWristConstants.SetPoints.HOME_ANGLE); Command runIntakeIndexer = - intake.runIntakeMotor(0, Constants.IntakeConstants.INDEX_MOTOR_FORWARD); + runIntakeMotor(intake, indexer, 0, Constants.IntakeConstants.INDEX_MOTOR_FORWARD); return moveElevatorWrist.andThen(runIntakeIndexer).unless(sensor); } @@ -40,8 +39,8 @@ public static Command runIntake(Intake intake, ElevatorWrist elevatorWrist) { * @param intake Intake subsystem * @return Returns a command */ - public static Command shootSpeaker(Shooter shooter, Intake intake) { - Command runIndexer = intake.runIndexerMotor(1); + public static Command shootSpeaker(Shooter shooter, Indexer indexer) { + Command runIndexer = indexer.runIndexerMotor(1); Command runshooter = shooter.shootSpeaker(); Command readytoShoot = Commands.waitUntil(() -> shooter.readyToShoot()); return runshooter.alongWith(readytoShoot.withTimeout(1).andThen(runIndexer)); @@ -54,12 +53,12 @@ public static Command shootSpeaker(Shooter shooter, Intake intake) { * @param intake Intake subsystem * @return Returns a command */ - public static Command passThroughShoot(Shooter shooter, Intake intake) { + public static Command passThroughShoot(Shooter shooter, Intake intake, Indexer indexer) { Command runshooter = shooter.shootSpeaker(); Command readytoShoot = Commands.waitUntil(() -> shooter.readyToShoot()); - return runshooter - .alongWith(readytoShoot.withTimeout(1).andThen(intake.runIntakeMotorNonStop(0, 1) - .withTimeout(1).andThen(intake.runIntakeMotorNonStop(1, 1)))); + return runshooter.alongWith( + readytoShoot.withTimeout(1).andThen(runIntakeMotorNonStop(intake, indexer, 0, 1) + .withTimeout(1).andThen(runIntakeMotorNonStop(intake, indexer, 1, 1)))); } /** @@ -69,8 +68,8 @@ public static Command passThroughShoot(Shooter shooter, Intake intake) { * @param intake Intake Subsystem * @return Command */ - public static Command spit(Shooter shooter, Intake intake) { - return shooter.spit().alongWith(intake.runIndexerMotor(1.0)); + public static Command spit(Shooter shooter, Indexer indexer) { + return shooter.spit().alongWith(indexer.runIndexerMotor(1.0)); } /** @@ -79,8 +78,8 @@ public static Command spit(Shooter shooter, Intake intake) { * @param intake Intake Subsystem * @return Command */ - public static Command intakeNote(Intake intake) { - return intake.runIntakeMotor(1, .2); + public static Command intakeNote(Intake intake, Indexer indexer) { + return runIntakeMotor(intake, indexer, 1, .2); } /** @@ -103,17 +102,16 @@ public static Command autoAngleWristSpeaker(ElevatorWrist elevatorWrist, Swerve * @param elevatorWrist Elevator Wrist Subsystem * @return Command */ - public static Command newIntakeCommand(Intake intake, ElevatorWrist elevatorWrist) { - Trigger noteInIntake = new Trigger(() -> intake.getintakeBeamBrakeStatus()).debounce(0.25, - Debouncer.DebounceType.kRising); - Command regularIntake = intakeNote(intake); + public static Command newIntakeCommand(Intake intake, Indexer indexer, + ElevatorWrist elevatorWrist) { + Command regularIntake = intakeNote(intake, indexer); Command altIntake = Commands .startEnd(() -> intake.setIntakeMotor(1), () -> intake.setIntakeMotor(0), intake) - .until(noteInIntake) - .andThen(Commands.waitUntil(() -> elevatorWrist.elevatorAtHome()), intakeNote(intake)); + .until(intake.noteInIntake) + .andThen(Commands.waitUntil(elevatorWrist.elevatorAtHome), intakeNote(intake, indexer)); - return Commands.either(regularIntake, altIntake, () -> elevatorWrist.elevatorAtHome()) - .unless(() -> intake.getIndexerBeamBrakeStatus()); + return Commands.either(regularIntake, altIntake, elevatorWrist.elevatorAtHome) + .unless(indexer.noteInIndexer); } /** @@ -128,9 +126,9 @@ public class Auto { * @param intake Intake Subsystem * @return Command */ - public static Command runIndexer(Intake intake) { - return Commands.waitUntil(() -> !intake.getIndexerBeamBrakeStatus()) - .andThen(Commands.waitSeconds(.25)).deadlineWith(intake.runIndexerMotor(1)) + public static Command runIndexer(Indexer indexer) { + return Commands.waitUntil(indexer.noteInIndexer.negate()) + .andThen(Commands.waitSeconds(.25)).deadlineWith(indexer.runIndexerMotor(1)) .withTimeout(5); } @@ -141,9 +139,9 @@ public static Command runIndexer(Intake intake) { * @param intake Intake Subsystem * @return Command */ - public static Command runIndexer(Intake intake, Shooter shooter) { + public static Command runIndexer(Indexer indexer, Shooter shooter) { return Commands.waitUntil(() -> shooter.readyToShoot()).withTimeout(2) - .andThen(CommandFactory.Auto.runIndexer(intake)); + .andThen(CommandFactory.Auto.runIndexer(indexer)); } /** @@ -152,9 +150,36 @@ public static Command runIndexer(Intake intake, Shooter shooter) { * @param intake Intake Subsystem * @return Command */ - public static Command waitForIntake(Intake intake) { - return Commands.waitUntil(() -> intake.getIndexerBeamBrakeStatus()); + public static Command waitForIntake(Indexer indexer) { + return Commands.waitUntil(() -> indexer.getIndexerBeamBrakeStatus()); } } + + /** + * Command to run the intake motor and indexer until the sensor trips + * + * @return {@link Command} to run the intake and indexer motors + */ + public static Command runIntakeMotor(Intake intake, Indexer indexer, double intakeSpeed, + double indexerSpeed) { + return runIntakeMotorNonStop(intake, indexer, intakeSpeed, indexerSpeed) + .until(indexer.noteInIndexer).unless(indexer.noteInIndexer); + } + + /** + * Command to run the intake motor and indexer until the sensor trips + * + * @return {@link Command} to run the intake and indexer motors + */ + public static Command runIntakeMotorNonStop(Intake intake, Indexer indexer, double intakeSpeed, + double indexerSpeed) { + return Commands.startEnd(() -> { + intake.setIntakeMotor(intakeSpeed); + indexer.setIndexerMotor(indexerSpeed); + }, () -> { + intake.setIntakeMotor(0); + indexer.setIndexerMotor(0); + }, intake, indexer); + } } diff --git a/src/main/java/frc/robot/commands/customproxy/ProxyConditionalCommand.java b/src/main/java/frc/robot/commands/customproxy/ProxyConditionalCommand.java new file mode 100644 index 00000000..0e36114d --- /dev/null +++ b/src/main/java/frc/robot/commands/customproxy/ProxyConditionalCommand.java @@ -0,0 +1,59 @@ +package frc.robot.commands.customproxy; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; +import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class ProxyConditionalCommand extends Command { + private final Command m_onTrue; + private final Command m_onFalse; + private final BooleanSupplier m_condition; + private Command m_selectedCommand; + + /** + * Creates a new ProxyConditionalCommand. + * + * @param onTrue the command to run if the condition is true + * @param onFalse the command to run if the condition is false + * @param condition the condition to determine which command to run + */ + public ProxyConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) { + m_onTrue = requireNonNullParam(onTrue, "onTrue", "ProxyConditionalCommand"); + m_onFalse = requireNonNullParam(onFalse, "onFalse", "ProxyConditionalCommand"); + m_condition = requireNonNullParam(condition, "condition", "ProxyConditionalCommand"); + + CommandScheduler.getInstance().registerComposedCommands(onTrue, onFalse); + + } + + @Override + public void initialize() { + if (m_condition.getAsBoolean()) { + m_selectedCommand = m_onTrue; + } else { + m_selectedCommand = m_onFalse; + } + m_selectedCommand.schedule(); + } + + @Override + public final void execute() {} + + @Override + public final void end(boolean interrupted) { + if (interrupted) { + m_selectedCommand.cancel(); + } + } + + @Override + public final boolean isFinished() { + return !m_selectedCommand.isScheduled(); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/customproxy/ProxyParallelCommand.java b/src/main/java/frc/robot/commands/customproxy/ProxyParallelCommand.java new file mode 100644 index 00000000..6ebbe00b --- /dev/null +++ b/src/main/java/frc/robot/commands/customproxy/ProxyParallelCommand.java @@ -0,0 +1,79 @@ +package frc.robot.commands.customproxy; + +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Supplier; +import edu.wpi.first.wpilibj2.command.Command; + +public class ProxyParallelCommand extends Command { + private final Supplier> m_supplier; + private final Map m_commands = new HashMap<>(); + + + /** + * Creates a new ProxyParallelCommand that schedules the supplied command when initialized, and + * ends when it is no longer scheduled. Useful for lazily creating commands at runtime. + * + * Supplier> str = () -> new ArrayList<>(); + * + * @param supplier the command supplier + */ + public ProxyParallelCommand(Supplier> supplier) { + m_supplier = supplier; + } + + // /** + // * Creates a new ProxyParallelCommand that schedules the given command when initialized, and + // * ends when it is no longer scheduled. + // * + // * @param command the command to run by proxy + // */ + // @SuppressWarnings("this-escape") + // public ProxyParallelCommand(Command command) { + // this(() -> ArraysList(command)); + // setName("Proxy(" + command.getName() + ")"); + // } + + @Override + public void initialize() { + List m_command = m_supplier.get(); + for (Command command : m_command) { + m_commands.put(command, true); + command.schedule(); + } + } + + @Override + public final void execute() { + for (Map.Entry commandRunning : m_commands.entrySet()) { + if (!commandRunning.getValue()) { + continue; + } + if (!commandRunning.getKey().isScheduled()) { + commandRunning.setValue(false); + } + } + } + + @Override + public final void end(boolean interrupted) { + if (interrupted) { + for (Map.Entry commandRunning : m_commands.entrySet()) { + if (commandRunning.getValue()) { + commandRunning.getKey().cancel();; + } + } + } + } + + @Override + public final boolean isFinished() { + return !m_commands.containsValue(true); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/customproxy/ProxySequentialCommand.java b/src/main/java/frc/robot/commands/customproxy/ProxySequentialCommand.java new file mode 100644 index 00000000..83ac66d1 --- /dev/null +++ b/src/main/java/frc/robot/commands/customproxy/ProxySequentialCommand.java @@ -0,0 +1,82 @@ +package frc.robot.commands.customproxy; + +import java.util.ArrayList; +import java.util.List; +import java.util.function.Supplier; +import edu.wpi.first.wpilibj2.command.Command; + +public class ProxySequentialCommand extends Command { + private final Supplier> m_supplier; + private final List m_commands = new ArrayList<>(); + private int m_currentCommandIndex = -1; + + + /** + * Creates a new ProxySequentialCommand that schedules the supplied command when initialized, + * and ends when it is no longer scheduled. Useful for lazily creating commands at runtime. + * + * Supplier> str = () -> new ArrayList<>(); + * + * @param supplier the command supplier + */ + public ProxySequentialCommand(Supplier> supplier) { + m_supplier = supplier; + } + + // /** + // * Creates a new ProxySequentialCommand that schedules the given command when initialized, and + // * ends when it is no longer scheduled. + // * + // * @param command the command to run by proxy + // */ + // @SuppressWarnings("this-escape") + // public ProxySequentialCommand(Command command) { + // this(() -> ArraysList(command)); + // setName("Proxy(" + command.getName() + ")"); + // } + + @Override + public void initialize() { + m_currentCommandIndex = 0; + List m_command = m_supplier.get(); + for (Command command : m_command) { + m_commands.add(command); + } + if (!m_commands.isEmpty()) { + m_commands.get(0).schedule(); + } + } + + @Override + public final void execute() { + if (m_commands.isEmpty()) { + return; + } + Command currentCommand = m_commands.get(m_currentCommandIndex); + if (!currentCommand.isScheduled()) { + m_currentCommandIndex++; + } + if (m_currentCommandIndex < m_commands.size()) { + m_commands.get(m_currentCommandIndex).schedule(); + } + } + + @Override + public final void end(boolean interrupted) { + if (interrupted && !m_commands.isEmpty() && m_currentCommandIndex > -1 + && m_currentCommandIndex < m_commands.size()) { + m_commands.get(m_currentCommandIndex).cancel(); + } + m_currentCommandIndex = -1; + } + + @Override + public final boolean isFinished() { + return m_currentCommandIndex == m_commands.size(); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java index 358d52be..57a68322 100644 --- a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java +++ b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java @@ -70,7 +70,10 @@ public class ElevatorWrist extends SubsystemBase { private double estimatedWristAngle = 0; + public Trigger elevatorAtHome = new Trigger(() -> elevatorAtHome()); public Trigger elevatorAtAmp = new Trigger(() -> elevatorAtAmp()); + public Trigger wristReverseOutakeLimit = + new Trigger(() -> getWristAngle().getDegrees() <= 24.0); /** * Create new ElevatorWrist. diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 00000000..48a6f2d8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,73 @@ +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +/** + * Intake Subsystem + */ +public class Indexer extends SubsystemBase { + private IndexerIO io; + private IndexerInputsAutoLogged indexerAutoLogged = new IndexerInputsAutoLogged(); + + // private GenericEntry beamBrake = RobotContainer.mainDriverTab.add("Have Note", false) + // .withWidget(BuiltInWidgets.kBooleanBox).withPosition(9, 4).withSize(3, 2).getEntry(); + + + public Trigger noteInIndexer = new Trigger(() -> getIndexerBeamBrakeStatus()).debounce(0.25, + Debouncer.DebounceType.kRising); + public Trigger noteNotInIndexer = new Trigger(() -> !getIndexerBeamBrakeStatus()); + public Trigger indexerActive = new Trigger(() -> getIndexerRPM() > 0); + + public Indexer(IndexerIO io) { + this.io = io; + io.updateInputs(indexerAutoLogged); + } + + @Override + public void periodic() { + io.updateInputs(indexerAutoLogged); + Logger.processInputs("Indexer", indexerAutoLogged); + } + + /** + * Set the power for the indexer motor + * + * @param percentage 0-1 power for the indexer motor + */ + public void setIndexerMotor(double percentage) { + Logger.recordOutput("/Intake/Indexer Percentage", percentage); + io.setIndexerMotorPercentage(percentage); + } + + /** + * Get the status of the indexer beam brake. + * + * @return True if beam brake is broken, False if open + */ + public boolean getIndexerBeamBrakeStatus() { + return indexerAutoLogged.indexerBeamBrake; + } + + + public double getIndexerRPM() { + return indexerAutoLogged.indexerRPM; + } + + /** + * Command to run the indexer + * + * @return {@link Command} to run the indexer motors + */ + public Command runIndexerMotor(double speed) { + return Commands.startEnd(() -> { + setIndexerMotor(speed); + }, () -> { + setIndexerMotor(0); + }, this); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java new file mode 100644 index 00000000..13091c90 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.AutoLog; + +/** + * Intake IO Interface + */ +public interface IndexerIO { + + /** + * Intake Inputs to Log + */ + @AutoLog + public static class IndexerInputs { + public boolean indexerBeamBrake; + public double indexerRPM; + } + + public default void updateInputs(IndexerInputs inputs) {} + + public default void setIndexerMotorPercentage(double percent) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java similarity index 52% rename from src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java rename to src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java index 9bb4f827..50c05704 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java @@ -1,57 +1,36 @@ -package frc.robot.subsystems.intake; +package frc.robot.subsystems.indexer; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkRelativeEncoder; import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.Constants; /** * Intake IO Layer with real motors and sensors */ -public class IntakeIOFalcon implements IntakeIO { +public class IndexerIOFalcon implements IndexerIO { - private final CANSparkMax intakeMotorLeft = - new CANSparkMax(Constants.Motors.Intake.INTAKE_MOTOR_ID_LEFT, MotorType.kBrushless); - private final CANSparkMax intakeMotorRight = - new CANSparkMax(Constants.Motors.Intake.INTAKE_MOTOR_ID_RIGHT, MotorType.kBrushless); - public final RelativeEncoder intakeRelativeEnc = - intakeMotorLeft.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); private final TalonFX indexerMotor = new TalonFX(Constants.Motors.Intake.INDEXER_MOTOR_ID); private final DutyCycleOut indexerDutyCycleOut = new DutyCycleOut(0); private final TalonFXConfiguration indexerConfig = new TalonFXConfiguration(); private final DigitalInput indexerBeamBrake = new DigitalInput(Constants.IntakeConstants.INDEXER_BEAM_BRAKE_DIO_PORT); - private final DigitalInput intakeBeamBrake = - new DigitalInput(Constants.IntakeConstants.INTAKE_BEAM_BRAKE_DIO_PORT); /** * Intake IO Layer with real motors and sensors */ - public IntakeIOFalcon() { - intakeMotorLeft.restoreFactoryDefaults(); - intakeMotorRight.restoreFactoryDefaults(); - intakeMotorLeft.setInverted(Constants.IntakeConstants.INTAKE_MOTOR_INVERTED); - intakeMotorRight.setInverted(false); - intakeMotorLeft.setIdleMode(IdleMode.kCoast); - intakeMotorRight.setIdleMode(IdleMode.kCoast); - intakeMotorLeft.setSmartCurrentLimit(40); - intakeMotorRight.setSmartCurrentLimit(40); + public IndexerIOFalcon() { indexerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; indexerConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; indexerMotor.getConfigurator().apply(indexerConfig); } @Override - public void updateInputs(IntakeInputs inputs) { + public void updateInputs(IndexerInputs inputs) { // inputs.intakeSupplyVoltage = intakeMotorLeft.getBusVoltage(); // inputs.intakeAmps = intakeMotorLeft.getOutputCurrent(); // inputs.intakeRPM = intakeRelativeEnc.getVelocity(); @@ -60,15 +39,7 @@ public void updateInputs(IntakeInputs inputs) { // inputs.indexerAmps = indexerMotor.getSupplyCurrent().getValueAsDouble(); // inputs.indexerRPM = indexerMotor.getVelocity().getValueAsDouble(); inputs.indexerBeamBrake = !indexerBeamBrake.get(); // true == game piece - inputs.intakeBeamBrake = !intakeBeamBrake.get(); // true == game piece - } - - @Override - public void setIntakeMotorPercentage(double percent) { - // Left ratio is 60:30 - // Right ratio is 32:30 - intakeMotorLeft.set(percent); - intakeMotorRight.set(percent); + inputs.indexerRPM = 0; } @Override diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java new file mode 100644 index 00000000..356816be --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.indexer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants; + +/** + * Intake IO Layer with real motors and sensors + */ +public class IndexerIOSim implements IndexerIO { + + private FlywheelSim indexerSim = new FlywheelSim(DCMotor.getFalcon500(1), 1, 0.025); + private final DigitalInput indexerBeamBrake = + new DigitalInput(Constants.IntakeConstants.INDEXER_BEAM_BRAKE_DIO_PORT); + private double turnAppliedVolts = 0.0; + + /** + * Intake IO Layer with real motors and sensors + */ + public IndexerIOSim() { + + } + + @Override + public void updateInputs(IndexerInputs inputs) { + indexerSim.update(Constants.loopPeriodSecs); + inputs.indexerBeamBrake = !indexerBeamBrake.get(); // true == game piece + inputs.indexerRPM = indexerSim.getAngularVelocityRPM(); + } + + @Override + public void setIndexerMotorPercentage(double percent) { + turnAppliedVolts = MathUtil.clamp(percent * 12.0, -12.0, 12.0); + indexerSim.setInputVoltage(turnAppliedVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index ffb9409c..e75df168 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,13 +1,9 @@ package frc.robot.subsystems.intake; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.RobotContainer; +import edu.wpi.first.wpilibj2.command.button.Trigger; /** * Intake Subsystem @@ -16,12 +12,9 @@ public class Intake extends SubsystemBase { private IntakeIO io; private IntakeInputsAutoLogged intakeAutoLogged = new IntakeInputsAutoLogged(); - // private GenericEntry beamBrake = RobotContainer.mainDriverTab.add("Have Note", false) - // .withWidget(BuiltInWidgets.kBooleanBox).withPosition(9, 4).withSize(3, 2).getEntry(); - - private String noNote = Color.kBlack.toHexString(); - private GenericEntry haveNote = RobotContainer.mainDriverTab.add("Have Note", noNote) - .withWidget("Single Color View").withPosition(9, 4).withSize(3, 2).getEntry(); + public Trigger noteInIntake = new Trigger(() -> getintakeBeamBrakeStatus()).debounce(0.25, + Debouncer.DebounceType.kRising); + public Trigger intakeActive = new Trigger(() -> getIntakeRPM() > 0); public Intake(IntakeIO io) { this.io = io; @@ -32,15 +25,6 @@ public Intake(IntakeIO io) { public void periodic() { io.updateInputs(intakeAutoLogged); Logger.processInputs("Intake", intakeAutoLogged); - if (getIndexerBeamBrakeStatus() && getintakeBeamBrakeStatus()) { - haveNote.setString(Constants.LEDConstants.ALERT_COLOR.toHexString()); - } else if (getIndexerBeamBrakeStatus()) { - haveNote.setString(Constants.LEDConstants.INDEXER_COLOR.toHexString()); - } else if (getintakeBeamBrakeStatus()) { - haveNote.setString(Constants.LEDConstants.INTAKE_COLOR.toHexString()); - } else { - haveNote.setString(noNote); - } } /** @@ -53,25 +37,6 @@ public void setIntakeMotor(double percentage) { io.setIntakeMotorPercentage(percentage); } - /** - * Set the power for the indexer motor - * - * @param percentage 0-1 power for the indexer motor - */ - public void setIndexerMotor(double percentage) { - Logger.recordOutput("/Intake/Indexer Percentage", percentage); - io.setIndexerMotorPercentage(percentage); - } - - /** - * Get the status of the indexer beam brake. - * - * @return True if beam brake is broken, False if open - */ - public boolean getIndexerBeamBrakeStatus() { - return intakeAutoLogged.indexerBeamBrake; - } - /** * Get the status of the intake beam brake. * @@ -81,46 +46,7 @@ public boolean getintakeBeamBrakeStatus() { return intakeAutoLogged.intakeBeamBrake; } - /** - * Command to run the intake motor and indexer until the sensor trips - * - * @return {@link Command} to run the intake and indexer motors - */ - public Command runIntakeMotor(double intakeSpeed, double indexerSpeed) { - return Commands.startEnd(() -> { - setIntakeMotor(intakeSpeed); - setIndexerMotor(indexerSpeed); - }, () -> { - setIntakeMotor(0); - setIndexerMotor(0); - }, this).until(() -> getIndexerBeamBrakeStatus()).unless(() -> getIndexerBeamBrakeStatus()); - } - - /** - * Command to run the intake motor and indexer until the sensor trips - * - * @return {@link Command} to run the intake and indexer motors - */ - public Command runIntakeMotorNonStop(double intakeSpeed, double indexerSpeed) { - return Commands.startEnd(() -> { - setIntakeMotor(intakeSpeed); - setIndexerMotor(indexerSpeed); - }, () -> { - setIntakeMotor(0); - setIndexerMotor(0); - }, this); - } - - /** - * Command to run the indexer - * - * @return {@link Command} to run the indexer motors - */ - public Command runIndexerMotor(double speed) { - return Commands.startEnd(() -> { - setIndexerMotor(speed); - }, () -> { - setIndexerMotor(0); - }, this); + public double getIntakeRPM() { + return intakeAutoLogged.intakeRPM; } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 3a8387d0..7cf2a8fa 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -12,21 +12,11 @@ public interface IntakeIO { */ @AutoLog public static class IntakeInputs { - // public double intakeSupplyVoltage; - // public double indexerSupplyVoltage; - // public double intakeMotorVoltage; - // public double indexerMotorVoltage; - // public double intakeAmps; - // public double indexerAmps; - // public double intakeRPM; - // public double indexerRPM; - public boolean indexerBeamBrake; public boolean intakeBeamBrake; + public double intakeRPM; } public default void updateInputs(IntakeInputs inputs) {} public default void setIntakeMotorPercentage(double percent) {} - - public default void setIndexerMotorPercentage(double percent) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIORev.java b/src/main/java/frc/robot/subsystems/intake/IntakeIORev.java new file mode 100644 index 00000000..6b1b0dfd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIORev.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.intake; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkRelativeEncoder; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.Constants; + +/** + * Intake IO Layer with real motors and sensors + */ +public class IntakeIORev implements IntakeIO { + + private final CANSparkMax intakeMotorLeft = + new CANSparkMax(Constants.Motors.Intake.INTAKE_MOTOR_ID_LEFT, MotorType.kBrushless); + private final CANSparkMax intakeMotorRight = + new CANSparkMax(Constants.Motors.Intake.INTAKE_MOTOR_ID_RIGHT, MotorType.kBrushless); + public final RelativeEncoder intakeRelativeEnc = + intakeMotorLeft.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); + + private final DigitalInput intakeBeamBrake = + new DigitalInput(Constants.IntakeConstants.INTAKE_BEAM_BRAKE_DIO_PORT); + + /** + * Intake IO Layer with real motors and sensors + */ + public IntakeIORev() { + intakeMotorLeft.restoreFactoryDefaults(); + intakeMotorRight.restoreFactoryDefaults(); + intakeMotorLeft.setInverted(Constants.IntakeConstants.INTAKE_MOTOR_INVERTED); + intakeMotorRight.setInverted(false); + intakeMotorLeft.setIdleMode(IdleMode.kCoast); + intakeMotorRight.setIdleMode(IdleMode.kCoast); + intakeMotorLeft.setSmartCurrentLimit(40); + intakeMotorRight.setSmartCurrentLimit(40); + } + + @Override + public void updateInputs(IntakeInputs inputs) { + inputs.intakeBeamBrake = !intakeBeamBrake.get(); // true == game piece + inputs.intakeRPM = intakeRelativeEnc.getVelocity(); + } + + @Override + public void setIntakeMotorPercentage(double percent) { + // Left ratio is 60:30 + // Right ratio is 32:30 + intakeMotorLeft.set(percent); + intakeMotorRight.set(percent); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 00000000..20e080af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,39 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants; + +/** + * Intake IO Layer with real motors and sensors + */ +public class IntakeIOSim implements IntakeIO { + + private FlywheelSim inputSim = new FlywheelSim(DCMotor.getNEO(2), 1, 0.025); + private double turnAppliedVolts = 0.0; + + private final DigitalInput intakeBeamBrake = + new DigitalInput(Constants.IntakeConstants.INTAKE_BEAM_BRAKE_DIO_PORT); + + /** + * Intake IO Layer with real motors and sensors + */ + public IntakeIOSim() { + + } + + @Override + public void updateInputs(IntakeInputs inputs) { + inputSim.update(Constants.loopPeriodSecs); + inputs.intakeBeamBrake = !intakeBeamBrake.get(); // true == game piece + inputs.intakeRPM = inputSim.getAngularVelocityRPM(); + } + + @Override + public void setIntakeMotorPercentage(double percent) { + turnAppliedVolts = MathUtil.clamp(percent * 12.0, -12.0, 12.0); + inputSim.setInputVoltage(turnAppliedVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index dbb1937c..92cd9725 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.ReconfigurableSimpleMotorFeedforward; import frc.robot.Constants; @@ -31,6 +32,8 @@ public class Shooter extends SubsystemBase { private double topValue; private double bottomValue; + public Trigger isShooting = new Trigger(() -> inputs.topShooterVelocityRotPerMin > 0); + /** * Shooter Subsystem * diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java new file mode 100644 index 00000000..3ebe69e9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants; + +/** + * Class for ShooterSim + */ +public class ShooterSim implements ShooterIO { + + private FlywheelSim topShooterMotor = + new FlywheelSim(DCMotor.getNeoVortex(1), 1 / Constants.ShooterConstants.GEAR_RATIO, 0.025); + private FlywheelSim bottomShooterMotor = + new FlywheelSim(DCMotor.getNeoVortex(1), 1 / Constants.ShooterConstants.GEAR_RATIO, 0.025); + + private double topAppliedVolts = 0.0; + private double bottomAppliedVolts = 0.0; + + + /** + * Constructor Shooter Subsystem - sets motor and encoder preferences + */ + public ShooterSim() {} + + public void setTopMotor(double power) { + topAppliedVolts = MathUtil.clamp(power, -12.0, 12.0); + topShooterMotor.setInputVoltage(topAppliedVolts); + } + + public void setBottomMotor(double power) { + bottomAppliedVolts = MathUtil.clamp(power, -12.0, 12.0); + bottomShooterMotor.setInputVoltage(bottomAppliedVolts); + } + + + @Override + public void updateInputs(ShooterIOInputsAutoLogged inputs) { + topShooterMotor.update(Constants.loopPeriodSecs); + bottomShooterMotor.update(Constants.loopPeriodSecs); + inputs.topShooterVelocityRotPerMin = topShooterMotor.getAngularVelocityRPM(); + inputs.bottomShooterVelocityRotPerMin = bottomShooterMotor.getAngularVelocityRPM(); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index cf8f1c2c..66bf799b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -11,6 +11,7 @@ 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.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -23,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.FieldConstants; import frc.lib.util.photon.PhotonCameraWrapper; import frc.lib.util.swerve.SwerveModule; @@ -43,19 +45,26 @@ public class Swerve extends SubsystemBase { private boolean hasInitialized = false; private PhotonCameraWrapper[] cameras; private Boolean[] cameraSeesTarget = {false, false, false, false}; - + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + private SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] {new SwerveModulePosition(), new SwerveModulePosition(), + new SwerveModulePosition(), new SwerveModulePosition()}; private GenericEntry aprilTagTarget = RobotContainer.mainDriverTab.add("See April Tag", false) .withWidget(BuiltInWidgets.kBooleanBox) .withProperties(Map.of("Color when true", "green", "Color when false", "red")) .withPosition(11, 0).withSize(2, 2).getEntry(); + public Trigger seeAprilTag = + new Trigger(() -> Arrays.asList(cameraSeesTarget).stream().anyMatch(val -> val == true)); + /** * Swerve Subsystem */ public Swerve(SwerveIO swerveIO, PhotonCameraWrapper[] cameras) { this.swerveIO = swerveIO; this.cameras = cameras; - fieldOffset = getGyroYaw().getDegrees(); swerveMods = new SwerveModule[] { swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.driveMotorID, Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID, @@ -69,6 +78,7 @@ public Swerve(SwerveIO swerveIO, PhotonCameraWrapper[] cameras) { swerveIO.createSwerveModule(3, Constants.Swerve.Mod3.driveMotorID, Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID, Constants.Swerve.Mod3.angleOffset)}; + fieldOffset = getGyroYaw().getDegrees(); swerveOdometry = new SwerveDrivePoseEstimator(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions(), new Pose2d()); @@ -210,9 +220,26 @@ public Rotation2d getHeading() { * @return Current rotation/yaw of gyro as {@link Rotation2d} */ public Rotation2d getGyroYaw() { - float yaw = inputs.yaw; - return (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(-yaw) - : Rotation2d.fromDegrees(yaw); + // Update gyro angle + if (inputs.gyroConnected) { + // Use the real gyro angle + float yaw = inputs.yaw; + rawGyroRotation = (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(-yaw) + : Rotation2d.fromDegrees(yaw); + } else { + // Use the angle delta from the kinematics and module deltas + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = swerveMods[moduleIndex].getPosition(); + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + Twist2d twist = Constants.Swerve.swerveKinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + return rawGyroRotation; } /** @@ -307,8 +334,7 @@ public void periodic() { Robot.profiler.push("field"); field.setRobotPose(getPose()); Robot.profiler.swap("apriltag"); - aprilTagTarget - .setBoolean(Arrays.asList(cameraSeesTarget).stream().anyMatch(val -> val == true)); + aprilTagTarget.setBoolean(seeAprilTag.getAsBoolean()); Robot.profiler.swap("dist-to-speaker"); SmartDashboard.putNumber("Distance to Speaker", diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java index 37175e2e..e54520a0 100755 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java @@ -14,6 +14,7 @@ public interface SwerveIO { @AutoLog public static class SwerveInputs { + public boolean gyroConnected; public float yaw; public float roll; public float pitch; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java index 9155a0cb..7e3101fa 100755 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -16,6 +16,7 @@ public SwerveReal() {} @Override public void updateInputs(SwerveInputs inputs) { + inputs.gyroConnected = true; inputs.yaw = gyro.getYaw(); inputs.pitch = gyro.getPitch(); inputs.roll = gyro.getRoll(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java new file mode 100644 index 00000000..7b700fdf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.swerve; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.lib.util.swerve.SwerveModule; +import frc.lib.util.swerve.SwerveModuleSim; + +/** Real Class for Swerve */ +public class SwerveSim implements SwerveIO { + + /** Real Swerve Initializer */ + public SwerveSim() {} + + @Override + public void updateInputs(SwerveInputs inputs) { + inputs.gyroConnected = false; + // inputs.yaw = 0; + // inputs.pitch = 0; + // inputs.roll = 0; + + } + + @Override + public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, + int cancoderID, Rotation2d angleOffset) { + return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset, + new SwerveModuleSim(moduleNumber)); + } + +}