Reformatted: Gonna try to release it as 2.0.3 because JitPack was unh…

…appy with 2.0.2
technototes · Sep 25, 2024 · 5d91c84 · 5d91c84
1 parent 0477a00
commit 5d91c84
Show file tree

Hide file tree

Showing 16 changed files with 92 additions and 95 deletions.
diff --git a/Path/src/main/java/com/technototes/path/command/MecanumDriveCommand.java b/Path/src/main/java/com/technototes/path/command/MecanumDriveCommand.java
@@ -27,8 +27,9 @@ public MecanumDriveCommand(
 
     @Override
     public void execute() {
-        Vector2d input = new Vector2d(-y.getAsDouble() * subsystem.speed, -x.getAsDouble() * subsystem.speed)
-            .rotated(-subsystem.getExternalHeading());
+        Vector2d input = new Vector2d(-y.getAsDouble() * subsystem.speed, -x.getAsDouble() * subsystem.speed).rotated(
+            -subsystem.getExternalHeading()
+        );
 
         subsystem.setWeightedDrivePower(
             new Pose2d(input.getX(), input.getY(), -Math.pow(r.getAsDouble() * subsystem.speed, 3))

diff --git a/Path/src/main/java/com/technototes/path/subsystem/DistanceSensorLocalizer.java b/Path/src/main/java/com/technototes/path/subsystem/DistanceSensorLocalizer.java
@@ -57,8 +57,10 @@ public void update(@Nullable Pose2d old) {
             Pose2d sensorPose = entry.getValue();
             double distance = sensor.getDistance();
             if (distance < maxSensorDistance && distance > 0.5) {
-                sensorPose =
-                    new Pose2d(sensorPose.vec().rotated(heading), Angle.norm(sensorPose.getHeading() + heading));
+                sensorPose = new Pose2d(
+                    sensorPose.vec().rotated(heading),
+                    Angle.norm(sensorPose.getHeading() + heading)
+                );
                 double change;
                 switch (MathUtils.closestTo((2 * sensorPose.getHeading()) / Math.PI, 0, 1, 2, 3, 4)) {
                     case 0:
@@ -90,8 +92,11 @@ public void update(@Nullable Pose2d old) {
             }
         }
         if (old == null) old = new Pose2d();
-        poseEstimate =
-            new Pose2d(totalX != 0 ? accumX / totalX : old.getX(), totalY != 0 ? accumY / totalY : old.getY(), heading);
+        poseEstimate = new Pose2d(
+            totalX != 0 ? accumX / totalX : old.getX(),
+            totalY != 0 ? accumY / totalY : old.getY(),
+            heading
+        );
     }
 
     public double gyroOffset = 0;

diff --git a/Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java b/Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java
@@ -163,14 +163,13 @@ public PathingMecanumDrivebaseSubsystem(
         VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
         ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
 
-        follower =
-            new HolonomicPIDVAFollower(
-                TRANSLATIONAL_PID,
-                TRANSLATIONAL_PID,
-                HEADING_PID,
-                new Pose2d(0.5, 0.5, Math.toRadians(5.0)),
-                0.5
-            );
+        follower = new HolonomicPIDVAFollower(
+            TRANSLATIONAL_PID,
+            TRANSLATIONAL_PID,
+            HEADING_PID,
+            new Pose2d(0.5, 0.5, Math.toRadians(5.0)),
+            0.5
+        );
 
         LynxModuleUtil.ensureMinimumFirmwareVersion(HardwareDevice.hardwareMap);
 
@@ -241,9 +240,8 @@ public void turn(double angle) {
     }
 
     public void followTrajectoryAsync(Trajectory trajectory) {
-        if (trajectory == null) trajectorySequenceRunner.followTrajectorySequenceAsync(
-            null
-        ); else trajectorySequenceRunner.followTrajectorySequenceAsync(
+        if (trajectory == null) trajectorySequenceRunner.followTrajectorySequenceAsync(null);
+        else trajectorySequenceRunner.followTrajectorySequenceAsync(
             trajectorySequenceBuilder(trajectory.start()).addTrajectory(trajectory).build()
         );
     }
@@ -321,13 +319,11 @@ public void setWeightedDrivePower(Pose2d drivePower) {
                 VY_WEIGHT * Math.abs(drivePower.getY()) +
                 OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
 
-            vel =
-                new Pose2d(
-                    VX_WEIGHT * drivePower.getX(),
-                    VY_WEIGHT * drivePower.getY(),
-                    OMEGA_WEIGHT * drivePower.getHeading()
-                )
-                    .div(denom);
+            vel = new Pose2d(
+                VX_WEIGHT * drivePower.getX(),
+                VY_WEIGHT * drivePower.getY(),
+                OMEGA_WEIGHT * drivePower.getHeading()
+            ).div(denom);
         }
 
         setDrivePower(vel);

diff --git a/Path/src/main/java/com/technototes/path/trajectorysequence/TrajectorySequenceBuilder.java b/Path/src/main/java/com/technototes/path/trajectorysequence/TrajectorySequenceBuilder.java
@@ -530,8 +530,13 @@ private void newPath() {
 
         double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
 
-        currentTrajectoryBuilder =
-            new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
+        currentTrajectoryBuilder = new TrajectoryBuilder(
+            lastPose,
+            tangent,
+            currentVelConstraint,
+            currentAccelConstraint,
+            resolution
+        );
     }
 
     public TrajectorySequence build() {
@@ -629,27 +634,25 @@ private List<SequenceSegment> projectGlobalMarkersToLocalSegments(
                 newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
 
                 TurnSegment thisSegment = (TurnSegment) segment;
-                newSegment =
-                    new TurnSegment(
-                        thisSegment.getStartPose(),
-                        thisSegment.getTotalRotation(),
-                        thisSegment.getMotionProfile(),
-                        newMarkers
-                    );
+                newSegment = new TurnSegment(
+                    thisSegment.getStartPose(),
+                    thisSegment.getTotalRotation(),
+                    thisSegment.getMotionProfile(),
+                    newMarkers
+                );
             } else if (segment instanceof TrajectorySegment) {
                 TrajectorySegment thisSegment = (TrajectorySegment) segment;
 
                 List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers());
                 newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
 
-                newSegment =
-                    new TrajectorySegment(
-                        new Trajectory(
-                            thisSegment.getTrajectory().getPath(),
-                            thisSegment.getTrajectory().getProfile(),
-                            newMarkers
-                        )
-                    );
+                newSegment = new TrajectorySegment(
+                    new Trajectory(
+                        thisSegment.getTrajectory().getPath(),
+                        thisSegment.getTrajectory().getProfile(),
+                        newMarkers
+                    )
+                );
             }
 
             sequenceSegments.set(segmentIndex, newSegment);

diff --git a/Path/src/main/java/com/technototes/path/trajectorysequence/TrajectorySequenceRunner.java b/Path/src/main/java/com/technototes/path/trajectorysequence/TrajectorySequenceRunner.java
@@ -146,8 +146,10 @@ public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence)
                 Pose2d startPose = currentSegment.getStartPose();
                 targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
 
-                driveSignal =
-                    new DriveSignal(new Pose2d(0, 0, targetOmega + correction), new Pose2d(0, 0, targetAlpha));
+                driveSignal = new DriveSignal(
+                    new Pose2d(0, 0, targetOmega + correction),
+                    new Pose2d(0, 0, targetAlpha)
+                );
 
                 if (deltaTime >= currentSegment.getDuration()) {
                     currentSegmentIndex++;

diff --git a/RobotLibrary/src/main/java/com/technototes/library/command/ChoiceCommand.java b/RobotLibrary/src/main/java/com/technototes/library/command/ChoiceCommand.java
@@ -21,8 +21,7 @@ public class ChoiceCommand extends ParallelRaceGroup {
     @SafeVarargs
     public ChoiceCommand(Pair<BooleanSupplier, Command>... cs) {
         super(
-            Arrays
-                .stream(cs)
+            Arrays.stream(cs)
                 .<Command>map(p -> new ConditionalCommand(p.first, p.second))
                 .collect(Collectors.toList())
                 .toArray(new Command[] {})

diff --git a/RobotLibrary/src/main/java/com/technototes/library/command/Command.java b/RobotLibrary/src/main/java/com/technototes/library/command/Command.java
@@ -223,7 +223,7 @@ default void run() {
             case INITIALIZING:
                 initialize();
                 setState(CommandState.EXECUTING);
-                // no return for fallthrough
+            // no return for fallthrough
             case EXECUTING:
                 execute();
                 if (isFinished()) setState(CommandState.FINISHED);

diff --git a/RobotLibrary/src/main/java/com/technototes/library/command/CommandGroup.java b/RobotLibrary/src/main/java/com/technototes/library/command/CommandGroup.java
@@ -91,8 +91,8 @@ public void initialize() {
     @Override
     public void execute() {
         // makes true if command just finished
-        commandMap.replaceAll((command, bool) ->
-            (countCancel ? command.justFinished() : command.justFinishedNoCancel()) || bool
+        commandMap.replaceAll(
+            (command, bool) -> (countCancel ? command.justFinished() : command.justFinishedNoCancel()) || bool
         );
         anyCancelled = commandMap.keySet().stream().anyMatch(Command::isCancelled) || anyCancelled;
     }

diff --git a/RobotLibrary/src/main/java/com/technototes/library/command/CommandScheduler.java b/RobotLibrary/src/main/java/com/technototes/library/command/CommandScheduler.java
@@ -168,9 +168,8 @@ public static void scheduleForState(
      * @param states  The list of states to schedule the command
      */
     public static void scheduleForState(Command command, CommandOpMode.OpModeState... states) {
-        schedule(
-            command.cancelUpon(() -> !opMode.getOpModeState().isState(states)),
-            () -> opMode.getOpModeState().isState(states)
+        schedule(command.cancelUpon(() -> !opMode.getOpModeState().isState(states)), () ->
+            opMode.getOpModeState().isState(states)
         );
     }
 

diff --git a/RobotLibrary/src/main/java/com/technototes/library/command/MethodCommand.java b/RobotLibrary/src/main/java/com/technototes/library/command/MethodCommand.java
@@ -91,7 +91,12 @@ public <T, U> MethodCommand(BiConsumer<T, U> m, T arg1, Supplier<U> arg2supplier
      * @param arg2supplier The function to compute the second argument to pass to the method
      * @param subs Any subsystems this command requires
      */
-    public <T, U> MethodCommand(BiConsumer<T, U> m, Supplier<T> arg1supplier, Supplier<U> arg2supplier, Subsystem... subs) {
+    public <T, U> MethodCommand(
+        BiConsumer<T, U> m,
+        Supplier<T> arg1supplier,
+        Supplier<U> arg2supplier,
+        Subsystem... subs
+    ) {
         super();
         method = () -> m.accept(arg1supplier.get(), arg2supplier.get());
         addRequirements(subs);

diff --git a/RobotLibrary/src/main/java/com/technototes/library/hardware/servo/ServoProfiler.java b/RobotLibrary/src/main/java/com/technototes/library/hardware/servo/ServoProfiler.java
@@ -88,12 +88,11 @@ public ServoProfiler update() {
         // range.clip makes the change fit the max constraints
         // the min and max make sure both constraints are hit
         // the deltasec makes it independent of looptime
-        delta =
-            Range.clip(
-                deltaSec * servoRange * proportion * (getTargetPosition() - getCurrentPosition()),
-                Math.max(pastDelta - maxAccel * deltaSec, -maxVel * deltaSec),
-                Math.min(pastDelta + maxAccel * deltaSec, maxVel * deltaSec)
-            );
+        delta = Range.clip(
+            deltaSec * servoRange * proportion * (getTargetPosition() - getCurrentPosition()),
+            Math.max(pastDelta - maxAccel * deltaSec, -maxVel * deltaSec),
+            Math.min(pastDelta + maxAccel * deltaSec, maxVel * deltaSec)
+        );
         servo.setPosition(getCurrentPosition() + delta / servoRange);
 
         return this;

diff --git a/RobotLibrary/src/main/java/com/technototes/library/logger/Logger.java b/RobotLibrary/src/main/java/com/technototes/library/logger/Logger.java
@@ -172,31 +172,25 @@ public void initUpdate() {
     }
 
     private void set(Annotation[] a, Method m, Object root) {
-        set(
-            a,
-            () -> {
-                try {
-                    return m.invoke(root);
-                } catch (IllegalAccessException | InvocationTargetException e) {
-                    e.printStackTrace();
-                }
-                return null;
+        set(a, () -> {
+            try {
+                return m.invoke(root);
+            } catch (IllegalAccessException | InvocationTargetException e) {
+                e.printStackTrace();
             }
-        );
+            return null;
+        });
     }
 
     private void set(Annotation[] a, Field m, Object root) {
-        set(
-            a,
-            () -> {
-                try {
-                    return m.get(root);
-                } catch (IllegalAccessException e) {
-                    e.printStackTrace();
-                }
-                return null;
+        set(a, () -> {
+            try {
+                return m.get(root);
+            } catch (IllegalAccessException e) {
+                e.printStackTrace();
             }
-        );
+            return null;
+        });
     }
 
     @SuppressWarnings({ "unchecked" })
@@ -211,14 +205,13 @@ private void set(Annotation[] a, Supplier<?> m) {
                 e = new StringEntry(((Log) as).name(), (Supplier<String>) m, ((Log) as).index(), ((Log) as).format());
                 e.setPriority(((Log) as).priority());
             } else if (as instanceof Log.Boolean) {
-                e =
-                    new BooleanEntry(
-                        ((Log.Boolean) as).name(),
-                        (Supplier<Boolean>) m,
-                        ((Log.Boolean) as).index(),
-                        ((Log.Boolean) as).trueValue(),
-                        ((Log.Boolean) as).falseValue()
-                    );
+                e = new BooleanEntry(
+                    ((Log.Boolean) as).name(),
+                    (Supplier<Boolean>) m,
+                    ((Log.Boolean) as).index(),
+                    ((Log.Boolean) as).trueValue(),
+                    ((Log.Boolean) as).falseValue()
+                );
                 e.setPriority(((Log.Boolean) as).priority());
             } else if (as instanceof LogConfig.Run) {
                 init = ((LogConfig.Run) as).duringInit();

diff --git a/Vision/src/main/java/com/technototes/vision/hardware/InternalCamera.java b/Vision/src/main/java/com/technototes/vision/hardware/InternalCamera.java
@@ -43,8 +43,7 @@ public OpenCvInternalCamera.CameraDirection getCameraDirection() {
      */
     @Override
     public OpenCvInternalCamera createCamera() {
-        return OpenCvCameraFactory
-            .getInstance()
+        return OpenCvCameraFactory.getInstance()
             .createInternalCamera(
                 getCameraDirection(),
                 hardwareMap.appContext

diff --git a/Vision/src/main/java/com/technototes/vision/hardware/SwitchableWebcam.java b/Vision/src/main/java/com/technototes/vision/hardware/SwitchableWebcam.java
@@ -51,8 +51,7 @@ public SwitchableWebcam(Webcam... device) {
      */
     @Override
     public OpenCvSwitchableWebcam createCamera() {
-        return OpenCvCameraFactory
-            .getInstance()
+        return OpenCvCameraFactory.getInstance()
             .createSwitchableWebcam(
                 hardwareMap.appContext
                     .getResources()

diff --git a/Vision/src/main/java/com/technototes/vision/hardware/Webcam.java b/Vision/src/main/java/com/technototes/vision/hardware/Webcam.java
@@ -40,8 +40,7 @@ public String LogLine() {
      */
     @Override
     public OpenCvWebcam createCamera() {
-        return OpenCvCameraFactory
-            .getInstance()
+        return OpenCvCameraFactory.getInstance()
             .createWebcam(
                 getRawDevice(),
                 hardwareMap.appContext

diff --git a/Vision/src/main/java/com/technototes/vision/subsystem/BasicVisionSubsystem.java b/Vision/src/main/java/com/technototes/vision/subsystem/BasicVisionSubsystem.java
@@ -87,9 +87,7 @@ public void startVisionPipeline() {
     public void stopVisionPipeline() {
         camera.setPipeline(null);
         pipelineSet = false;
-        camera.closeCameraDeviceAsync(() -> {
-            /* Do we need to do anything to stop the vision pipeline? */
-        });
+        camera.closeCameraDeviceAsync(() -> {});
     }
 
     /**