Post season

src/main/java/frc/robot/Constants.java

@@ -7,8 +7,8 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.util.Units;
+//import edu.wpi.first.wpilibj.PS4Controller.Button;
 import edu.wpi.first.wpilibj.XboxController.Button;
-
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
  * numerical or boolean
@@ -22,12 +22,25 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+  public class BlinkinConstants {
+
+    public static final int kBlinkinPort = 0;
+
+  }
 
   public static final class DriveConstants {
 
     // TODO turn field oriented on or off
     public static final boolean kFieldOriented = true;
 
+    //Preset drive angles
+    public static final double kSourceTheta = 60;
+    public static final double kSpeakerTheta = 180;
+
+    //Turn theta 
+    public static final double kTurnThetaMaxSpeed = 0.9;
+    public static final double kTurnThetaShutoffSensitivity = 0.005;
+
     // Distance between centers of right and left wheels on robot
     public static final double kTrackWidth = Units.inchesToMeters(24.75);
     // Distance between front and back wheels on robot
@@ -110,9 +123,9 @@ public static final class DriveConstants {
     public static final double kTeleMaxMetersPerSec = 0.3;
     public static final double kFastTeleMaxMetersPerSec = 1.0;
     public static final double kFasterTeleMaxMetersPerSec = 1.8;
-    public static final double kNudgeSpeed = 0.5;
+    public static final double kNudgeSpeed = 0.8;
 
-    public static final Button kTestMotorButton = Button.kLeftBumper;
+    //public static final Button kTestMotorButton = Button.kLeftBumper;
   }
 
   public static final class ModuleConstants {
@@ -144,18 +157,21 @@ public static final class ShooterConstants {
     public static final int kIndexMotorCanId = 53;
     public static final int kWinchMotorCanId = 54;
 
+    public static final double kShooterMaxSpeed = 1.0;
     public static final double kFrontShootPower = 1.0;
     public static final double kBackShootPower = -1.0;
     public static final double kFrontIntakePower = -0.35;
-    public static final double kBackIntakePower = -0.30;
-    public static final double kIndexIntakePower = 0.1;
-    public static final double kIndexPower = -1.0;
+    public static final double kBackIntakePower = 0.35;
+
     public static final double kWinchUpPower = 0.5;
     public static final double kWinchDownPower = -1.0;
-    
+
     public static final double kWinchDeadBand = 0.05;
     public static final double kWinchUpPreset = 0;
-    public static final double kWinchDownPreset = 1.5;
+    public static final double kWinchDownPreset = 1;//was 1.5
+
+    public static final int kShootDelay = 500; // 1/2 second
+    public static final int kSpunUpSpeed = 6000;
   }
 
   public static final class ClimberConstants {

src/main/java/frc/robot/RobotContainer.java

@@ -3,41 +3,46 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
-import java.util.function.Supplier;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.PS4Controller;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.PS4Controller.Button;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import frc.robot.Constants.ClimberConstants;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.OIConstants;
 import frc.robot.Constants.ShooterConstants;
 import frc.robot.commands.SwerveDriveCommand;
+import frc.robot.commands.TurnToThetaCommand;
 import frc.robot.commands.WinchCommand;
 import frc.robot.commands.WinchPresetCommand;
 import frc.robot.commands.ZeroHeadingCommand;
 import frc.robot.commands.auto.DriveAutoCommand;
 import frc.robot.commands.auto.ShootAndDriveAutoCommand;
 import frc.robot.commands.auto.ShootAutoCommand;
 import frc.robot.commands.auto.TurnAndShootAutoCommand;
+import frc.robot.commands.auto.TwoRingAutoCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
 import frc.robot.commands.AirIntakeCommand;
 import frc.robot.commands.ClimbCommand;
+import frc.robot.commands.DropFlipperCommand;
 import frc.robot.commands.ExpelRingCommand;
 import frc.robot.commands.FeedShooterCommand;
+import frc.robot.commands.GroundExpelCommand;
 import frc.robot.commands.GroundIntakeCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.ResetWinchCommand;
 import frc.robot.commands.RetractIntakeCommand;
 import frc.robot.commands.ShootCommand;
+import frc.robot.commands.ShooterCommand;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 
@@ -48,16 +53,13 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-
   // The driver's controller
-  XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
-  XboxController m_auxController = new XboxController(OIConstants.kSecondControllerPort);
+  PS4Controller m_driverController = new PS4Controller(OIConstants.kDriverControllerPort),
+  m_auxController = new PS4Controller(OIConstants.kSecondControllerPort);
 
   // Subsystems
   private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
   private final SwerveSubsystem m_robotDrive = new SwerveSubsystem();
-  private boolean fastMode = false;
-  private boolean fasterMode = false;
   private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
 
@@ -66,6 +68,7 @@ public class RobotContainer {
   private SendableChooser<Command> m_chosenAuto = new SendableChooser<>();
   private GenericEntry m_delay;
 
+
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
@@ -76,34 +79,39 @@ public RobotContainer() {
             () -> getLeftY(),
             () -> getRightX(),
             DriveConstants.kFieldOriented,
-            this::getFastMode,
+            this::getSlowMode,
             this::getFasterMode,
-            this::getPOV));
+            this::getPOV,
+            this::getAuxLTrig,
+            this::getAuxRTrig
+            ));
     m_climberSubsystem.setDefaultCommand(new ClimbCommand(m_climberSubsystem, this::getAuxRightY, this::getAuxLeftY));
     m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_shooterSubsystem, this::POVToWinchSpeed));
     m_intakeSubsystem.setDefaultCommand(new RetractIntakeCommand(m_intakeSubsystem));
 
-    // Auto chooser setup
+    // Auto chooser setup (sqeak!)
     m_tab = Shuffleboard.getTab("Auto");
 
     m_delay = m_tab.add("Delay", 0).getEntry();
 
     m_chosenAuto.setDefaultOption("Shoot and drive from middle",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //used to be an empty Pose2D (should not change but maybe the empty pose has something to do with it)
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay));
     m_chosenAuto.addOption("Shoot and drive from left",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), m_delay.getDouble(0)));
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), this::getDelay));
     m_chosenAuto.addOption("Shoot and drive from right",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), m_delay.getDouble(0)));
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), this::getDelay));
     m_chosenAuto.addOption("Drive, turn, and shoot from left",
-      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 55, m_delay.getDouble(0)));
+      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), 55, this::getDelay));
     m_chosenAuto.addOption("Drive, turn, and shoot from right",
-      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55, m_delay.getDouble(0)));
+      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), -55, this::getDelay));
     m_chosenAuto.addOption("Drive back only",
-      new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //drive back only auto
+      new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), this::getDelay));
     m_chosenAuto.addOption("Shoot only",
-      new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), m_delay.getDouble(0)).withTimeout(4));
+      new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay).withTimeout(4));
+    m_chosenAuto.addOption("2 rings",
+      new TwoRingAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay));
 
-    m_tab.add(m_chosenAuto);
+    m_tab.add("Auto", m_chosenAuto);
 
     // Configure the button bindings
     configureButtonBindings();
@@ -119,69 +127,86 @@ public void robotInit() {
    * created by
    * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its
    * subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling
+   * edu.wpi.first.wpilibj.Joystick} or {@link PS4Controller}), and then calling
    * passing it to a
    * {@link JoystickButton}.
    */
   private void configureButtonBindings() {
 
     // Driver stuff
-    new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value) // Reset gyro
-      .whileTrue(new ZeroHeadingCommand(m_robotDrive));
-    new JoystickButton(m_driverController, XboxController.Button.kA.value)
+
+    new JoystickButton(m_driverController, Button.kR1.value) //drops flipper while intaking with automatic retraction
       .whileTrue(new GroundIntakeCommand(m_intakeSubsystem));
-    new JoystickButton(m_driverController, XboxController.Button.kB.value)
+    new JoystickButton(m_driverController, Button.kL1.value) // Expels the ring on the ground (Max wanted this) i see
+      .whileTrue(new GroundExpelCommand(m_intakeSubsystem));
+    new JoystickButton(m_driverController, Button.kCircle.value)
       .whileTrue(new ExpelRingCommand(m_intakeSubsystem));
-    new JoystickButton(m_driverController, XboxController.Button.kX.value)
-      .whileTrue(new FeedShooterCommand(m_intakeSubsystem));
-    new JoystickButton(m_driverController, XboxController.Button.kY.value)
-      .whileTrue(new AirIntakeCommand(m_intakeSubsystem));
-      // Left bumper = Toggle fastmode
-      // Left trigger = Toggle fastermode
+
+    // new JoystickButton(m_auxController, Button.kL2.value)///speaker
+    //   .onTrue(new TurnToThetaCommand(m_robotDrive, () -> getLeftX(),
+    //         () -> getLeftY(),
+    //         this::getSlowMode,
+    //         this::getFasterMode, 
+    //         DriveConstants.kSpeakerTheta));
+            //.withTimeout(1.5));
+    // new JoystickButton(m_auxController, Button.kR2.value)//source
+    //   .onTrue(new TurnToThetaCommand(m_robotDrive, () -> getLeftX(),
+    //         () -> getLeftY(),
+    //         this::getSlowMode,
+    //         this::getFasterMode, 
+    //         DriverStation.getAlliance().get().compareTo(Alliance.Red) == 0 ? DriveConstants.kSourceTheta : DriveConstants.kSourceTheta + 180));
+    //         //.alongWith(new WinchPresetCommand(m_shooterSubsystem, 1.2)).withTimeout(1.5));
+
+      // Right trigger = Slowmode
+      // Left trigger = Fastermode
       // POV = Nudge
       // Right joystick = Move
       // Left joystick = Turn
 
     // Auxillary stuff
-    new JoystickButton(m_auxController, XboxController.Button.kRightBumper.value) // Shoot
-      .whileTrue(new ShootCommand(m_shooterSubsystem));
-    new JoystickButton(m_auxController, XboxController.Button.kLeftBumper.value) // Intake
-      .whileTrue(new IntakeCommand(m_shooterSubsystem));
-    new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up
-      .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower));
-    new JoystickButton(m_auxController, XboxController.Button.kA.value) // Climber down
-      .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower));
-    new JoystickButton(m_auxController, XboxController.Button.kY.value) // Winch up preset
+    new JoystickButton(m_auxController, Button.kR1.value) // Shoot
+      .whileTrue(new ShootCommand(m_shooterSubsystem, m_intakeSubsystem));
+    new JoystickButton(m_auxController, Button.kL1.value) // Intake
+      .whileTrue(new IntakeCommand(m_shooterSubsystem))
+      .whileTrue(new AirIntakeCommand(m_intakeSubsystem));
+    new JoystickButton(m_auxController, Button.kTriangle.value) // Winch up preset
       .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchUpPreset));
-    new JoystickButton(m_auxController, XboxController.Button.kX.value) // Winch down preset
+    new JoystickButton(m_auxController, Button.kSquare.value) // Winch down preset
       .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchDownPreset));
-    new JoystickButton(m_auxController, XboxController.Button.kBack.value) // Reset winch encoder
-      .whileTrue(new ResetWinchCommand(m_shooterSubsystem));
+    // new JoystickButton(m_auxController, Button.kOptions.value) // Reset winch encoder
+      // .whileTrue(new ResetWinchCommand(m_shooterSubsystem));
+    new JoystickButton(m_auxController, Button.kShare.value) // Reset gyro
+      .whileTrue(new ZeroHeadingCommand(m_robotDrive));
+    new JoystickButton(m_auxController, Button.kCross.value)
+      .whileTrue(new AirIntakeCommand(m_intakeSubsystem));
+    new JoystickButton(m_auxController, Button.kCircle.value)
+      .whileTrue(new ExpelRingCommand(m_intakeSubsystem));
       // POV = Winch
       // Joysticks = Manual climbers
   }
 
-  boolean getFastMode() {
-    if (m_driverController.getLeftBumperPressed()) {
-      fastMode = !fastMode;
-    }
-    return fastMode;
+  boolean getFasterMode() {
+    return m_driverController.getL2Button();
   }
 
-  boolean getFasterMode() {
-    if (m_driverController.getLeftTriggerAxis() > OIConstants.kTriggerDeadband) {
-      fasterMode = true;
-    }
-    else fasterMode = false;
-    return fasterMode;
+  boolean getSlowMode() {
+    return !m_driverController.getR2Button();
   }
+
+  double getDelay() {
+    return m_delay.getDouble(0);
+  }
+
   double getRightX() {return m_driverController.getRightX();}
   double getLeftX() {return -m_driverController.getLeftX();}
   double getLeftY() {return -m_driverController.getLeftY();}
   double getPOV() {return m_driverController.getPOV();}
+  boolean getRightTrigger() {return m_driverController.getR2Button();}
   double getAuxRightY() {return Math.abs(m_auxController.getRightY()) > OIConstants.kDriveDeadband ? m_auxController.getRightY() : 0;}
   double getAuxLeftY() {return Math.abs(m_auxController.getLeftY()) > OIConstants.kDriveDeadband ? m_auxController.getLeftY() : 0;}
   double getAuxPOV() {return m_auxController.getPOV();}
+  double getAuxLTrig(){return m_auxController.getL2Axis();}
+  double getAuxRTrig(){return m_auxController.getR2Axis();}
   double POVToWinchSpeed() {
     return getAuxPOV() == 0 ? ShooterConstants.kWinchUpPower : (getAuxPOV() == 180 ? ShooterConstants.kWinchDownPower : 0);
   }

src/main/java/frc/robot/commands/DropFlipperCommand.java

@@ -0,0 +1,36 @@
+// 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class DropFlipperCommand extends Command {
+  private final IntakeSubsystem m_subsystem;
+
+  public DropFlipperCommand(IntakeSubsystem subsystem) {
+    m_subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_subsystem.dropFlipper();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_subsystem.stopEverything();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;//m_subsystem.m_intakeLimitSwitch.isPressed();
+    // return false;
+  }
+}

src/main/java/frc/robot/commands/GroundExpelCommand.java

@@ -0,0 +1,35 @@
+// 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class GroundExpelCommand extends Command {
+  private final IntakeSubsystem m_subsystem;
+
+  public GroundExpelCommand(IntakeSubsystem subsystem) {
+    m_subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_subsystem.expelRingGround();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_subsystem.stopEverything();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}