Winchy squinchy subsystem

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

@@ -22,11 +22,20 @@
  * 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;
+
+    //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);
@@ -110,7 +119,7 @@
     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;
   }
@@ -144,18 +153,20 @@
     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 class WinchySquinchyConstants{
     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.75;//was 1.5
   }
 
   public static final class ClimberConstants {

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

@@ -17,6 +17,7 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.OIConstants;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.Constants.WinchySquinchyConstants;
 import frc.robot.commands.SwerveDriveCommand;
 import frc.robot.commands.WinchCommand;
 import frc.robot.commands.WinchPresetCommand;
@@ -25,10 +26,12 @@
 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.subsystems.WinchySubsystem;
 import frc.robot.commands.AirIntakeCommand;
 import frc.robot.commands.ClimbCommand;
 import frc.robot.commands.ExpelRingCommand;
@@ -38,6 +41,7 @@
 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;
 
@@ -51,10 +55,12 @@ public class RobotContainer {
 
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
-  XboxController m_auxController = new XboxController(OIConstants.kSecondControllerPort);
+  XboxController 
+  m_auxController = new XboxController(OIConstants.kSecondControllerPort);
 
   // Subsystems
   private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
+  private final WinchySubsystem m_winchySubsystem = new WinchySubsystem();
   private final SwerveSubsystem m_robotDrive = new SwerveSubsystem();
   private boolean fastMode = false;
   private boolean fasterMode = false;
@@ -80,30 +86,32 @@ public RobotContainer() {
             this::getFasterMode,
             this::getPOV));
     m_climberSubsystem.setDefaultCommand(new ClimbCommand(m_climberSubsystem, this::getAuxRightY, this::getAuxLeftY));
-    m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_shooterSubsystem, this::POVToWinchSpeed));
+    m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_winchySubsystem, 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();
@@ -126,16 +134,13 @@ public void robotInit() {
   private void configureButtonBindings() {
 
     // Driver stuff
-    new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value) // Reset gyro
+    new JoystickButton(m_driverController, XboxController.Button.kStart.value) // Reset gyro
       .whileTrue(new ZeroHeadingCommand(m_robotDrive));
-    new JoystickButton(m_driverController, XboxController.Button.kA.value)
+    new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value)
       .whileTrue(new GroundIntakeCommand(m_intakeSubsystem));
     new JoystickButton(m_driverController, XboxController.Button.kB.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
       // POV = Nudge
@@ -144,19 +149,25 @@ private void configureButtonBindings() {
 
     // Auxillary stuff
     new JoystickButton(m_auxController, XboxController.Button.kRightBumper.value) // Shoot
-      .whileTrue(new ShootCommand(m_shooterSubsystem));
+      .whileTrue(new ShootCommand(m_shooterSubsystem, m_intakeSubsystem));
     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));
+      .whileTrue(new IntakeCommand(m_shooterSubsystem))
+      .whileTrue(new AirIntakeCommand(m_intakeSubsystem));
+    // new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up
+    //   .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower));
+    // new JoystickButton(m_auxControllser, XboxController.Button.kA.value) // Climber down
+    // .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower));
     new JoystickButton(m_auxController, XboxController.Button.kY.value) // Winch up preset
-      .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchUpPreset));
+      .whileTrue(new WinchPresetCommand(m_winchySubsystem, WinchySquinchyConstants.kWinchUpPreset));
     new JoystickButton(m_auxController, XboxController.Button.kX.value) // Winch down preset
-      .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchDownPreset));
+      .whileTrue(new WinchPresetCommand(m_winchySubsystem, WinchySquinchyConstants.kWinchDownPreset));
     new JoystickButton(m_auxController, XboxController.Button.kBack.value) // Reset winch encoder
-      .whileTrue(new ResetWinchCommand(m_shooterSubsystem));
+      .whileTrue(new ResetWinchCommand(m_winchySubsystem));
+
+    new JoystickButton(m_auxController, XboxController.Button.kA.value)
+      .whileTrue(new FeedShooterCommand(m_intakeSubsystem));
+    new JoystickButton(m_auxController, XboxController.Button.kB.value)
+      .whileTrue(new ShooterCommand(m_shooterSubsystem));
       // POV = Winch
       // Joysticks = Manual climbers
   }
@@ -175,6 +186,11 @@ boolean getFasterMode() {
     else fasterMode = false;
     return fasterMode;
   }
+
+  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();}
@@ -183,7 +199,7 @@ boolean getFasterMode() {
   double getAuxLeftY() {return Math.abs(m_auxController.getLeftY()) > OIConstants.kDriveDeadband ? m_auxController.getLeftY() : 0;}
   double getAuxPOV() {return m_auxController.getPOV();}
   double POVToWinchSpeed() {
-    return getAuxPOV() == 0 ? ShooterConstants.kWinchUpPower : (getAuxPOV() == 180 ? ShooterConstants.kWinchDownPower : 0);
+    return getAuxPOV() == 0 ? WinchySquinchyConstants.kWinchUpPower : (getAuxPOV() == 180 ? WinchySquinchyConstants.kWinchDownPower : 0);
   }
 
   /**

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

@@ -4,6 +4,9 @@
 
 package frc.robot.commands;
 
+import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeSubsystem;
 
@@ -30,6 +33,7 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return m_subsystem.m_intakeLimitSwitch.isPressed();
+    // return false;
   }
 }

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

@@ -2,11 +2,12 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.WinchySubsystem;
 
 public class ResetWinchCommand extends Command {
-  private final ShooterSubsystem m_subsystem;
+  private final WinchySubsystem m_subsystem;
 
-  public ResetWinchCommand(ShooterSubsystem subsystem) {
+  public ResetWinchCommand(WinchySubsystem subsystem) {
     m_subsystem = subsystem;
     addRequirements(subsystem);
   }

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

@@ -1,14 +1,20 @@
 package frc.robot.commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
 public class ShootCommand extends SequentialCommandGroup {
-    public ShootCommand (
-        ShooterSubsystem shooterSubsystem
-    ) {
+    public ShootCommand (ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) {
         addCommands(
-            new ShooterCommand(shooterSubsystem, "both").withTimeout(1.75),
-            new ShooterCommand(shooterSubsystem, "all")
+            new ParallelCommandGroup(
+                new AirIntakeCommand(intakeSubsystem).withTimeout(0.2),
+                new ShooterCommand(shooterSubsystem, "both")
+            ).withTimeout(1.75),
+            new ParallelCommandGroup(
+                new ShooterCommand(shooterSubsystem, "both"),
+                new FeedShooterCommand(intakeSubsystem)
+            )
         );
     }
 }

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

@@ -23,19 +23,15 @@ public ShooterCommand(ShooterSubsystem subsystem) {
   @Override
   public void execute() {
     if (m_commandType.equals("both")) m_subsystem.runShooter();
-    else if (m_commandType.equals("all")) {
-      m_subsystem.runShooter();
-      m_subsystem.runIndex();
-    }
     else if (m_commandType.equals("front")) m_subsystem.runShooter(0.0);
-    else if (m_commandType.equals("index")) m_subsystem.runIndex();
+    // else if (m_commandType.equals("index")) m_subsystem.runIndex();
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     if (m_commandType.equals("both") || m_commandType.equals("front") || m_commandType.equals("all")) m_subsystem.stopShooter();
-    if (m_commandType.equals("index") || m_commandType.equals("all")) m_subsystem.stopIndex();
+    // if (m_commandType.equals("index") || m_commandType.equals("all")) m_subsystem.stopIndex();
   }
 
   // Returns true when the command should end.