Clean up Manual Zeroing methods

FRCTeam3255 · Nov 6, 2024 · 8ba16eb · 8ba16eb
1 parent 3fdea96
commit 8ba16eb
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 21 deletions.
diff --git a/src/main/java/frc/robot/commands/Zeroing/ManualZeroElevator.java b/src/main/java/frc/robot/commands/Zeroing/ManualZeroElevator.java
@@ -9,7 +9,6 @@
 import edu.wpi.first.units.Time;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.Velocity;
-import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -36,36 +35,31 @@ public void initialize() {
 
   @Override
   public void execute() {
-    // Check if we have raised the shooter above a certain speed
+    // Check if we have raised the elevator above a certain speed
     if (subElevator.getRotorVelocity().gte(constElevator.MANUAL_ZEROING_START_VELOCITY)
         || Elevator.attemptingZeroing) {
       // Enter zeroing mode!
       if (!Elevator.attemptingZeroing) {
         Elevator.attemptingZeroing = true;
         zeroingTimestamp = Units.Seconds.of(Timer.getFPGATimestamp());
-
         System.out.println("Elevator Zeroing Started!");
       }
 
-      // Check if time elapsed since previous zeroing is too high - if true, then exit
-      // zeroing mode :(
+      // Check if time elapsed is too high (zeroing timeout)
       if (Units.Seconds.of(Timer.getFPGATimestamp()).minus(zeroingTimestamp).gte(constElevator.ZEROING_TIMEOUT)) {
         Elevator.attemptingZeroing = false;
         System.out.println("Elevator Zeroing Failed :(");
-
       } else {
-        boolean rotorVelocity = subElevator.getRotorVelocity().minus(lastRotorVelocity)
+        boolean deltaRotorVelocity = subElevator.getRotorVelocity().minus(lastRotorVelocity)
             .lte(constElevator.MANUAL_ZEROING_DELTA_VELOCITY);
 
-        if (rotorVelocity && lastRotorVelocity.lte(Units.RotationsPerSecond.of(0))) {
+        if (deltaRotorVelocity && lastRotorVelocity.lte(Units.RotationsPerSecond.of(0))) {
           zeroingSuccess = true;
         } else {
           lastRotorVelocity = subElevator.getRotorVelocity();
         }
       }
-
     }
-
   }
 
   @Override
@@ -84,6 +78,5 @@ public boolean isFinished() {
     boolean rotorVelocityIsZero = subElevator.getRotorVelocity().isNear(Units.RotationsPerSecond.zero(), 0.01);
     SmartDashboard.putBoolean("Zeroing/Pivot/Is Rotor Velocity Zero", rotorVelocityIsZero);
     return zeroingSuccess && rotorVelocityIsZero;
-
   }
 }
diff --git a/src/main/java/frc/robot/commands/Zeroing/ManualZeroShooterPivot.java b/src/main/java/frc/robot/commands/Zeroing/ManualZeroShooterPivot.java
@@ -9,7 +9,6 @@
 import edu.wpi.first.units.Time;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.Velocity;
-import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -26,7 +25,6 @@ public class ManualZeroShooterPivot extends Command {
 
   public ManualZeroShooterPivot(Shooter subShooter) {
     this.subShooter = subShooter;
-
     addRequirements(subShooter);
   }
 
@@ -43,29 +41,24 @@ public void execute() {
       if (!Shooter.attemptingZeroing) {
         Shooter.attemptingZeroing = true;
         zeroingTimestamp = Units.Seconds.of(Timer.getFPGATimestamp());
-
         System.out.println("Shooter Zeroing Started!");
       }
 
-      // Check if time elapsed since previous zeroing is too high - if true, then exit
-      // zeroing mode :(
+      // Check if time elapsed is too high (zeroing timeout)
       if (Units.Seconds.of(Timer.getFPGATimestamp()).minus(zeroingTimestamp).gte(constShooter.ZEROING_TIMEOUT)) {
         Shooter.attemptingZeroing = false;
         System.out.println("Shooter Zeroing Failed :(");
-
       } else {
-        boolean rotorVelocity = subShooter.getPivotRotorVelocity().minus(lastRotorVelocity)
+        boolean deltaRotorVelocity = subShooter.getPivotRotorVelocity().minus(lastRotorVelocity)
             .lte(constShooter.MANUAL_ZEROING_DELTA_VELOCITY);
 
-        if (rotorVelocity && lastRotorVelocity.lte(Units.RotationsPerSecond.of(0))) {
+        if (deltaRotorVelocity && lastRotorVelocity.lte(Units.RotationsPerSecond.of(0))) {
           zeroingSuccess = true;
         } else {
           lastRotorVelocity = subShooter.getPivotRotorVelocity();
         }
       }
-
     }
-
   }
 
   @Override