Preset source speaker angles

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

@@ -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 = 45;
+    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,7 +123,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 +157,18 @@
     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.75;//was 1.5
   }
 
   public static final class ClimberConstants {

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

@@ -3,28 +3,30 @@
 // 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.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;
@@ -38,6 +40,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,7 +54,8 @@
 
   // 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();
@@ -66,6 +70,7 @@
   private SendableChooser<Command> m_chosenAuto = new SendableChooser<>();
   private GenericEntry m_delay;
 
+
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
@@ -78,32 +83,37 @@
             DriveConstants.kFieldOriented,
             this::getFastMode,
             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();
@@ -126,16 +136,25 @@
   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));
+    new JoystickButton(m_driverController, Button.kR2.value)
+      .onTrue(new TurnToThetaCommand(m_robotDrive, () -> getLeftX(),
+            () -> getLeftY(),
+            this::getFastMode,
+            this::getFasterMode, 180).withTimeout(1.5));
+    new JoystickButton(m_driverController, Button.kL2.value)
+      .onTrue(new TurnToThetaCommand(m_robotDrive, () -> getLeftX(),
+            () -> getLeftY(),
+            this::getFastMode,
+            this::getFasterMode, 
+            DriverStation.getAlliance().get().compareTo(Alliance.Red) == 0 ? 90 : 270)
+            .alongWith(new WinchPresetCommand(m_shooterSubsystem, 1.2)).withTimeout(1.5));
+
       // Left bumper = Toggle fastmode
       // Left trigger = Toggle fastermode
       // POV = Nudge
@@ -144,19 +163,25 @@
 
     // 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));
     new JoystickButton(m_auxController, XboxController.Button.kX.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, 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,13 +200,20 @@
     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();}
   double getPOV() {return m_driverController.getPOV();}
   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.getLeftTriggerAxis();}
+  double getAuxRTrig(){return m_auxController.getRightTriggerAxis();}
   double POVToWinchSpeed() {
     return getAuxPOV() == 0 ? ShooterConstants.kWinchUpPower : (getAuxPOV() == 180 ? ShooterConstants.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/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.