diff --git a/example.chor b/example.chor index ef477733..917c3047 100644 --- a/example.chor +++ b/example.chor @@ -5292,7 +5292,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Triple Helix": { "waypoints": [ @@ -6616,7 +6616,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Alliance Auto": { "waypoints": [ @@ -7405,7 +7405,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Allience Backup Path": { "waypoints": [ @@ -7632,7 +7632,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpHelper.java b/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpHelper.java index a081a8f2..25332d81 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpHelper.java +++ b/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpHelper.java @@ -3,27 +3,23 @@ import org.jmhsrobotics.frc2024.Constants; import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem; import org.jmhsrobotics.frc2024.subsystems.arm.commands.CommandArm; -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterAutoCommand; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.commands.IntakeCommand; +import org.jmhsrobotics.frc2024.subsystems.shintake.commands.ShooterAutoCommand; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; public class AmpHelper extends SequentialCommandGroup { private ArmPIDSubsystem armPIDSubsystem; - private ShooterSubsystem shooterSubsystem; - private IntakeSubsystem intakeSubsystem; + private ShintakeSubsystem shintakeSubsystem; - public AmpHelper(ArmPIDSubsystem armPIDSubsystem, ShooterSubsystem shooterSubsystem, - IntakeSubsystem intakeSubsystem) { - this.intakeSubsystem = intakeSubsystem; - this.shooterSubsystem = shooterSubsystem; + public AmpHelper(ArmPIDSubsystem armPIDSubsystem, ShintakeSubsystem shintakeSubsystem) { + this.shintakeSubsystem = shintakeSubsystem; this.armPIDSubsystem = armPIDSubsystem; addCommands(new CommandArm(this.armPIDSubsystem, Constants.ArmSetpoint.AMP.value), - new ParallelRaceGroup(new ShooterAutoCommand(this.shooterSubsystem, 5000).withTimeout(5), - new IntakeCommand(0.8, this.intakeSubsystem, this.shooterSubsystem).withTimeout(5))); + new ParallelRaceGroup(new ShooterAutoCommand(this.shintakeSubsystem, 5000).withTimeout(5), + new IntakeCommand(0.8, true, this.shintakeSubsystem).withTimeout(5))); } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpShotCommand.java b/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpShotCommand.java new file mode 100644 index 00000000..adc925fa --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpShotCommand.java @@ -0,0 +1,45 @@ +package org.jmhsrobotics.frc2024.ComboCommands; + +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem.ControlType; + +import edu.wpi.first.wpilibj2.command.Command; + +public class AmpShotCommand extends Command { + + private ShintakeSubsystem shintakeSubsystem; + /** + * Runs the intake and the shooter to expell a note into the amp. Command Never + * ends + * + * @param intakeSubsystem + * @param shooterSubsystem + */ + public AmpShotCommand(ShintakeSubsystem shintakeSubsystem) { + this.shintakeSubsystem = shintakeSubsystem; + + addRequirements(this.shintakeSubsystem); + } + + @Override + public void initialize() { + this.shintakeSubsystem.setIntakeSpeed(0); + } + + @Override + public void execute() { + this.shintakeSubsystem.setIntakeSpeed(1); + this.shintakeSubsystem.setShooterGoal(0.4 * 12, ControlType.VOLTAGE); + } + + @Override + public boolean isFinished() { + // return this.intakeSubsystem.hasNote(); + return false; + } + + @Override + public void end(boolean interrupted) { + this.shintakeSubsystem.setIntakeSpeed(0); + } +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/ComboIntakeArmCommand.java b/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/ComboIntakeArmCommand.java index b383bcd1..5d6c0110 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/ComboIntakeArmCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/ComboCommands/ComboIntakeArmCommand.java @@ -3,9 +3,8 @@ import org.jmhsrobotics.frc2024.Constants; import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem; import org.jmhsrobotics.frc2024.subsystems.arm.commands.CommandArm; -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.commands.IntakeCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -15,12 +14,11 @@ public class ComboIntakeArmCommand extends ParallelCommandGroup { * Moves arm to the Pickup position and Intakes. * * @param arm - * @param shooter - * @param intake + * @param shintake */ - public ComboIntakeArmCommand(ArmPIDSubsystem arm, ShooterSubsystem shooter, IntakeSubsystem intake) { + public ComboIntakeArmCommand(ArmPIDSubsystem arm, ShintakeSubsystem shintakeSubsystem) { addCommands(new CommandArm(arm, Constants.ArmSetpoint.PICKUP.value), // move arm to intake position - new IntakeCommand(1, intake, shooter)// start intkae + new IntakeCommand(1, false, shintakeSubsystem)// start intkae ); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 82f98c33..92a43432 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -4,11 +4,12 @@ package org.jmhsrobotics.frc2024; -import org.jmhsrobotics.frc2024.ComboCommands.AmpHelper; +import org.jmhsrobotics.frc2024.ComboCommands.AmpShotCommand; import org.jmhsrobotics.frc2024.ComboCommands.ComboIntakeArmCommand; -import org.jmhsrobotics.frc2024.autoCommands.AutoAmpShotCommand; -import org.jmhsrobotics.frc2024.autoCommands.FireCommand; -import org.jmhsrobotics.frc2024.autoCommands.TurnAndShootCommand; +import org.jmhsrobotics.frc2024.autoCommands.NFireAmp; +import org.jmhsrobotics.frc2024.autoCommands.NFloorIntake; +import org.jmhsrobotics.frc2024.autoCommands.NSpinupAndShoot; +import org.jmhsrobotics.frc2024.autoCommands.NSpinupNoStop; import org.jmhsrobotics.frc2024.controlBoard.CompControl; import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; import org.jmhsrobotics.frc2024.controlBoard.SingleControl; @@ -28,21 +29,12 @@ import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.LockSpeaker; import org.jmhsrobotics.frc2024.subsystems.drive.commands.auto.DriveTimeCommand; -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.AmpShotCommand; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.AutoIntakeCommand; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.DefaultIntakeCommand; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.ExtakeCommand; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeFireCommand; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterAutoCommand; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.commands.DefaultShintakeCommand; +import org.jmhsrobotics.frc2024.subsystems.shintake.commands.IntakeCommand; +import org.jmhsrobotics.frc2024.subsystems.shintake.commands.ShooterAutoCommand; import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; import org.jmhsrobotics.frc2024.utils.RumbleTimeCommand; -import org.jmhsrobotics.frc2024.utils.newcmd.NFireAmp; -import org.jmhsrobotics.frc2024.utils.newcmd.NFloorIntake; -import org.jmhsrobotics.frc2024.utils.newcmd.NSpinupAndShoot; -import org.jmhsrobotics.frc2024.utils.newcmd.NSpinupNoStop; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -72,9 +64,11 @@ public class RobotContainer implements Logged { private final VisionSubsystem visionSubsystem = new VisionSubsystem(this.driveSubsystem); - private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + // private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); private final LEDSubsystem ledSubsystem = new LEDSubsystem(); - private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + + // private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + private final ShintakeSubsystem shintakeSubsystem = new ShintakeSubsystem(); private final ArmPIDSubsystem armSubsystem = new ArmPIDSubsystem(); @@ -100,14 +94,12 @@ public RobotContainer() { this.driveSubsystem .setDefaultCommand(new DriveCommand(this.driveSubsystem, this.visionSubsystem, this.control)); - this.intakeSubsystem.setDefaultCommand(new DefaultIntakeCommand(intakeSubsystem, shooterSubsystem)); + this.shintakeSubsystem.setDefaultCommand(new DefaultShintakeCommand(this.shintakeSubsystem)); this.ledSubsystem.setDefaultCommand(new RainbowLEDCommand(this.ledSubsystem)); configureSmartDashboard(); - SmartDashboard.putBoolean("HasNote", false); configureDriverFeedback(); - configureBindings(); // Named commands must be added before building the chooser. configurePathPlanner(); @@ -133,8 +125,8 @@ public RobotContainer() { var preLoadOnePiece = Commands.sequence( Commands.race(new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value), - new NSpinupNoStop(this.shooterSubsystem, 5000)), - new NSpinupAndShoot(this.shooterSubsystem, this.intakeSubsystem, 5000)); + new NSpinupNoStop(this.shintakeSubsystem, 5000)), + new NSpinupAndShoot(this.shintakeSubsystem, 5000)); autoChooser.addOption("preLoadOnePiece", preLoadOnePiece); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -142,52 +134,38 @@ public RobotContainer() { // Commands to test SmartDashboard.putData("Arm Preset Shoot", new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value)); - SmartDashboard.putData("Intake Floor", new NFloorIntake(armSubsystem, intakeSubsystem)); - SmartDashboard.putData("Fire in Amp", new NFireAmp(this.shooterSubsystem, this.intakeSubsystem)); - SmartDashboard.putData("Spinup and Shoot", new NSpinupAndShoot(shooterSubsystem, intakeSubsystem, 5000)); - SmartDashboard.putData("Spinup no Stop", new NSpinupNoStop(shooterSubsystem, 5000)); + SmartDashboard.putData("Intake Floor", new NFloorIntake(armSubsystem, shintakeSubsystem)); + SmartDashboard.putData("Fire in Amp", new NFireAmp(shintakeSubsystem)); + SmartDashboard.putData("Spinup and Shoot", new NSpinupAndShoot(shintakeSubsystem, 5000)); + SmartDashboard.putData("Spinup no Stop", new NSpinupNoStop(shintakeSubsystem, 5000)); SmartDashboard.putData("Aim Arm Vision", new ArmVision(armSubsystem, visionSubsystem, driveSubsystem).until(armSubsystem::atGoal)); // TODO: // Handle // End - // // condition - // SmartDashboard.putData("RedLED", new setRedLEDCommand(ledSubsystem, - // intakeSubsystem)); SmartDashboard.putData("ArmVision", new ArmVision(armSubsystem, visionSubsystem, driveSubsystem)); SmartDashboard.putData("Lock Speaker", new LockSpeaker(driveSubsystem, visionSubsystem)); } private void configureDriverFeedback() { - new Trigger(intakeSubsystem::hasNote) + new Trigger(shintakeSubsystem::hasNote) .onTrue(new ParallelCommandGroup(new RumbleTimeCommand(control, RumbleType.kLeftRumble, 1, 1), - new setBlueLEDCommand(ledSubsystem, this.intakeSubsystem))); - - // new Trigger(this.defaultIntakeCommand::isScheduled) - // .onTrue(new FlashingLEDCommand(ledSubsystem, intakeSubsystem)); - // new Trigger(intakeSubsystem::isIntaking).onTrue(new - // FlashingLEDCommand(ledSubsystem, intakeSubsystem)); + new setBlueLEDCommand(ledSubsystem, this.shintakeSubsystem))); - // new Trigger(intakeSubsystem.getCurrentCommand()==new - // DefaultIntakeCommand(this.intakeSubsystem, this.shooterSubsystem)).onTrue(new - // FlashingLEDCommand(ledSubsystem, intakeSubsystem)); - // new Trigger(this.defaultIntakeCommand::isScheduled) - // .onTrue(new FlashingLEDCommand(ledSubsystem, intakeSubsystem)); + new Trigger(shintakeSubsystem::noteTooHigh).onTrue(new setRedLEDCommand(ledSubsystem, shintakeSubsystem)); - new Trigger(intakeSubsystem::noteTooHigh).onTrue(new setRedLEDCommand(ledSubsystem, intakeSubsystem)); - - new Trigger(shooterSubsystem::atGoal) + new Trigger(shintakeSubsystem::shooterAtGoal) .whileTrue(new RumbleTimeCommand(this.control, RumbleType.kRightRumble, 0.2, 1)); - new Trigger(shooterSubsystem::atGoal).onTrue(new setYellowLEDCommand(ledSubsystem, shooterSubsystem)); + new Trigger(shintakeSubsystem::shooterAtGoal).onTrue(new setYellowLEDCommand(ledSubsystem, shintakeSubsystem)); } private void configurePathPlanner() { // Add path planner auto chooser. var preLoadOnePiece = Commands.sequence( Commands.race(new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value), - new NSpinupNoStop(this.shooterSubsystem, 5000)), - new NSpinupAndShoot(this.shooterSubsystem, this.intakeSubsystem, 5000)); + new NSpinupNoStop(this.shintakeSubsystem, 5000)), + new NSpinupAndShoot(this.shintakeSubsystem, 5000)); AutoBuilder.configureHolonomic(driveSubsystem::getPose, driveSubsystem::resetOdometry, driveSubsystem::getChassisSpeeds, driveSubsystem::drive, @@ -197,42 +175,26 @@ private void configurePathPlanner() { NamedCommands.registerCommand("ArmAmp", new CommandArm(this.armSubsystem, Constants.ArmSetpoint.AMP.value)); NamedCommands.registerCommand("ArmShoot", new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value)); - NamedCommands.registerCommand("Extake", new ExtakeCommand(this.intakeSubsystem, 1).withTimeout(5)); - NamedCommands.registerCommand("TurnAndShoot", new TurnAndShootCommand(this.visionSubsystem, this.driveSubsystem, - this.armSubsystem, this.shooterSubsystem, this.intakeSubsystem)); - NamedCommands.registerCommand("Intake", - new IntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem).withTimeout(0.5)); - NamedCommands.registerCommand("AutoIntake", - new AutoIntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem)); + NamedCommands.registerCommand("Intake", new IntakeCommand(1, false, this.shintakeSubsystem).withTimeout(0.5)); // Move Arm to Pickup position NamedCommands.registerCommand("ArmPickup", new CommandArm(this.armSubsystem, Constants.ArmSetpoint.PICKUP.value)); - NamedCommands.registerCommand("Fire", new FireCommand(this.intakeSubsystem, this.shooterSubsystem)); NamedCommands.registerCommand("PrepareShot", - new PrepareShot(this.driveSubsystem, this.armSubsystem, this.shooterSubsystem, this.visionSubsystem) + new PrepareShot(this.driveSubsystem, this.armSubsystem, this.shintakeSubsystem, this.visionSubsystem) .withTimeout(1)); - NamedCommands.registerCommand("AmpScore", - new AmpHelper(this.armSubsystem, this.shooterSubsystem, this.intakeSubsystem)); - // New Commands + // AutoCommands NamedCommands.registerCommand("Arm Preset Shoot", new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value)); - NamedCommands.registerCommand("Intake Floor", new NFloorIntake(armSubsystem, intakeSubsystem).withTimeout(2)); - NamedCommands.registerCommand("Fire in Amp", new NFireAmp(this.shooterSubsystem, this.intakeSubsystem)); - NamedCommands.registerCommand("Spinup and Shoot", new NSpinupAndShoot(shooterSubsystem, intakeSubsystem, 5000)); - NamedCommands.registerCommand("Spinup no Stop", new NSpinupNoStop(shooterSubsystem, 5000)); + NamedCommands.registerCommand("Intake Floor", new NFloorIntake(armSubsystem, shintakeSubsystem).withTimeout(2)); + NamedCommands.registerCommand("Fire in Amp", new NFireAmp(this.shintakeSubsystem)); + NamedCommands.registerCommand("Spinup and Shoot", new NSpinupAndShoot(shintakeSubsystem, 5000)); + NamedCommands.registerCommand("Spinup no Stop", new NSpinupNoStop(shintakeSubsystem, 5000)); NamedCommands.registerCommand("Aim Arm Vision", new ArmVision(armSubsystem, visionSubsystem, driveSubsystem)); // TODO: // Handle // End - // condition NamedCommands.registerCommand("Lock Speaker", new LockSpeaker(driveSubsystem, visionSubsystem)); - NamedCommands.registerCommand("ComboIntake", - new ComboIntakeArmCommand(this.armSubsystem, this.shooterSubsystem, this.intakeSubsystem) - .withTimeout(1)); - // NamedCommands.registerCommand("AmpShoot", new AmpShotCommand(intakeSubsystem, - // shooterSubsystem).withTimeout(1)); - NamedCommands.registerCommand("AmpShoot", new AutoAmpShotCommand(this.intakeSubsystem, this.shooterSubsystem)); NamedCommands.registerCommand("One Piece Preload Shoot", preLoadOnePiece); } @@ -253,16 +215,16 @@ private void configureBindings() { this.control.presetMid().onTrue(new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value)); // this.control.presetLow().onTrue(new CommandArm(this.armSubsystem, // Constants.ArmSetpoint.PICKUP.value)); - this.control.presetLow().whileTrue(new ComboIntakeArmCommand(armSubsystem, shooterSubsystem, intakeSubsystem)); + this.control.presetLow().whileTrue(new ComboIntakeArmCommand(armSubsystem, shintakeSubsystem)); this.control.presetLow().onFalse(new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value)); /* Intake Controls */ - this.control.intakeInput().whileTrue(new IntakeFireCommand(1, this.intakeSubsystem)); - this.control.extakeInput().whileTrue(new IntakeCommand(-1, this.intakeSubsystem, this.shooterSubsystem)); + this.control.intakeInput().whileTrue(new IntakeCommand(1, true, this.shintakeSubsystem)); + this.control.extakeInput().whileTrue(new IntakeCommand(-1, false, this.shintakeSubsystem)); /* Shooter Controls */ - this.control.shooterInput().whileTrue(new ShooterAutoCommand(this.shooterSubsystem, 5000)); - this.control.ampShooterInput().whileTrue(new AmpShotCommand(intakeSubsystem, shooterSubsystem)); + this.control.shooterInput().whileTrue(new ShooterAutoCommand(this.shintakeSubsystem, 5000)); + this.control.ampShooterInput().whileTrue(new AmpShotCommand(shintakeSubsystem)); // temp climber controls // this.control.climberRetract().onTrue(new ClimbCommand(this.climberSubsystem, @@ -303,7 +265,7 @@ public void configureSmartDashboard() { public void configureTeam() { this.control.AprilLockOn() .whileTrue(Commands.repeatingSequence(new ArmVision(armSubsystem, visionSubsystem, driveSubsystem))); - this.control.AprilLockOn().whileTrue(new ShooterAutoCommand(shooterSubsystem, 5000)); + this.control.AprilLockOn().whileTrue(new ShooterAutoCommand(shintakeSubsystem, 5000)); // if (getAllianceFlipState()) { // this.control.AprilLockOn() // .whileTrue(Commands.repeatingSequence(new PrepareShot(driveSubsystem, diff --git a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/AutoAmpShotCommand.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/AutoAmpShotCommand.java deleted file mode 100644 index 650be725..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/AutoAmpShotCommand.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.jmhsrobotics.frc2024.autoCommands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterAutoCommand; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; - -public class AutoAmpShotCommand extends ParallelCommandGroup { - private ShooterSubsystem shooterSubsystem; - private IntakeSubsystem intakeSubsystem; - - public AutoAmpShotCommand(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) { - this.shooterSubsystem = shooterSubsystem; - this.intakeSubsystem = intakeSubsystem; - addCommands(new ShooterAutoCommand(this.shooterSubsystem, 2000), - new IntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem).withTimeout(.75)); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/FireCommand.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/FireCommand.java deleted file mode 100644 index 9a8da4c7..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/FireCommand.java +++ /dev/null @@ -1,27 +0,0 @@ -package org.jmhsrobotics.frc2024.autoCommands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterAutoCommand; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public class FireCommand extends SequentialCommandGroup { - private IntakeSubsystem intakeSubsystem; - private ShooterSubsystem shooterSubsystem; - - /** - * Runs the intake for 0.2 seconds and spins up the shooter flywheels to 5000 - * rpm with a timeout of 0.2 seconds Whole command ends is 0.2 seconds - * - * @param intakeSubsystem - * @param shooterSubsystem - */ - public FireCommand(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) { - this.intakeSubsystem = intakeSubsystem; - this.shooterSubsystem = shooterSubsystem; - addCommands(new ShooterAutoCommand(this.shooterSubsystem, 5000).withTimeout(0.2), - new IntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem).withTimeout(0.2)); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NFireAmp.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NFireAmp.java new file mode 100644 index 00000000..3cbfc194 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NFireAmp.java @@ -0,0 +1,48 @@ +package org.jmhsrobotics.frc2024.autoCommands; + +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem.ControlType; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj2.command.Command; + +public class NFireAmp extends Command { + ShintakeSubsystem shintakeSubsystem; + Debouncer sensorDebounce = new Debouncer(0.5); + + /** + * Fires a note into the amp. Runs shooter and flywheel. End when both hasnote + * and note too high are false. + * + * @param shooter + * @param intake + */ + public NFireAmp(ShintakeSubsystem shintakeSubsystem) { + this.shintakeSubsystem = shintakeSubsystem; + addRequirements(shintakeSubsystem); + } + + @Override + public boolean isFinished() { + return sensorDebounce.calculate(!this.shintakeSubsystem.hasNote() && !this.shintakeSubsystem.noteTooHigh()); + } + + @Override + public void initialize() { + shintakeSubsystem.setShooterGoal(12, ControlType.VOLTAGE); + shintakeSubsystem.setIntakeSpeed(1); + } + + @Override + public void execute() { + shintakeSubsystem.setShooterGoal(12, ControlType.VOLTAGE); + shintakeSubsystem.setIntakeSpeed(1); + } + + @Override + public void end(boolean interrupted) { + shintakeSubsystem.setShooterGoal(12, ControlType.VOLTAGE); + shintakeSubsystem.setIntakeSpeed(0); + } + +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NFloorIntake.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NFloorIntake.java similarity index 55% rename from src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NFloorIntake.java rename to src/main/java/org/jmhsrobotics/frc2024/autoCommands/NFloorIntake.java index 7f955b51..8c0ebe7e 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NFloorIntake.java +++ b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NFloorIntake.java @@ -1,9 +1,9 @@ -package org.jmhsrobotics.frc2024.utils.newcmd; +package org.jmhsrobotics.frc2024.autoCommands; import org.jmhsrobotics.frc2024.Constants; import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem; import org.jmhsrobotics.frc2024.subsystems.arm.commands.CommandArm; -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj2.command.Command; @@ -18,36 +18,36 @@ public class NFloorIntake extends SequentialCommandGroup { * @param arm * @param intake */ - public NFloorIntake(ArmPIDSubsystem arm, IntakeSubsystem intake) { + public NFloorIntake(ArmPIDSubsystem arm, ShintakeSubsystem shintakeSubsystem) { // new CommandArm(arm, Constants.ArmSetpoint.PICKUP.value), - addCommands(Commands.either(Commands.none(), - Commands.parallel(new CommandArm(arm, Constants.ArmSetpoint.PICKUP.value), new LIntake(intake)), - intake::hasNote)); + addCommands(Commands.either(Commands.none(), Commands + .parallel(new CommandArm(arm, Constants.ArmSetpoint.PICKUP.value), new LIntake(shintakeSubsystem)), + shintakeSubsystem::hasNote)); } private class LIntake extends Command { private Debouncer debounce = new Debouncer(0.04); // TODO: Tune - private IntakeSubsystem intake; + private ShintakeSubsystem shintakeSubsystem; - public LIntake(IntakeSubsystem intake) { - this.intake = intake; - addRequirements(intake); + public LIntake(ShintakeSubsystem shintakeSubsystem) { + this.shintakeSubsystem = shintakeSubsystem; + addRequirements(shintakeSubsystem); } @Override public void execute() { - intake.set(1); + shintakeSubsystem.setIntakeSpeed(1); } @Override public boolean isFinished() { - return debounce.calculate(intake.hasNote()); + return debounce.calculate(shintakeSubsystem.hasNote()); } @Override public void end(boolean interrupted) { - intake.set(0); + shintakeSubsystem.setIntakeSpeed(0); } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NSpinupAndShoot.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NSpinupAndShoot.java new file mode 100644 index 00000000..43138f71 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NSpinupAndShoot.java @@ -0,0 +1,54 @@ +package org.jmhsrobotics.frc2024.autoCommands; + +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem.ControlType; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj2.command.Command; + +public class NSpinupAndShoot extends Command { + private ShintakeSubsystem shintake; + private double targetRPM; + private boolean shouldFire = false; + private Debouncer noteExitDebouncer = new Debouncer(0.1); + + /** + * Spins up Shooter Flywheel to target RPM and then runs intake to shoot a note + * into the speaker. Ends when both has note and note too high are false + * + * @param shintake + * @param rpm + */ + public NSpinupAndShoot(ShintakeSubsystem shintake, double rpm) { + this.shintake = shintake; + this.targetRPM = rpm; + addRequirements(shintake); + } + + @Override + public void initialize() { + this.shouldFire = false; + this.shintake.setShooterGoal(targetRPM, ControlType.PID); + } + + @Override + public void execute() { + if (this.shintake.isShooterAtGoal()) { // TODO: Do we need debouncing? + shouldFire = true; + } + + shintake.setIntakeSpeed(shouldFire ? 1 : 0); + } + + @Override + public boolean isFinished() { + return shouldFire && noteExitDebouncer.calculate(!this.shintake.hasNote() && !this.shintake.noteTooHigh()); + } + + @Override + public void end(boolean interrupted) { + shintake.setIntakeSpeed(0); + this.shintake.setShooterGoal(targetRPM, ControlType.PID); + } + +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NSpinupNoStop.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NSpinupNoStop.java new file mode 100644 index 00000000..03c3e4bf --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/NSpinupNoStop.java @@ -0,0 +1,42 @@ +package org.jmhsrobotics.frc2024.autoCommands; + +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem.ControlType; + +import edu.wpi.first.wpilibj2.command.Command; + +public class NSpinupNoStop extends Command { + private ShintakeSubsystem shintake; + private double targetRPM; + + /** + * Spins flywheel up to target RPM. Never ends. + * + * @param shintake + * @param rpm + */ + public NSpinupNoStop(ShintakeSubsystem shintake, double rpm) { + this.shintake = shintake; + this.targetRPM = rpm; + addRequirements(shintake); + } + @Override + public void initialize() { + shintake.setShooterGoal(targetRPM, ControlType.PID); + } + @Override + public boolean isFinished() { + return false; + } + + @Override + public void execute() { + this.shintake.setShooterGoal(targetRPM, ControlType.PID); + } + + @Override + public void end(boolean interrupted) { + shintake.setShooterGoal(0, ControlType.PID); + } + +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/ScoreAmp.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/ScoreAmp.java deleted file mode 100644 index 33165197..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/ScoreAmp.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.jmhsrobotics.frc2024.autoCommands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ScoreAmp extends Command { - private IntakeSubsystem intake; - private ShooterSubsystem shooter; - public ScoreAmp(IntakeSubsystem intake, ShooterSubsystem shooter) { - this.intake = intake; - this.shooter = shooter; - addRequirements(this.intake, this.shooter); - } - - @Override - public void initialize() { - this.intake.set(1); - this.shooter.set(12, ControlType.VOLTAGE); - } - - @Override - public void execute() { - this.intake.set(1); - this.shooter.set(12, ControlType.VOLTAGE); - } - - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/TurnAndShootCommand.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/TurnAndShootCommand.java deleted file mode 100644 index 7a3c39cf..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/TurnAndShootCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.jmhsrobotics.frc2024.autoCommands; - -import org.jmhsrobotics.frc2024.Constants; -import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem; -import org.jmhsrobotics.frc2024.subsystems.arm.commands.CommandArm; -import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; -import org.jmhsrobotics.frc2024.subsystems.drive.commands.LockSpeaker; -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public class TurnAndShootCommand extends SequentialCommandGroup { - - private VisionSubsystem visionSubsystem; - private DriveSubsystem driveSubsystem; - private ArmPIDSubsystem armSubsystem; - private ShooterSubsystem shooterSubsystem; - private IntakeSubsystem intakeSubsystem; - - public TurnAndShootCommand(VisionSubsystem visionSubsystem, DriveSubsystem driveSubsystem, - ArmPIDSubsystem armSubsystem, ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { - // TODO: rename the folder name - this.visionSubsystem = visionSubsystem; - this.driveSubsystem = driveSubsystem; - this.armSubsystem = armSubsystem; - this.shooterSubsystem = shooterSubsystem; - this.intakeSubsystem = intakeSubsystem; - - addCommands(new ParallelCommandGroup(new LockSpeaker(this.driveSubsystem, this.visionSubsystem), // TODO: - // Remove - // hardcoded - // id - new CommandArm(this.armSubsystem, Constants.ArmSetpoint.SHOOT.value)), - new FireCommand(this.intakeSubsystem, this.shooterSubsystem)); - - addRequirements(this.visionSubsystem, this.armSubsystem, this.driveSubsystem, this.shooterSubsystem, - this.intakeSubsystem); - } - -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/FlashingLEDCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/FlashingLEDCommand.java index bb8004f2..c8612cdc 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/FlashingLEDCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/FlashingLEDCommand.java @@ -1,7 +1,7 @@ package org.jmhsrobotics.frc2024.subsystems.LED.commands; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -9,12 +9,12 @@ public class FlashingLEDCommand extends Command { private LEDSubsystem ledSubsystem; - private IntakeSubsystem intakeSubsystem; + private ShintakeSubsystem shintakeSubsystem; private Timer timer; - public FlashingLEDCommand(LEDSubsystem ledSubsystem, IntakeSubsystem intakeSubsystem) { + public FlashingLEDCommand(LEDSubsystem ledSubsystem, ShintakeSubsystem shintakeSubsystem) { this.ledSubsystem = ledSubsystem; - this.intakeSubsystem = intakeSubsystem; + this.shintakeSubsystem = shintakeSubsystem; this.timer = new Timer(); addRequirements(this.ledSubsystem); @@ -40,7 +40,7 @@ public void execute() { @Override public boolean isFinished() { - return !this.intakeSubsystem.isIntaking(); + return false; } @Override diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setBlueLEDCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setBlueLEDCommand.java index 0c063ef1..187d5907 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setBlueLEDCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setBlueLEDCommand.java @@ -1,18 +1,18 @@ package org.jmhsrobotics.frc2024.subsystems.LED.commands; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; import edu.wpi.first.wpilibj2.command.Command; public class setBlueLEDCommand extends Command { private LEDSubsystem ledSubsystem; - private IntakeSubsystem intakeSubsystem; + private ShintakeSubsystem shintakeSubsystem; - public setBlueLEDCommand(LEDSubsystem ledSubsystem, IntakeSubsystem intakeSubsystem) { + public setBlueLEDCommand(LEDSubsystem ledSubsystem, ShintakeSubsystem shintakeSubsystem) { this.ledSubsystem = ledSubsystem; - this.intakeSubsystem = intakeSubsystem; + this.shintakeSubsystem = shintakeSubsystem; addRequirements(this.ledSubsystem); } @@ -31,7 +31,7 @@ public void execute() { @Override public boolean isFinished() { - return !this.intakeSubsystem.hasNote(); + return !this.shintakeSubsystem.hasNote(); } @Override diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setRedLEDCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setRedLEDCommand.java index f36e48b2..25fe58ec 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setRedLEDCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setRedLEDCommand.java @@ -1,18 +1,18 @@ package org.jmhsrobotics.frc2024.subsystems.LED.commands; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; import edu.wpi.first.wpilibj2.command.Command; public class setRedLEDCommand extends Command { private LEDSubsystem ledSubsystem; - private IntakeSubsystem intakeSubsystem; + private ShintakeSubsystem shintakeSubsystem; - public setRedLEDCommand(LEDSubsystem ledSubsystem, IntakeSubsystem intakeSubsystem) { + public setRedLEDCommand(LEDSubsystem ledSubsystem, ShintakeSubsystem shintakeSubsystem) { this.ledSubsystem = ledSubsystem; - this.intakeSubsystem = intakeSubsystem; + this.shintakeSubsystem = shintakeSubsystem; addRequirements(this.ledSubsystem); } @@ -31,7 +31,7 @@ public void execute() { @Override public boolean isFinished() { - return !this.intakeSubsystem.noteTooHigh(); + return !this.shintakeSubsystem.noteTooHigh(); } @Override diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setYellowLEDCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setYellowLEDCommand.java index 44dbe71b..c21646c1 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setYellowLEDCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/LED/commands/setYellowLEDCommand.java @@ -1,18 +1,18 @@ package org.jmhsrobotics.frc2024.subsystems.LED.commands; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; import edu.wpi.first.wpilibj2.command.Command; public class setYellowLEDCommand extends Command { + private ShintakeSubsystem shintakeSubsystem; private LEDSubsystem ledSubsystem; - private ShooterSubsystem shooterSubsystem; - public setYellowLEDCommand(LEDSubsystem ledSubsystem, ShooterSubsystem shooterSubsystem) { + public setYellowLEDCommand(LEDSubsystem ledSubsystem, ShintakeSubsystem shintakeSubsystem) { this.ledSubsystem = ledSubsystem; - this.shooterSubsystem = shooterSubsystem; + this.shintakeSubsystem = shintakeSubsystem; addRequirements(this.ledSubsystem); } @@ -31,7 +31,7 @@ public void execute() { @Override public boolean isFinished() { - return !this.shooterSubsystem.atGoal(); + return !this.shintakeSubsystem.shooterAtGoal(); } @Override diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/PrepareShot.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/PrepareShot.java index 8dd5ef62..ac14c5bd 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/PrepareShot.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/PrepareShot.java @@ -2,8 +2,8 @@ import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterAutoCommand; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.commands.ShooterAutoCommand; import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; import edu.wpi.first.wpilibj2.command.Command; @@ -17,10 +17,11 @@ public class PrepareShot extends Command { * * @param drive * @param arm - * @param shooter + * @param shintake * @param vision */ - public PrepareShot(DriveSubsystem drive, ArmPIDSubsystem arm, ShooterSubsystem shooter, VisionSubsystem vision) { - Commands.repeatingSequence(new ArmVision(arm, vision, drive), new ShooterAutoCommand(shooter, 5000)); + public PrepareShot(DriveSubsystem drive, ArmPIDSubsystem arm, ShintakeSubsystem shintakeSubsystem, + VisionSubsystem vision) { + Commands.repeatingSequence(new ArmVision(arm, vision, drive), new ShooterAutoCommand(shintakeSubsystem, 5000)); } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java deleted file mode 100644 index 66fcc0d8..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java +++ /dev/null @@ -1,134 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.intake; - -import org.jmhsrobotics.frc2024.Constants; -import org.jmhsrobotics.frc2024.Robot; -import org.jmhsrobotics.frc2024.utils.SimTimeOfFlight; -import org.jmhsrobotics.frc2024.utils.SimableTimeOfFlight; -import org.jmhsrobotics.warcore.rev.RevEncoderSimWrapper; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.playingwithfusion.TimeOfFlight; -import com.playingwithfusion.TimeOfFlight.RangingMode; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkLimitSwitch; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.DIOSim; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import monologue.Logged; - -public class IntakeSubsystem extends SubsystemBase implements Logged { - private CANSparkMax intakeMotor; - - private SimableTimeOfFlight lowerSensor; - private SimableTimeOfFlight upperSensor; - - private boolean isIntaking = false; - - public IntakeSubsystem() { - intakeMotor = new CANSparkMax(Constants.CAN.kIntakeId, MotorType.kBrushless); - intakeMotor.setInverted(true); - intakeMotor.setIdleMode(IdleMode.kBrake); - - this.lowerSensor = new SimableTimeOfFlight(1); - this.upperSensor = new SimableTimeOfFlight(0); - intakeMotor.setSmartCurrentLimit(35); - - this.lowerSensor.setRangingMode(RangingMode.Short, 24); - this.upperSensor.setRangingMode(RangingMode.Short, 24); - if (RobotBase.isSimulation()) { - simInit(); - } - } - - public boolean isIntaking() { - return isIntaking; - } - - @Override - public void periodic() { - this.isIntaking = this.intakeMotor.get() != 0 ? true : false; - // SmartDashboard.putNumber("intake/velocityRPM", - // intakeMotor.getEncoder().getVelocity()); - // SmartDashboard.putNumber("intake/currentDrawAmps", - // intakeMotor.getOutputCurrent()); - // SmartDashboard.putBoolean("Intake/highSwitchState", - // this.highSwitch().isPressed()); - // SmartDashboard.putBoolean("Intake/lowSwitchState", - // this.lowSwitch().isPressed()); - - // SmartDashboard.putBoolean("intake/hasNote", this.hasNote()); - - log("intakeDutyCycle", intakeMotor.get()); - log("hasNote", this.hasNote()); - log("noteTooHigh", this.noteTooHigh()); - log("lowerSensoDistance", this.lowerSensor.getRange()); - log("upperSensor", this.upperSensor.getRange()); - } - - public void set(double speed) { - intakeMotor.set(-speed); - } - - public TimeOfFlight lowerSensor() { - return this.lowerSensor; - } - - public TimeOfFlight upperSensor() { - return this.upperSensor; - } - - public SparkLimitSwitch lowSwitch() { - return this.intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed); - } - - public SparkLimitSwitch highSwitch() { - return this.intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed); - } - - public boolean hasNote() { - return this.lowerSensor.getRange() < 270; - } - - public boolean noteTooHigh() { - return this.upperSensor.getRange() < 320; - } - - private DIOSim intakeSwitchSim; - private DCMotorSim intakeSim; - private RevEncoderSimWrapper intakeEncSim; - private SimTimeOfFlight lowerSim; - private SimTimeOfFlight upperSim; - private Debouncer intakeDebounceSim = new Debouncer(0.2); - public void simInit() { - intakeSwitchSim = new DIOSim(Constants.DIO.kIntakeSwitch); - intakeSim = new DCMotorSim(DCMotor.getNEO(1), 1, 0.3); - intakeEncSim = RevEncoderSimWrapper.create(intakeMotor); - lowerSim = new SimTimeOfFlight(lowerSensor); - upperSim = new SimTimeOfFlight(upperSensor); - upperSim.setRange(400); - } - - @Override - public void simulationPeriodic() { - double intakeVolts = MathUtil.clamp(intakeMotor.get() * 12, -12, 12); - intakeSim.setInput(intakeVolts); - Robot.objSim.setIntake(intakeDebounceSim.calculate(intakeMotor.get() < 0)); - - intakeSim.update(Constants.ksimDtSec); - if (Robot.objSim.hasObject()) { - lowerSim.setRange(20); - } else { - lowerSim.setRange(400); - } - intakeSwitchSim.setValue(true); // TODO placeholder. - intakeEncSim.setDistance(intakeSim.getAngularPositionRotations()); - intakeEncSim.setVelocity(intakeSim.getAngularVelocityRPM()); - - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/AmpShotCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/AmpShotCommand.java deleted file mode 100644 index 3c4905d8..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/AmpShotCommand.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.intake.commands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.wpilibj2.command.Command; - -public class AmpShotCommand extends Command { - - private IntakeSubsystem intakeSubsystem; - private ShooterSubsystem shooterSubsystem; - /** - * Runs the intake and the shooter to expell a note into the amp. Command Never - * ends - * - * @param intakeSubsystem - * @param shooterSubsystem - */ - public AmpShotCommand(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) { - this.intakeSubsystem = intakeSubsystem; - this.shooterSubsystem = shooterSubsystem; - - addRequirements(this.intakeSubsystem); - } - - @Override - public void initialize() { - this.intakeSubsystem.set(0); - } - - @Override - public void execute() { - this.intakeSubsystem.set(1); - this.shooterSubsystem.set(0.4 * 12, ControlType.VOLTAGE); - } - - @Override - public boolean isFinished() { - // return this.intakeSubsystem.hasNote(); - return false; - } - - @Override - public void end(boolean interrupted) { - this.intakeSubsystem.set(0); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/AutoIntakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/AutoIntakeCommand.java deleted file mode 100644 index c4476391..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/AutoIntakeCommand.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.intake.commands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.wpilibj2.command.Command; - -public class AutoIntakeCommand extends Command { - - private IntakeSubsystem intakeSubsystem; - private ShooterSubsystem shooterSubsystem; - - private double speed; - - /** - * Runs the intake at full speed while running the shooter backwards. Ends once - * the intake has a note - * - * @param speed - * @param intakeSubsystem - * @param shooterSubsystem - */ - public AutoIntakeCommand(double speed, IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) { - this.speed = speed; - this.intakeSubsystem = intakeSubsystem; - this.shooterSubsystem = shooterSubsystem; - - addRequirements(this.intakeSubsystem); - } - - @Override - public void initialize() { - this.intakeSubsystem.set(0); - } - - @Override - public void execute() { - this.intakeSubsystem.set(this.speed); - this.shooterSubsystem.set(-.05, ControlType.VOLTAGE); - } - - @Override - public boolean isFinished() { - return this.intakeSubsystem.hasNote(); - // return false; - } - - @Override - public void end(boolean interrupted) { - this.intakeSubsystem.set(0); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/DefaultIntakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/DefaultIntakeCommand.java deleted file mode 100644 index 0a3b07eb..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/DefaultIntakeCommand.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.intake.commands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.wpilibj2.command.Command; - -public class DefaultIntakeCommand extends Command { - private IntakeSubsystem intakeSubsystem; - private ShooterSubsystem shooterSubsystem; - - private boolean isDefaultIntakeRunning = false; - - public DefaultIntakeCommand(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) { - this.intakeSubsystem = intakeSubsystem; - this.shooterSubsystem = shooterSubsystem; - - addRequirements(intakeSubsystem); - } - - // @Override - // public void initialize() {} - - @Override - public void end(boolean interrupted) { - this.isDefaultIntakeRunning = false; - intakeSubsystem.set(0); - } - - @Override - public void execute() { - boolean hasNote = this.intakeSubsystem.hasNote(); - boolean noteTooHigh = this.intakeSubsystem.noteTooHigh(); - - // boolean lowSwitchPressed = lowSwitch.isPressed(); - // boolean noteTooHigh = highSwitch.isPressed(); - // boolean hasNote = lowSwitch.isPressed(); - - if (noteTooHigh) { - this.isDefaultIntakeRunning = true; - intakeSubsystem.set(-0.1); - this.shooterSubsystem.set(-1.3, ControlType.VOLTAGE); - } else { - intakeSubsystem.set(0); - this.shooterSubsystem.set(0, ControlType.VOLTAGE); - } - } - - public boolean isDefaultIntakeRunning() { - return this.isDefaultIntakeRunning; - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/ExtakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/ExtakeCommand.java deleted file mode 100644 index 1154c8c2..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/ExtakeCommand.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.intake.commands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ExtakeCommand extends Command { - - private IntakeSubsystem intakeSubsystem; - - private double speed; - - public ExtakeCommand(IntakeSubsystem intakeSubsystem, double speed) { - this.speed = speed; - this.intakeSubsystem = intakeSubsystem; - - addRequirements(this.intakeSubsystem); - } - - @Override - public void initialize() { - this.intakeSubsystem.set(0); - } - - @Override - public void execute() { - this.intakeSubsystem.set(-this.speed); - } - - @Override - public boolean isFinished() { - return !this.intakeSubsystem.lowSwitch().isPressed() && !this.intakeSubsystem.highSwitch().isPressed(); - } - - @Override - public void end(boolean interrupted) { - this.intakeSubsystem.set(0); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/IntakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/IntakeCommand.java deleted file mode 100644 index 033f2e36..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/IntakeCommand.java +++ /dev/null @@ -1,57 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.intake.commands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeCommand extends Command { - - private final IntakeSubsystem intakeSubsystem; - private final ShooterSubsystem shooterSubsystem; - - private double speed; - /** - * Blindly Intakes while running the shooter motor backwards. Command never ends - * - * @param speed - * @param intakeSubsystem - * @param shooterSubsystem - */ - - public IntakeCommand(double speed, IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) { // Fixme: - // add - // requirements - // for - // shooter! - this.speed = speed; - this.intakeSubsystem = intakeSubsystem; - this.shooterSubsystem = shooterSubsystem; - - addRequirements(this.intakeSubsystem); - } - - @Override - public void initialize() { - this.intakeSubsystem.set(0); - } - - @Override - public void execute() { - this.intakeSubsystem.set(this.speed); - this.shooterSubsystem.set(-1, ControlType.VOLTAGE); - - } - - @Override - public boolean isFinished() { - // return this.intakeSubsystem.hasNote(); - return false; - } - - @Override - public void end(boolean interrupted) { - this.intakeSubsystem.set(0); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/IntakeFireCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/IntakeFireCommand.java deleted file mode 100644 index 1909bdcf..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/IntakeFireCommand.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.intake.commands; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; - -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeFireCommand extends Command { - - private IntakeSubsystem intakeSubsystem; - - private double speed; - - public IntakeFireCommand(double speed, IntakeSubsystem intakeSubsystem) { - this.speed = speed; - this.intakeSubsystem = intakeSubsystem; - - addRequirements(this.intakeSubsystem); - } - - @Override - public void initialize() { - this.intakeSubsystem.set(0); - } - - @Override - public void execute() { - this.intakeSubsystem.set(this.speed); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - this.intakeSubsystem.set(0); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/LimitSwitchesPlanning.txt b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/LimitSwitchesPlanning.txt deleted file mode 100644 index ba6e3a67..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/LimitSwitchesPlanning.txt +++ /dev/null @@ -1,44 +0,0 @@ - -Default command -False(Lower), False(Upper) // Nothing is in the intake -Nothing - - - -True, False // Note is loaded before high sensor -Nothing - - - -False, True // Jammed or currently shooting -Extake until true, false - - - -True, True // Note is loaded too high -Extake until true, false - - - -Intake command -False(Lower), False(Upper) // Nothing is in the intake -Continuously intake - - -True, False // Note is loaded before high sensor -Intake at low speed for set time - - -False, True // Jammed or currently shooting -Extake slowly until true, false - - -True, True // Note is loaded too high -Extake slowly until true, false - - - -//Extake stays the same - - - diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/ShintakeSubsystem.java similarity index 66% rename from src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java rename to src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/ShintakeSubsystem.java index 81eab85f..87e0d476 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/ShintakeSubsystem.java @@ -1,11 +1,14 @@ -package org.jmhsrobotics.frc2024.subsystems.shooter; +package org.jmhsrobotics.frc2024.subsystems.shintake; import org.jmhsrobotics.frc2024.Constants; import org.jmhsrobotics.frc2024.Robot; +import org.jmhsrobotics.frc2024.utils.SimTimeOfFlight; +import org.jmhsrobotics.frc2024.utils.SimableTimeOfFlight; import org.jmhsrobotics.warcore.rev.RevEncoderSimWrapper; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.playingwithfusion.TimeOfFlight.RangingMode; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -13,16 +16,21 @@ import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.DIOSim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj2.command.SubsystemBase; import monologue.Logged; -public class ShooterSubsystem extends SubsystemBase implements Logged { +public class ShintakeSubsystem extends SubsystemBase implements Logged { private CANSparkMax topFlywheel = new CANSparkMax(Constants.CAN.kShooterTopId, MotorType.kBrushless); - private CANSparkMax bottomFlywheel = new CANSparkMax(Constants.CAN.kShooterBottomId, MotorType.kBrushless);; + private CANSparkMax bottomFlywheel = new CANSparkMax(Constants.CAN.kShooterBottomId, MotorType.kBrushless); + private CANSparkMax intakeMotor = new CANSparkMax(Constants.CAN.kIntakeId, MotorType.kBrushless); + private RelativeEncoder topEncoder; private RelativeEncoder bottomEncoder; @@ -35,13 +43,15 @@ public class ShooterSubsystem extends SubsystemBase implements Logged { private SimpleMotorFeedforward upperFeedforward = new SimpleMotorFeedforward(0.10948, 0.019707, 0.0018555); private SimpleMotorFeedforward lowerFeedforward = new SimpleMotorFeedforward(0.10948, 0.019707, 0.0018555); + private SimableTimeOfFlight lowerSensor; + private SimableTimeOfFlight upperSensor; public enum ControlType { BANG_BANG, VOLTAGE, PID }; // private double speed; - public ShooterSubsystem() { + public ShintakeSubsystem() { // Initializes motor(s) this.bangBangController = new BangBangController(); this.bangBangController.setTolerance(200); @@ -51,6 +61,11 @@ public ShooterSubsystem() { this.upperPID.setTolerance(Units.rotationsPerMinuteToRadiansPerSecond(100)); // WARNING: this value is in Rad/s this.lowerPID.setTolerance(Units.rotationsPerMinuteToRadiansPerSecond(100)); // WARNING: this value is in Rad/s + this.lowerSensor = new SimableTimeOfFlight(1); + this.upperSensor = new SimableTimeOfFlight(0); + + this.lowerSensor.setRangingMode(RangingMode.Short, 24); + this.upperSensor.setRangingMode(RangingMode.Short, 24); initializeMotors(); if (RobotBase.isSimulation()) { initSim(); @@ -64,7 +79,7 @@ public ShooterSubsystem() { public void periodic() { switch (this.controlType) { case BANG_BANG : - double outPut = this.bangBangController.calculate(this.getRPM(), this.reference); + double outPut = this.bangBangController.calculate(this.getShooterRPM(), this.reference); this.topFlywheel.set(outPut); this.bottomFlywheel.set(outPut); break; @@ -90,45 +105,22 @@ public void periodic() { log("controlType", this.controlType.toString()); log("reference", this.reference); log("topFlywheelDutyCycle", topFlywheel.get()); - log("topflywheelSpeed", getRPM()); + log("topflywheelSpeed", getShooterRPM()); log("bottomflywheelSpeed", getBottomRRPM()); - log("AtGoal", this.atGoal()); - - } - - private void setVoltages(double topVoltage, double bottomVoltage) { - if (Robot.isSimulation()) { // TODO: Simplation Hack - this.topFlywheel.set(topVoltage / 12.0); - this.bottomFlywheel.set(bottomVoltage / 12.0); - } - this.topFlywheel.setVoltage(topVoltage); - this.bottomFlywheel.setVoltage(bottomVoltage); - } - - private double getBottomRRPM() { - if (Robot.isSimulation()) { - return simVelocity; - } - return bottomEncoder.getVelocity(); - } + log("AtGoal", this.isShooterAtGoal()); - private double getTopRRPM() { - if (Robot.isSimulation()) { - return simVelocity; - } - return topEncoder.getVelocity(); } - public double getRPM() { + public double getShooterRPM() { return getTopRRPM(); } - public void set(double goal, ControlType controlType) { + public void setShooterGoal(double goal, ControlType controlType) { this.reference = goal; this.controlType = controlType; } - public boolean atGoal() { + public boolean isShooterAtGoal() { if (controlType == ControlType.VOLTAGE) { return false; } else { @@ -136,6 +128,17 @@ public boolean atGoal() { } } + public boolean hasNote() { + return this.lowerSensor.getRange() < 270; + } + + public boolean noteTooHigh() { + return this.upperSensor.getRange() < 320; + } + + public boolean shooterAtGoal() { + return this.lowerPID.atSetpoint() || this.upperPID.atSetpoint(); + } private void initializeMotors() { // this.topFlywheel.restoreFactoryDefaults(); this.topFlywheel.setIdleMode(IdleMode.kCoast); @@ -153,16 +156,56 @@ private void initializeMotors() { this.bottomEncoder.setAverageDepth(2); this.bottomEncoder.setMeasurementPeriod(16); - // this.bottomFlywheel.follow(topFlywheel); + intakeMotor.setInverted(true); + intakeMotor.setIdleMode(IdleMode.kBrake); + } + + private void setVoltages(double topVoltage, double bottomVoltage) { + if (Robot.isSimulation()) { // TODO: Simplation Hack + this.topFlywheel.set(topVoltage / 12.0); + this.bottomFlywheel.set(bottomVoltage / 12.0); + } + this.topFlywheel.setVoltage(topVoltage); + this.bottomFlywheel.setVoltage(bottomVoltage); + } + + private double getBottomRRPM() { + if (Robot.isSimulation()) { + return simVelocity; + } + return bottomEncoder.getVelocity(); + } + + public void setIntakeSpeed(double speed) { + intakeMotor.set(-speed); + } + + private double getTopRRPM() { + if (Robot.isSimulation()) { + return simVelocity; + } + return topEncoder.getVelocity(); } private FlywheelSim flywheelSim; private RevEncoderSimWrapper encSim; + private DIOSim intakeSwitchSim; + private DCMotorSim intakeSim; + private RevEncoderSimWrapper intakeEncSim; + private SimTimeOfFlight lowerSim; + private SimTimeOfFlight upperSim; + private Debouncer intakeDebounceSim = new Debouncer(0.2); private double simVelocity = 0; public void initSim() { flywheelSim = new FlywheelSim(DCMotor.getNEO(1), 1, 0.002); encSim = RevEncoderSimWrapper.create(topFlywheel); + intakeSwitchSim = new DIOSim(Constants.DIO.kIntakeSwitch); + intakeSim = new DCMotorSim(DCMotor.getNEO(1), 1, 0.3); + intakeEncSim = RevEncoderSimWrapper.create(intakeMotor); + lowerSim = new SimTimeOfFlight(lowerSensor); + upperSim = new SimTimeOfFlight(upperSensor); + upperSim.setRange(400); } @Override @@ -172,9 +215,24 @@ public void simulationPeriodic() { flywheelSim.setInputVoltage(motorVolts); flywheelSim.update(Constants.ksimDtSec); simVelocity = flywheelSim.getAngularVelocityRPM(); + double intakeVolts = MathUtil.clamp(intakeMotor.get() * 12, -12, 12); + intakeSim.setInput(intakeVolts); + Robot.objSim.setIntake(intakeDebounceSim.calculate(intakeMotor.get() < 0)); + intakeSim.update(Constants.ksimDtSec); + if (simVelocity > 1000) { Robot.objSim.fire(); + } else { + encSim.setVelocity(simVelocity); } - encSim.setVelocity(simVelocity); + if (Robot.objSim.hasObject()) { + lowerSim.setRange(20); + } else { + lowerSim.setRange(400); + } + intakeSwitchSim.setValue(true); // TODO placeholder. + intakeEncSim.setDistance(intakeSim.getAngularPositionRotations()); + intakeEncSim.setVelocity(intakeSim.getAngularVelocityRPM()); + } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/DefaultShintakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/DefaultShintakeCommand.java new file mode 100644 index 00000000..5e899388 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/DefaultShintakeCommand.java @@ -0,0 +1,42 @@ +package org.jmhsrobotics.frc2024.subsystems.shintake.commands; + +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem.ControlType; + +import edu.wpi.first.wpilibj2.command.Command; + +public class DefaultShintakeCommand extends Command { + private ShintakeSubsystem shintakeSubsystem; + + public DefaultShintakeCommand(ShintakeSubsystem shintakeSubsystem) { + this.shintakeSubsystem = shintakeSubsystem; + + addRequirements(this.shintakeSubsystem); + } + + // @Override + // public void initialize() {} + + @Override + public void end(boolean interrupted) { + this.shintakeSubsystem.setIntakeSpeed(0); + } + + @Override + public void execute() { + boolean hasNote = this.shintakeSubsystem.hasNote(); + boolean noteTooHigh = this.shintakeSubsystem.noteTooHigh(); + + // boolean lowSwitchPressed = lowSwitch.isPressed(); + // boolean noteTooHigh = highSwitch.isPressed(); + // boolean hasNote = lowSwitch.isPressed(); + + if (noteTooHigh) { + shintakeSubsystem.setIntakeSpeed(-0.1); + this.shintakeSubsystem.setShooterGoal(-1.3, ControlType.VOLTAGE); + } else { + shintakeSubsystem.setIntakeSpeed(0); + this.shintakeSubsystem.setShooterGoal(0, ControlType.VOLTAGE); + } + } +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/IntakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/IntakeCommand.java new file mode 100644 index 00000000..04ac5619 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/IntakeCommand.java @@ -0,0 +1,57 @@ +package org.jmhsrobotics.frc2024.subsystems.shintake.commands; + +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem.ControlType; + +import edu.wpi.first.wpilibj2.command.Command; + +public class IntakeCommand extends Command { + + private final ShintakeSubsystem shintakeSubsystem; + + private double speed; + private boolean isFiring = false; + /** + * Blindly Intakes while running the shooter motor backwards. Command never ends + * + * @param speed + * @param shitnakeSubsystem + */ + + public IntakeCommand(double speed, boolean isFiring, ShintakeSubsystem shintakeSubsystem) { // Fixme: + // add + // requirements + // for + // shooter! + this.speed = speed; + this.shintakeSubsystem = shintakeSubsystem; + this.isFiring = isFiring; + + addRequirements(this.shintakeSubsystem); + } + + @Override + public void initialize() { + this.shintakeSubsystem.setIntakeSpeed(0); + } + + @Override + public void execute() { + if (this.isFiring) { + this.shintakeSubsystem.setIntakeSpeed(this.speed); + } else { + this.shintakeSubsystem.setIntakeSpeed(this.speed); + this.shintakeSubsystem.setShooterGoal(-1, ControlType.VOLTAGE); + } + } + @Override + public boolean isFinished() { + // return this.intakeSubsystem.hasNote(); + return false; + } + + @Override + public void end(boolean interrupted) { + this.shintakeSubsystem.setIntakeSpeed(0); + } +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/ShooterAutoCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/ShooterAutoCommand.java new file mode 100644 index 00000000..9d854e0f --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shintake/commands/ShooterAutoCommand.java @@ -0,0 +1,40 @@ +package org.jmhsrobotics.frc2024.subsystems.shintake.commands; + +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shintake.ShintakeSubsystem.ControlType; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ShooterAutoCommand extends Command { + private ShintakeSubsystem shintakeSubsystem; + private double targetRPM; + + /** + * Spins up shooter to the target rpm. Command never ends, When command is + * exited, spin wheels back to zero rpm + * + * @param shooterSubsystem + * @param targetRPM + */ + public ShooterAutoCommand(ShintakeSubsystem shintakeSubsystem, double targetRPM) { + this.shintakeSubsystem = shintakeSubsystem; + this.targetRPM = targetRPM; + } + + @Override + public void execute() { + this.shintakeSubsystem.setShooterGoal(this.targetRPM, ControlType.VOLTAGE); + } + + @Override + public boolean isFinished() { + return this.shintakeSubsystem.shooterAtGoal(); + // return false; + + } + + @Override + public void end(boolean interrupted) { + this.shintakeSubsystem.setShooterGoal(0, ControlType.PID); + } +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterAutoCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterAutoCommand.java deleted file mode 100644 index 70a383fe..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterAutoCommand.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.shooter.commands; - -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ShooterAutoCommand extends Command { - private ShooterSubsystem shooterSubsystem; - private double targetRPM; - - /** - * Spins up shooter to the target rpm. Command never ends, When command is - * exited, spin wheels back to zero rpm - * - * @param shooterSubsystem - * @param targetRPM - */ - public ShooterAutoCommand(ShooterSubsystem shooterSubsystem, double targetRPM) { - this.shooterSubsystem = shooterSubsystem; - this.targetRPM = targetRPM; - } - - @Override - public void execute() { - this.shooterSubsystem.set(this.targetRPM, ControlType.PID); - } - - @Override - public boolean isFinished() { - // return this.shooterSubsystem.atGoal(); - return false; - - } - - @Override - public void end(boolean interrupted) { - this.shooterSubsystem.set(0, ControlType.PID); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NFireAmp.java b/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NFireAmp.java deleted file mode 100644 index 281d2910..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NFireAmp.java +++ /dev/null @@ -1,51 +0,0 @@ -package org.jmhsrobotics.frc2024.utils.newcmd; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj2.command.Command; - -public class NFireAmp extends Command { - ShooterSubsystem shooter; - IntakeSubsystem intake; - Debouncer sensorDebounce = new Debouncer(0.5); - - /** - * Fires a note into the amp. Runs shooter and flywheel. End when both hasnote - * and note too high are false. - * - * @param shooter - * @param intake - */ - public NFireAmp(ShooterSubsystem shooter, IntakeSubsystem intake) { - this.shooter = shooter; - this.intake = intake; - addRequirements(shooter, intake); - } - - @Override - public boolean isFinished() { - return sensorDebounce.calculate(!this.intake.hasNote() && !this.intake.noteTooHigh()); - } - - @Override - public void initialize() { - shooter.set(12, ControlType.VOLTAGE); - intake.set(1); - } - - @Override - public void execute() { - shooter.set(12, ControlType.VOLTAGE); - intake.set(1); - } - - @Override - public void end(boolean interrupted) { - shooter.set(0, ControlType.VOLTAGE); - intake.set(0); - } - -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NSpinupAndShoot.java b/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NSpinupAndShoot.java deleted file mode 100644 index 59556a96..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NSpinupAndShoot.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.jmhsrobotics.frc2024.utils.newcmd; - -import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj2.command.Command; - -public class NSpinupAndShoot extends Command { - - private ShooterSubsystem shooter; - private IntakeSubsystem intake; - private double targetRPM; - private boolean shouldFire = false; - private Debouncer noteExitDebouncer = new Debouncer(0.1); - - /** - * Spins up Shooter Flywheel to target RPM and then runs intake to shoot a note - * into the speaker. Ends when both has note and note too high are false - * - * @param shooter - * @param intake - * @param rpm - */ - public NSpinupAndShoot(ShooterSubsystem shooter, IntakeSubsystem intake, double rpm) { - this.shooter = shooter; - this.intake = intake; - this.targetRPM = rpm; - addRequirements(shooter, intake); - } - - @Override - public void initialize() { - this.shouldFire = false; - this.shooter.set(targetRPM, ControlType.PID); - } - - @Override - public void execute() { - if (this.shooter.atGoal()) { // TODO: Do we need debouncing? - shouldFire = true; - } - - intake.set(shouldFire ? 1 : 0); - } - - @Override - public boolean isFinished() { - return shouldFire && noteExitDebouncer.calculate(!this.intake.hasNote() && !this.intake.noteTooHigh()); - } - - @Override - public void end(boolean interrupted) { - intake.set(0); - shooter.set(0, ControlType.PID); - } - -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NSpinupNoStop.java b/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NSpinupNoStop.java deleted file mode 100644 index 1bce2fe1..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/utils/newcmd/NSpinupNoStop.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.jmhsrobotics.frc2024.utils.newcmd; - -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem.ControlType; - -import edu.wpi.first.wpilibj2.command.Command; - -public class NSpinupNoStop extends Command { - private ShooterSubsystem shooter; - private double targetRPM; - - /** - * Spins flywheel up to target RPM. Never ends. - * - * @param shooter - * @param rpm - */ - public NSpinupNoStop(ShooterSubsystem shooter, double rpm) { - this.shooter = shooter; - this.targetRPM = rpm; - addRequirements(shooter); - } - @Override - public void initialize() { - shooter.set(targetRPM, ControlType.PID); - } - @Override - public boolean isFinished() { - return false; - } - - @Override - public void execute() { - this.shooter.set(targetRPM, ControlType.PID); - } - - @Override - public void end(boolean interrupted) { - shooter.set(0, ControlType.PID); - } - -}