Merge pull request #102 from errorcodexero/bugfixes-postcais

Potential fix for oscillation

Author: themehdev

src/main/java/frc/robot/commands/robot/RobotCommands.java

@@ -36,6 +36,9 @@ public static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem i
         BooleanSupplier shouldXWheels =
             () -> drive.rotationIsNear(RobotState.rotationToHub(), ShooterConstants.xWheelTolerance);
 
+        BooleanSupplier shouldRotateAgain =
+            () -> !drive.rotationIsNear(RobotState.rotationToHub(), ShooterConstants.unXWheelTolerance);
+
         return shootHubNoAim(shooter, hopper, intake, drive).alongWith(
             DriveCommands.joystickDriveAtAngle(
                 drive,
@@ -44,7 +47,7 @@ public static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem i
                 RobotState::rotationToHub
             )
             .until(shouldXWheels)
-            .andThen(drive.stopWithXCmd(), drive.idle().onlyWhile(shouldXWheels))
+            .andThen(drive.stopWithXCmd(), drive.idle().until(shouldRotateAgain))
             .repeatedly()
         );
     }

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -13,8 +13,11 @@
 
 package frc.robot.subsystems.drive;
 
+import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.derive;
 
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
@@ -51,6 +54,7 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.AngleUnit;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.LinearVelocity;
@@ -59,6 +63,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
@@ -447,7 +452,8 @@ public Rotation2d getRotation() {
 
     /** Whether the rotation of the robot is near to a target. */
     public boolean rotationIsNear(Rotation2d target, Angle tolerance) {
-        return target.getMeasure().isNear(getRotation().getMeasure(), tolerance);
+        var difference = Degrees.of(target.minus(getRotation()).getMeasure().abs(Degrees));
+        return difference.lt(tolerance);
     }
 
     /** Resets the current odometry pose. */

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

@@ -39,6 +39,7 @@ public class ShooterConstants {
     public static final Angle defenseTolerance = Degrees.of(7.5);
 
     public static final Angle xWheelTolerance = Degrees.of(1);
+    public static final Angle unXWheelTolerance = Degrees.of(3);
 
     public static final Distance allowedTrenchDistance = Meters.of(1.0);