added a method to check if setVoltage is close to appliedVoltage

Author: clairethebear1

src/main/java/frc/robot/subsystems/groundIntake/GroundIntake.java

@@ -27,9 +27,9 @@ public GroundIntake (GroundIntakeIO io) {
         logP = new LoggedDashboardNumber("GroundIntake/P", io.getP());
         logI = new LoggedDashboardNumber("GroundIntake/I", io.getI());
         logD = new LoggedDashboardNumber("GroundIntake/D", io.getD());
-
     }
 
+
     public void periodic() {
         io.updateInputs(inputs);
         // Update PID constants to ensure they are up to date
@@ -48,12 +48,23 @@ public void periodic() {
         Logger.recordOutput("GroundIntake/GIntakeMotorConnected", inputs.velocityRadsPerSec != 0);
     }
 
+     //Check if the actual voltage is close to the desired voltage
+    public boolean isVoltageClose(double setVoltage, double tolerance) {
+        // Calculate the absolute difference between the desired and applied voltages
+        double voltageDifference = Math.abs(setVoltage - inputs.appliedVoltage);
+        
+        // Check if the absolute difference is within the tolerance
+        return voltageDifference <= tolerance;
+    }
+
     public void setVoltage(double voltage) {
         if (SmartDashboard.getBoolean("Turbo Mode", false)){
             io.setVoltage(0);
         } else {
             io.setVoltage(voltage);
         }
+
+        Logger.recordOutput('GroundIntake/Close', isVoltageClose(voltage, 1));
     }
 
     public void setBrake(boolean brake) {

src/main/java/frc/robot/subsystems/indexer/Indexer.java

@@ -43,11 +43,21 @@ public Indexer (IndexerIO io) {
         logD = new LoggedDashboardNumber("Intake/D", io.getD());
     }
 
+         //Check if the actual voltage is close to the desired voltage
+    public boolean isVoltageClose(double setVoltage, double tolerance) {
+        // Calculate the absolute difference between the desired and applied voltages
+        double voltageDifference = Math.abs(setVoltage - inputs.appliedVoltage);
+        
+        // Check if the absolute difference is within the tolerance
+        return voltageDifference <= tolerance;
+    }
+
     public void periodic() {
         io.updateInputs(inputs);
         // Update PID constants to ensure they are up to date
         Logger.processInputs("Intake", inputs);
 
+
         // Updates state of where the note is in the intake
         if(noteState == NoteState.NOT_ENOUGH && inputs.breakBeam) { // Note just entered the right spot
             noteState = NoteState.GOLDILOCKS;
@@ -95,6 +105,7 @@ public void setVoltage(double voltage) {
         } else {
             io.setVoltage(voltage);
         }
+         Logger.recordOutput('Indexer/Close', isVoltageClose(voltage, 1));
     }
 
     public void setBrake(boolean brake) {

src/main/java/frc/robot/subsystems/pivotArm/PivotArm.java

@@ -134,6 +134,15 @@ public void setBrake(boolean brake) {
         io.setBrake(brake);
     }
 
+         //Check if the actual voltage is close to the desired voltage
+    public boolean isVoltageClose(double setVoltage, double tolerance) {
+        // Calculate the absolute difference between the desired and applied voltages
+        double voltageDifference = Math.abs(setVoltage - inputs.appliedVoltage);
+        
+        // Check if the absolute difference is within the tolerance
+        return voltageDifference <= tolerance;
+    }
+
     public void setVoltage(double motorVolts) {
         // limit the arm if its past the limit
         if (io.getAngle() > PivotArmConstants.PIVOT_ARM_MAX_ANGLE && motorVolts > 0) {
@@ -147,6 +156,7 @@ public void setVoltage(double motorVolts) {
         } else {
             io.setVoltage(motorVolts);
         }
+        Logger.recordOutput('PivotArm/Close', isVoltageClose(voltage, 1));
     }
 
     public void move(double speed) {
@@ -324,5 +334,4 @@ public Command dynamicBack() {
                 .alongWith(new InstantCommand(() -> Logger.recordOutput("PivotArm/sysid-test-state-", "dynamic-reverse")));
     }
 
-}
-
+}

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -60,6 +60,15 @@ public Shooter(ShooterIO io) {
     shooterIO.setRightPID(rightkP.get(), rightkI.get(), rightkD.get());
   }
 
+         //Check if the actual voltage is close to the desired voltage
+    public boolean isVoltageClose(double setVoltage, double tolerance) {
+        // Calculate the absolute difference between the desired and applied voltages
+        double voltageDifference = Math.abs(setVoltage - inputs.appliedVoltage);
+        
+        // Check if the absolute difference is within the tolerance
+        return voltageDifference <= tolerance;
+  
+
   @Override
   public void periodic() {
     // check controllers
@@ -175,6 +184,8 @@ public void setVoltage(DoubleSupplier leftVoltage, DoubleSupplier rightVoltage){
         shooterIO.setVoltage(leftMotorVoltage, rightMotorVoltage);
     }
   }
+        Logger.recordOutput('Shooter/Close', isVoltageClose(voltage, 1));
+  }
 
   public void setVoltage(double volts){
     setVoltage(() -> volts, () -> volts);