From 2310f1eae0f021935a9b1f637f3323a2c9360228 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Wed, 21 Feb 2024 20:27:48 -0700 Subject: [PATCH] Almost Ready for Florida!! (im geekin) --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 85 +++++++++++-------- .../robot/commands/Autos/ArmIntakeInAuto.java | 31 +++++++ .../robot/commands/Intake/ArmIntakeIn.java | 1 + .../commands/Swerve/JoystickPlayback.java | 8 +- .../commands/Swerve/JoystickRecorder.java | 58 +------------ 6 files changed, 90 insertions(+), 95 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7e099c0..848edae 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -168,7 +168,7 @@ public final class Constants { public static final class ShooterConstants { public static final int LEFT_SHOOTER_ID = 15; // final public static final int RIGHT_SHOOTER_ID = 16; // final - public static final double SHOOTER_SPEED = 0.35; // final + public static final double SHOOTER_SPEED = 0.40; // final public static final double SHOOTER_IDLE = 0.35; // final public static final double SHOOTER_IDLE_LIMELIGHT = 0.8; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 585e3c3..ad3a927 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -25,6 +26,7 @@ import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; import frc4388.robot.commands.Intake.ArmIntakeIn; +import frc4388.robot.commands.Autos.ArmIntakeInAuto; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; @@ -76,8 +78,8 @@ public class RobotContainer { private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.talonPIDIn()) - //new InstantCommand(() -> m_robotShooter.spin()) + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin()) ); // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( @@ -112,9 +114,9 @@ public class RobotContainer { ); private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) - // new WaitCommand(0.75), - //new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(0.75), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) ); private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( @@ -126,6 +128,7 @@ public class RobotContainer { private Command taxi = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt"); private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); + private Command interrupt = new InstantCommand(() -> {}, m_robotSwerveDrive, m_robotIntake, m_robotShooter); private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), @@ -155,6 +158,7 @@ public class RobotContainer { ejectToShoot.asProxy(), taxi.asProxy() ); + private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new WaitCommand(1).asProxy(), @@ -162,26 +166,32 @@ public class RobotContainer { new WaitCommand(1).asProxy(), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - intakeToShootStuff.asProxy(), - new WaitCommand(1).asProxy(), - new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt1.txt"), - intakeToShoot.asProxy(), - new WaitCommand(1).asProxy(), - new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"), - new WaitCommand(0.5).asProxy(), + new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), + new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1).asProxy(), + new WaitCommand(1.4).asProxy(), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(1).asProxy(), + new WaitCommand(0.5), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") + // new WaitCommand(1).asProxy(), + // new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"), + // new WaitCommand(0.5).asProxy(), + // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + // new WaitCommand(1).asProxy(), + // new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + // new WaitCommand(1).asProxy(), + // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) .addOption("Taxi Auto", taxi.asProxy()) .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker) .addOption("One Note Auto Starting in Front of Speaker, But Stay", oneNoteStartingSpeakerStationary) - .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft) - .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight) + // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft) + // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight) + .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker) .buildDisplay(); @@ -222,20 +232,18 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); /* Auto Recording */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .whileTrue(new JoystickRecorder(m_robotSwerveDrive, m_robotShooter, m_robotIntake, - () -> getDeadbandedDriverController().getLeftX(), - () -> getDeadbandedDriverController().getLeftY(), - () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY(), - () -> getDeadbandedOperatorController().getLeftBumper(), - () -> getDeadbandedOperatorController().getRightBumper(), - "Taxi.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Taxi.txt")) + // .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + // .onFalse(new InstantCommand()); // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, @@ -251,9 +259,9 @@ public class RobotContainer { // .onFalse(new InstantCommand()); // /* Speed */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); @@ -301,6 +309,11 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake)) + .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); + + } private void configureVirtualButtonBindings() { @@ -347,9 +360,9 @@ public class RobotContainer { .onFalse(turnOffShoot); - new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(i) - .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); + // new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(i) + // .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java new file mode 100644 index 0000000..40bc938 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Autos; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc4388.robot.commands.Intake.ArmIntakeIn; +import frc4388.robot.commands.Swerve.JoystickPlayback; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.SwerveDrive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ArmIntakeInAuto extends ParallelCommandGroup { + private final Intake intake; + private final Shooter shooter; + private final SwerveDrive swerve; + /** Creates a new ArmIntakeInAuto. */ + public ArmIntakeInAuto(Intake intake, Shooter shooter, SwerveDrive swerve) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + this.intake = intake; + this.shooter = shooter; + this.swerve = swerve; + addCommands(new ArmIntakeIn(intake, shooter), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt"))); + } +} diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index fc08304..78c558b 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -7,6 +7,7 @@ package frc4388.robot.commands.Intake; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.SwerveDrive; public class ArmIntakeIn extends Command { /** Creates a new ArmIntakeIn. */ diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 4a91f7a..4a4d7d8 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -9,11 +9,11 @@ import java.io.FileNotFoundException; import java.util.ArrayList; import java.util.Scanner; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; -public class JoystickPlayback extends Command { +public class JoystickPlayback extends CommandBase { private final SwerveDrive swerve; private String filename; private int mult = 1; @@ -102,7 +102,7 @@ public class JoystickPlayback extends Command { return; } lastIndex = i; - } // Why is this done rather than using the variable counter + } TimedOutput lastOut = outputs.get(lastIndex - 1); TimedOutput out = outputs.get(lastIndex); @@ -138,4 +138,4 @@ public class JoystickPlayback extends Command { public boolean isFinished() { return m_finished; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index c4c527b..7c83c58 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -11,67 +11,34 @@ import java.util.ArrayList; import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.Intake.ArmIntakeIn; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.Shooter; +import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; -public class JoystickRecorder extends Command { +public class JoystickRecorder extends CommandBase { public final SwerveDrive swerve; - public final Shooter m_robotShooter; - public final Intake m_robotIntake; public final Supplier leftX; public final Supplier leftY; public final Supplier rightX; public final Supplier rightY; - public final Supplier OPLB; - public final Supplier OPRB; - - private Command intakeToShootStuff; - private Command intakeToShoot; - private Command i; - - private boolean lastOPLB; - private boolean lastOPRB; - private String filename; public final ArrayList outputs = new ArrayList<>(); private long startTime = -1; /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Shooter m_robotShooter, Intake m_robotIntake, - Supplier leftX, Supplier leftY, + public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, Supplier rightX, Supplier rightY, - Supplier OPLB, Supplier OPRB, String filename) { this.swerve = swerve; - this.m_robotShooter = m_robotShooter; - this.m_robotIntake = m_robotIntake; - this.leftX = leftX; this.leftY = leftY; this.rightX = rightX; this.rightY = rightY; - this.OPLB = OPLB; - this.OPRB = OPRB; this.filename = filename; - intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); - intakeToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin()) - ); - i = new SequentialCommandGroup( - intakeToShootStuff, intakeToShoot - ); - addRequirements(this.swerve); } @@ -93,8 +60,6 @@ public class JoystickRecorder extends Command { inputs.leftY = leftY.get(); inputs.rightX = rightX.get(); inputs.rightY = rightY.get(); - inputs.OPLB = OPLB.get(); - inputs.OPRB = OPRB.get(); inputs.timedOffset = System.currentTimeMillis() - startTime; outputs.add(inputs); @@ -102,23 +67,8 @@ public class JoystickRecorder extends Command { swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), new Translation2d(inputs.rightX, inputs.rightY), true); - - if(lastOPLB != inputs.OPLB && inputs.OPLB == true){ - m_robotShooter.spin(); - m_robotIntake.talonHandoff(); - }else if(lastOPLB != inputs.OPLB && inputs.OPLB == false){ - - } - - if(lastOPRB != inputs.OPRB){ - m_robotShooter.spin(); - m_robotIntake.talonHandoff(); - } System.out.println("RECORDING"); - - lastOPLB = inputs.OPLB; - lastOPRB = inputs.OPRB; } // Called once the command ends or is interrupted. @@ -144,4 +94,4 @@ public class JoystickRecorder extends Command { public boolean isFinished() { return false; } -} +} \ No newline at end of file