arm brake driver rumble

Author: IsaacThoman

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

@@ -34,9 +34,9 @@ public class RobotContainer {
   CommandXboxController driver = new CommandXboxController(0);
   CommandXboxController operator = new CommandXboxController(1);
 
-  StateControllerSub stateController = new StateControllerSub(); //this MUST be created before any pathplanner commands
+  StateControllerSub stateController = new StateControllerSub(driver,operator); //this MUST be created before any pathplanner commands
 
-  PneumaticControlSub pneumaticControlSub = new PneumaticControlSub();
+  PneumaticControlSub pneumaticControlSub = new PneumaticControlSub(stateController);
 
 
   VisionSubsystem visionSub = new VisionSubsystem(stateController);

src/main/java/frc/robot/StateControllerSub.java

@@ -5,10 +5,12 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.*;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.drive.Alignments;
 import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.MBUtils;
@@ -197,7 +199,9 @@ public void setClimbState(ClimbState climbState){
     DigitalInput beamBreak1 = new DigitalInput(4);
 
 
-    public StateControllerSub(){
+    CommandXboxController driver;
+    CommandXboxController operator;
+    public StateControllerSub(CommandXboxController driver, CommandXboxController operator){
         NamedCommands.registerCommand("intakeMode",new InstantCommand(this::intakePressed));
         NamedCommands.registerCommand("holdMode",new InstantCommand(this::holdPressed));
         NamedCommands.registerCommand("setObjectiveSpeaker",new InstantCommand(this::speakerPressed));
@@ -215,6 +219,8 @@ public StateControllerSub(){
         SmartDashboard.putBoolean("alignWhenClose",alignWhenCloseEnabled);
 
 
+        this.driver = driver;
+        this.operator = operator;
 
     }
 
@@ -230,6 +236,8 @@ public void periodic(){
 
         publishTableEntries();
 
+        updateRumbles();
+
         //get an x y and z from smartDashboard
 
 
@@ -288,6 +296,28 @@ public void periodic(){
 
     }
 
+    public void updateRumbles(){
+        double bothRumbleVal = 0;
+
+        if(Timer.getFPGATimestamp() - lastBrakeEngagementTimestamp <0.2 && armState == ArmState.SPEAKER)
+            bothRumbleVal = 1;
+
+        driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble,bothRumbleVal);
+        operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble,bothRumbleVal);
+
+    }
+    boolean brakeEngaged = false;
+    double lastBrakeEngagementTimestamp = 0;
+    public void feedBrakeEngaged(boolean brakeEngaged){
+        if(brakeEngaged == true && this.brakeEngaged == false)
+            onBrakeEngaged();
+        this.brakeEngaged = brakeEngaged;
+    }
+
+    void onBrakeEngaged(){
+        lastBrakeEngagementTimestamp = Timer.getFPGATimestamp();
+    }
+
 
     public void setDuckMode(boolean down){
         if(down) duckMode = DuckMode.DOWN;

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

@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.StateControllerSub;
 
 public class PneumaticControlSub extends SubsystemBase {
     PneumaticsControlModule pcm = new PneumaticsControlModule(59);
@@ -12,15 +13,18 @@ public class PneumaticControlSub extends SubsystemBase {
     Solenoid climbSolenoid;
     Solenoid brakeSolenoid;
 
-    public PneumaticControlSub(){
+
+    StateControllerSub stateControllerSub;
+
+    public PneumaticControlSub(StateControllerSub stateControllerSub){
         pcm.disableCompressor();
         SmartDashboard.putBoolean("enableCompressor",false);
 
         climbSolenoid = pcm.makeSolenoid(6);
         brakeSolenoid = pcm.makeSolenoid(5);
 
        SmartDashboard.putBoolean("enableBrakes",true);
-
+this.stateControllerSub = stateControllerSub;
 
     }
 
@@ -52,6 +56,7 @@ public void setBrakeSolenoid(boolean state){
         SmartDashboard.putBoolean("brakeSolenoidClosed",state);
         if(!SmartDashboard.getBoolean("enableBrakes",true))
             state = false;
+        stateControllerSub.feedBrakeEngaged(state);
         brakeSolenoid.set(state);
     }