diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6b9c00d..082c376 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -155,7 +155,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 = 1.0; // final + public static final double SHOOTER_SPEED = 0.35; // final public static final double SHOOTER_IDLE = 0.4; // final } @@ -164,7 +164,7 @@ public final class Constants { public static final int INTAKE_MOTOR_ID = 18; public static final int PIVOT_MOTOR_ID = 17; public static final double INTAKE_SPEED = 0.75; - public static final double INTAKE_OUT_SPEED_UNPRESSED = 0.7; + public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0; public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; public static final double HANDOFF_SPEED = 0.4; public static final double PIVOT_SPEED = 0.2; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e5e850a..d664129 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -38,7 +38,6 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - // CameraServer.startAutomaticCapture(); } /** @@ -84,20 +83,20 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { - // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // /* - // * String autoSelected = SmartDashboard.getString("Auto Selector", - // * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - // * = new MyAutoCommand(); break; case "Default Auto": default: - // * autonomousCommand = new ExampleCommand(); break; } - // */ + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ - // // schedule the autonomous command (example) - // if (m_autonomousCommand != null) { - // m_autonomousCommand.schedule(); - // } - // m_robotTime.startMatchTime(); + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + m_robotTime.startMatchTime(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fb14d47..51c0419 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,9 @@ package frc4388.robot; +import com.fasterxml.jackson.databind.ser.std.InetAddressSerializer; + +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; @@ -14,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; @@ -87,8 +91,22 @@ public class RobotContainer { private Command startRightMoveLeft = new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( - ejectToShoot.asProxy(), - taxi.asProxy() + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake), + new WaitCommand(1).asProxy(), + new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") + ); + private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup ( + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) ); private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( startLeftMoveRight.asProxy(), @@ -100,10 +118,31 @@ public class RobotContainer { ejectToShoot.asProxy(), taxi.asProxy() ); - + private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), 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 InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), 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) .buildDisplay(); @@ -117,6 +156,7 @@ public class RobotContainer { configureButtonBindings(); DriverStation.silenceJoystickConnectionWarning(true); + CameraServer.startAutomaticCapture(); /* Default Commands */ // drives the robot with a two-axis input from the driver controller @@ -145,26 +185,28 @@ public class RobotContainer { /* Auto Recording */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "IntenseTaxi.txt")) - // .onFalse(new InstantCommand()); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + () -> getDeadbandedDriverController().getLeftX(), + () -> getDeadbandedDriverController().getLeftY(), + () -> getDeadbandedDriverController().getRightX(), + () -> getDeadbandedDriverController().getRightY(), + () -> getDeadbandedOperatorController().getLeftBumper(), + () -> getDeadbandedOperatorController().getRightBumper(), + "TwoNotePrt1.txt")) + .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) .onFalse(new InstantCommand()); - /* Speed */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + // /* 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.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); @@ -178,6 +220,10 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); + // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index a270d7b..49ffcaf 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -42,6 +42,7 @@ public class PlaybackChooser { m_playback = m_choosers.get(0); nextChooser(); + // ! This does not work, why? Shuffleboard.getTab("Auto Chooser") .add("Add Sequence", new InstantCommand(() -> nextChooser())) .withPosition(4, 0); @@ -68,13 +69,12 @@ public class PlaybackChooser { String[] dirs = m_dir.list(); - if(dirs == null){ // Fix funny error - return; + if(dirs != null){ // Fix funny error + for (String auto : dirs) { + chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } } - for (String auto : dirs) { - chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); - } for (var cmd_name : m_commandPool.keySet()) { chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); } diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index 620dd86..56275b8 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -12,33 +12,66 @@ 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 frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; public class JoystickRecorder extends Command { 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, Supplier leftX, Supplier leftY, + public JoystickRecorder(SwerveDrive swerve, Shooter m_robotShooter, Intake m_robotIntake, + 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.pidIn()), + new InstantCommand(() -> m_robotShooter.spin()) + ); + i = new SequentialCommandGroup( + intakeToShootStuff, intakeToShoot + ); + addRequirements(this.swerve); } @@ -60,6 +93,8 @@ 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); @@ -67,8 +102,23 @@ 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.handoff(); + }else if(lastOPLB != inputs.OPLB && inputs.OPLB == false){ + + } + + if(lastOPRB != inputs.OPRB){ + m_robotShooter.spin(); + m_robotIntake.handoff(); + } System.out.println("RECORDING"); + + lastOPLB = inputs.OPLB; + lastOPRB = inputs.OPRB; } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 6339fb2..776dd98 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.ShooterConstants; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -20,7 +21,8 @@ public class Shooter extends SubsystemBase { private TalonFX leftShooter; private TalonFX rightShooter; - + + private double smartDashboardShooterSpeed; /** Creates a new Shooter. */ public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) { leftShooter = leftTalonFX; @@ -28,6 +30,8 @@ public class Shooter extends SubsystemBase { leftShooter.setNeutralMode(NeutralModeValue.Coast); rightShooter.setNeutralMode(NeutralModeValue.Coast); + SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + } public Shooter(TalonFX leftShooter) { @@ -44,11 +48,11 @@ public class Shooter extends SubsystemBase { } public void spin() { - spin(ShooterConstants.SHOOTER_SPEED); + spin(smartDashboardShooterSpeed); } public void spin(double speed) { - leftShooter.set(speed); + leftShooter.set(-speed); rightShooter.set(-speed); } @@ -70,6 +74,8 @@ public class Shooter extends SubsystemBase { // This method will be called once per scheduler run // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); + smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + } } diff --git a/src/main/java/frc4388/utility/Configurable/Configurable.java b/src/main/java/frc4388/utility/Configurable/Configurable.java new file mode 100644 index 0000000..d8c1c1d --- /dev/null +++ b/src/main/java/frc4388/utility/Configurable/Configurable.java @@ -0,0 +1,5 @@ +package frc4388.utility.Configurable; + +public abstract class Configurable { + +} diff --git a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java new file mode 100644 index 0000000..c1ea101 --- /dev/null +++ b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java @@ -0,0 +1,4 @@ +package frc4388.utility.Configurable; + +public class ConfigurableDouble { +} diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java index e8b10cc..ac414e2 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -6,6 +6,10 @@ public class UtilityStructs { public double leftY = 0.0; public double rightX = 0.0; public double rightY = 0.0; + + public boolean OPLB; + public boolean OPRB; + public long timedOffset = 0; }