feature/signaling

src/main/java/frc/robot/Robot.java

@@ -19,6 +19,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.subsystems.RumbleSubsystem;
 import monologue.Monologue;
 import monologue.Annotations.Log;
 import monologue.Logged;
@@ -161,10 +162,11 @@ public void teleopInit() {
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
-    // this line or comment it out.
+    // this line or comment it out. 
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    m_robotContainer.m_rumble.teleopRumble(5);
   }
 
   /** This function is called periodically during operator control. */

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

@@ -38,6 +38,7 @@
 import frc.robot.commands.Shooter.PrepareShooter;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.RumbleSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.UmbrellaSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
@@ -81,10 +82,16 @@ public class RobotContainer implements Logged {
   private final PhotonCameraWrapper m_photonCameraWrapper = new PhotonCameraWrapper();
 
   protected final DriveSubsystem m_robotDrive = new DriveSubsystem(m_photonCameraWrapper);
+
+  //controllers
+  XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
+  XboxController m_manipController = new XboxController(OIConstants.kManipControllerPort);
 
   private final IntakeSubsystem m_robotIntake = new IntakeSubsystem();
   //private final UmbrellaSubsystem m_umbrella  = new UmbrellaSubsystem();
   private final ShooterSubsystem m_shooter = new ShooterSubsystem();
+  public final RumbleSubsystem m_rumble = new RumbleSubsystem(m_driverController, m_manipController, 0, 0);
+
 
   private final Climber m_Climber = new Climber();
 
@@ -160,17 +167,17 @@ public class RobotContainer implements Logged {
     private final Command resetGyro = new ResetGyro(m_robotDrive);
     private final Command intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition);
     private final Command extakeCommand = new RunIntake(m_robotIntake, outtakePower, intakePosition);
-    private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition);
+    private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, m_rumble, intakePower, intakePosition, indexPower, intakeShooterPosition);
     private final Command outTakePiece = new OuttakePiece(m_robotIntake, m_shooter, outtakePower, intakePosition, outdexPower, intakeShooterPosition, outtakeFlywheelPower);
     private final Command stowIntake = new StowIntake(m_robotIntake);
     private final Command parkCommand = new ParkCommand(m_robotDrive);
     private final Command stowShooter = new StowShooter(m_shooter, shooterHome);
     private final Command raiseShooter = new PositionShooter(m_shooter, intakeShooterPosition);
     private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower);
     private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower);
-    private final Command shootSubwoofer = new ShootPiece(m_shooter, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
-    private final Command shootPodium = new ShootPiece(m_shooter, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
-    private final Command shootWing = new ShootPiece(m_shooter, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+    private final Command shootSubwoofer = new ShootPiece(m_shooter, m_rumble, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+    private final Command shootPodium = new ShootPiece(m_shooter, m_rumble, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+    private final Command shootWing = new ShootPiece(m_shooter, m_rumble, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
     private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM);
     private final Command preSpinShooter = new PrepareShooter(m_shooter, preSpinDistanceM);
     private final Command ejectPiece = new EjectPiece(m_shooter);
@@ -182,7 +189,7 @@ public class RobotContainer implements Logged {
 
     private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15);
 
-    private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+    private final Command shooterTune = new ShootPiece(m_shooter, m_rumble, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
     private final Command shootInterpolated = new ShootInterpolated(m_shooter, m_photonCameraWrapper, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
     private final Command driveToTarget = new DriveToTarget(m_robotDrive, 
             m_photonCameraWrapper, 
@@ -197,7 +204,7 @@ public class RobotContainer implements Logged {
   private final Command autoAmpRingShot = new AutonShoot(m_shooter, autoAmpRingShotAngle, autoAmpRingShotRPM, shooterIndexPower).withTimeout(2);
   private final Command ampShotStart = new AutonShoot(m_shooter, ampRingShotOnlyAngle, ampRingShotOnlyRPM, shooterIndexPower).withTimeout(2);
   private final Command ringToss = new RingToss(m_robotIntake, m_shooter, intakePower, intakePosition, shooterIndexPower, intakeShooterPosition, () -> 1000.0);
-  private final Command autoIntake = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition).withTimeout(3.75);
+  private final Command autoIntake = new IntakePiece(m_robotIntake, m_shooter, m_rumble, intakePower, intakePosition, indexPower, intakeShooterPosition).withTimeout(3.75);
 
 
   //Autonomous Wait Command
@@ -210,9 +217,6 @@ public class RobotContainer implements Logged {
   private final double pathSpeed = 2;
 
 
-  // The driver's controller
-  XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
-  XboxController m_manipController = new XboxController(OIConstants.kManipControllerPort);  
 
 private static class Operator {
 
@@ -355,6 +359,7 @@ private void configureDefaultCommands(){
     m_shooter.setDefaultCommand(stowShooter);
 
   }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *

src/main/java/frc/robot/RobotState.java

@@ -48,7 +48,7 @@ public synchronized void setSpeakerPose(Pose2d pose, int id){
     }
 
     public Pose2d getSpeakerPose2d(){
-        return m_robotPose2d;
+        return m_speakerPose2d;
     }
 
     public int getSpeakerID(){

src/main/java/frc/robot/commands/Shooter/PrepareShooter.java

@@ -10,6 +10,7 @@
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.utils.InterCalculator;
 import frc.robot.RobotState;
+import frc.robot.subsystems.RumbleSubsystem;
 
 import java.util.function.Supplier;
 

src/main/java/frc/robot/commands/Shooter/ShootPiece.java

@@ -6,20 +6,23 @@
 
 import edu.wpi.first.wpilibj.shuffleboard.SuppliedValueWidget;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.RumbleSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
 import java.util.function.Supplier;
 
 
 public class ShootPiece extends Command {
   private final ShooterSubsystem m_shooter;
+  private final RumbleSubsystem m_rumble;
   private final Supplier<Double> m_position;
   private final Supplier<Double> m_rpm;
   private final Supplier<Double> m_indexPower;
   private final Supplier<Boolean> m_ready; 
   /** Creates a new ShootPiece. */
-  public ShootPiece(ShooterSubsystem shooter, Supplier<Double> position, Supplier<Double> rpm, Supplier<Double> indexpower, Supplier<Boolean> ready) {
+  public ShootPiece(ShooterSubsystem shooter, RumbleSubsystem rumble, Supplier<Double> position, Supplier<Double> rpm, Supplier<Double> indexpower, Supplier<Boolean> ready) {
     m_shooter = shooter;
+    m_rumble = rumble;
     m_position = position;
     m_rpm = rpm;
     m_indexPower = indexpower;
@@ -41,6 +44,9 @@ public void execute() {
     if (m_ready.get()){
       m_shooter.runIndex(m_indexPower.get());
     }
+    if(m_shooter.armAtSetpoint() && m_shooter.atRPM()) {
+      m_rumble.shooterAtRPMAndAngle(3);
+    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/intake/IntakePiece.java

@@ -9,12 +9,14 @@
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.RumbleSubsystem;
 
 import java.util.function.Supplier;
 
 public class IntakePiece extends Command {
   private final IntakeSubsystem m_intake;
   private final ShooterSubsystem m_shooter;
+  private final RumbleSubsystem m_rumble;
   private final Supplier<Double> m_intakePower;
   private final Supplier<Double> m_intakePosition;
   private final Supplier<Double> m_indexPower;
@@ -26,10 +28,11 @@ public class IntakePiece extends Command {
 
 
   /** Creates a new IntakePiece. */
-  public IntakePiece(IntakeSubsystem intake, ShooterSubsystem shooter, Supplier<Double> intakePower, Supplier<Double> intakePosition, Supplier<Double> indexPower, Supplier<Double> indexPosition ) {
+  public IntakePiece(IntakeSubsystem intake, ShooterSubsystem shooter, RumbleSubsystem rumble, Supplier<Double> intakePower, Supplier<Double> intakePosition, Supplier<Double> indexPower, Supplier<Double> indexPosition ) {
 
     m_intake = intake; 
     m_shooter = shooter;
+    m_rumble = rumble;
     m_intakePower = intakePower;
     m_intakePosition = intakePosition;
     m_indexPower = indexPower;
@@ -57,6 +60,9 @@ public void execute() {
   public void end(boolean interrupted) {
     m_shooter.stopAll();
     m_intake.stop();
+    if(isFinished()) {
+      m_rumble.noteLoadedRumble(3);
+    }
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/subsystems/RumbleSubsystem.java

@@ -0,0 +1,81 @@
+// 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.subsystems;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+
+public class RumbleSubsystem extends SubsystemBase {
+  /** Creates a new RumbleSubsystem. */
+  XboxController m_driveController;
+  XboxController m_manipController;
+  double m_rumbleTime;
+  double m_driverRumblePower;
+  double m_manipRumblePower;
+  Timer rumbleTimer = new Timer();
+  double noteLoops;
+  boolean isContinuous;
+
+  public RumbleSubsystem(XboxController driverController, XboxController manipController, double driverRumblePower, double manipRumblePower) {
+    m_driveController = driverController;
+    m_manipController = manipController;
+    m_driverRumblePower = driverRumblePower;
+    m_manipRumblePower = manipRumblePower;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    if(rumbleTimer.hasElapsed(m_rumbleTime)){
+      m_driveController.setRumble(RumbleType.kBothRumble, 0);
+      m_manipController.setRumble(RumbleType.kBothRumble, 0);
+    }
+
+    if(noteLoops <= 2 && !isContinuous) {
+      noteLoadedRumbleLoop(0.25);
+      noteLoops++;
+    } else {
+      m_driveController.setRumble(RumbleType.kBothRumble, 0);
+      m_manipController.setRumble(RumbleType.kBothRumble, 0);
+    }
+  }
+
+  public void teleopRumble(double rumbleTime) {
+    rumbleTimer.reset();
+    rumbleTimer.start();
+    m_rumbleTime = rumbleTime;
+    isContinuous = true;
+    m_driveController.setRumble(RumbleType.kBothRumble, 1);
+    m_manipController.setRumble(RumbleType.kBothRumble, 0);
+  }
+
+  public void noteLoadedRumble(double rumbleTime) {
+    rumbleTimer.reset();
+    rumbleTimer.start();
+    m_rumbleTime = rumbleTime;
+    isContinuous = false;
+    m_driveController.setRumble(RumbleType.kBothRumble, 1);
+    m_manipController.setRumble(RumbleType.kBothRumble, 1);
+  }
+
+  public void noteLoadedRumbleLoop(double rumbleTime) {
+    rumbleTimer.reset();
+    rumbleTimer.start();
+    m_rumbleTime = rumbleTime;
+    isContinuous = true;
+    m_driveController.setRumble(RumbleType.kBothRumble, 1);
+    m_manipController.setRumble(RumbleType.kBothRumble, 1);
+  }
+
+  public void shooterAtRPMAndAngle(double rumbleTime) {
+    rumbleTimer.reset();
+    rumbleTimer.start();
+    m_rumbleTime = rumbleTime;
+    isContinuous = true;
+    m_driveController.setRumble(RumbleType.kBothRumble, 0.5);
+    m_manipController.setRumble(RumbleType.kBothRumble, 0.5);
+  }
+}

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -395,7 +395,12 @@ public void stopAll(){
   @Log.File
   @Log.NT
   public boolean getIndexerBeamBreak(){
-    return !m_indexerBeamBreak.get();
+    if(Robot.isReal()){
+      return !m_indexerBeamBreak.get();
+    } else {
+      return true;
+    }
+
   }
 
   @Override