Skip to content

Commit 2c8a6df

Browse files
github-actionsTortuga-AM
github-actions
authored and
Tortuga-AM
committed
Apply Prettier format
1 parent b1bfc56 commit 2c8a6df

File tree

5 files changed

+61
-44
lines changed

5 files changed

+61
-44
lines changed

‎src/main/java/frc/robot/Subsystems/Drive/Drive.java‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import frc.robot.GlobalConstants.Controllers;
1818
import frc.robot.GlobalConstants.RobotMode;
1919
import frc.robot.Utilitys.PathFinder;
20+
import java.io.File;
2021
import org.littletonrobotics.junction.Logger;
2122
import org.team7525.subsystem.Subsystem;
2223
import swervelib.SwerveDrive;
@@ -25,8 +26,6 @@
2526
import swervelib.telemetry.SwerveDriveTelemetry;
2627
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
2728

28-
import java.io.File;
29-
3029
public class Drive extends Subsystem<DriveStates> {
3130

3231
// Swerve-related components
@@ -96,13 +95,14 @@ private Drive() {
9695
private void establishTriggers() {
9796
addRunnableTrigger(() -> swerveDrive.lockPose(), DRIVER_CONTROLLER::getAButton);
9897
addRunnableTrigger(
99-
() -> swerveDrive.resetOdometry(
100-
new Pose2d(
101-
swerveDrive.getPose().getX(),
102-
swerveDrive.getPose().getY(),
103-
Rotation2d.fromDegrees(0)
104-
)
105-
),
98+
() ->
99+
swerveDrive.resetOdometry(
100+
new Pose2d(
101+
swerveDrive.getPose().getX(),
102+
swerveDrive.getPose().getY(),
103+
Rotation2d.fromDegrees(0)
104+
)
105+
),
106106
DRIVER_CONTROLLER::getRightBumperButtonPressed
107107
);
108108
}

‎src/main/java/frc/robot/Subsystems/Drive/DriveConstants.java‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import static edu.wpi.first.units.Units.DegreesPerSecond;
44
import static edu.wpi.first.units.Units.MetersPerSecond;
55
import static edu.wpi.first.units.Units.RotationsPerSecond;
6+
67
import com.pathplanner.lib.config.PIDConstants;
78
import com.pathplanner.lib.path.PathConstraints;
89
import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -24,7 +25,9 @@ public final class DriveConstants {
2425
public static final LinearVelocity MAX_SPEED = MetersPerSecond.of(4.6);
2526
public static final LinearVelocity SLOW_SPEED = MetersPerSecond.of(MAX_SPEED.magnitude() * 0.2);
2627
public static final AngularVelocity MAX_ANGULAR_VELOCITY = RotationsPerSecond.of(3);
27-
public static final AngularVelocity SLOW_ANGULAR_VELOCITY = RotationsPerSecond.of(MAX_ANGULAR_VELOCITY.magnitude() * 0.2);
28+
public static final AngularVelocity SLOW_ANGULAR_VELOCITY = RotationsPerSecond.of(
29+
MAX_ANGULAR_VELOCITY.magnitude() * 0.2
30+
);
2831

2932
// Rate limit for acceleration
3033
public static final int RATE_LIMIT = 6;

‎src/main/java/frc/robot/Subsystems/Vision/Vision.java‎

Lines changed: 38 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -145,7 +145,13 @@ public void periodic() {
145145
}
146146

147147
// Log camera data
148-
logCameraData(cameraIndex, tagPoses, robotPoses, robotPosesAccepted, robotPosesRejected);
148+
logCameraData(
149+
cameraIndex,
150+
tagPoses,
151+
robotPoses,
152+
robotPosesAccepted,
153+
robotPosesRejected
154+
);
149155

150156
// Aggregate data
151157
allTagPoses.addAll(tagPoses);
@@ -158,16 +164,17 @@ public void periodic() {
158164
logSummaryData(allTagPoses, allRobotPoses, allRobotPosesAccepted, allRobotPosesRejected);
159165
}
160166

161-
162-
// Determines whether a pose observation should be rejected.
167+
// Determines whether a pose observation should be rejected.
163168
private boolean shouldRejectPose(VisionIO.PoseObservation observation) {
164-
return observation.tagCount() == 0 ||
165-
(observation.tagCount() == 1 && observation.ambiguity() > maxAmbiguity) ||
166-
Math.abs(observation.pose().getZ()) > maxZError ||
167-
observation.pose().getX() < 0.0 ||
168-
observation.pose().getX() > APRIL_TAG_FIELD_LAYOUT.getFieldLength() ||
169-
observation.pose().getY() < 0.0 ||
170-
observation.pose().getY() > APRIL_TAG_FIELD_LAYOUT.getFieldWidth();
169+
return (
170+
observation.tagCount() == 0 ||
171+
(observation.tagCount() == 1 && observation.ambiguity() > maxAmbiguity) ||
172+
Math.abs(observation.pose().getZ()) > maxZError ||
173+
observation.pose().getX() < 0.0 ||
174+
observation.pose().getX() > APRIL_TAG_FIELD_LAYOUT.getFieldLength() ||
175+
observation.pose().getY() < 0.0 ||
176+
observation.pose().getY() > APRIL_TAG_FIELD_LAYOUT.getFieldWidth()
177+
);
171178
}
172179

173180
// Adds rejection reasons to logs
@@ -193,8 +200,12 @@ private void logRejectionReasons(int cameraIndex, VisionIO.PoseObservation obser
193200
}
194201

195202
// Calculates standard deviations for a pose observation.
196-
private double[] calculateStandardDeviations(int cameraIndex, VisionIO.PoseObservation observation) {
197-
double stdDevFactor = Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
203+
private double[] calculateStandardDeviations(
204+
int cameraIndex,
205+
VisionIO.PoseObservation observation
206+
) {
207+
double stdDevFactor =
208+
Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
198209
double linearStdDev = linearStdDevBaseline * stdDevFactor;
199210
double angularStdDev = angularStdDevBaseline * stdDevFactor;
200211
if (observation.type() == PoseObservationType.MEGATAG_2) {
@@ -208,10 +219,14 @@ private double[] calculateStandardDeviations(int cameraIndex, VisionIO.PoseObser
208219
return new double[] { linearStdDev, angularStdDev };
209220
}
210221

211-
212222
// Logs camera-specific data.
213-
private void logCameraData(int cameraIndex, List<Pose3d> tagPoses, List<Pose3d> robotPoses,
214-
List<Pose3d> robotPosesAccepted, List<Pose3d> robotPosesRejected) {
223+
private void logCameraData(
224+
int cameraIndex,
225+
List<Pose3d> tagPoses,
226+
List<Pose3d> robotPoses,
227+
List<Pose3d> robotPosesAccepted,
228+
List<Pose3d> robotPosesRejected
229+
) {
215230
Logger.recordOutput(
216231
"Vision/Camera" + cameraIndex + "/TagPoses",
217232
tagPoses.toArray(new Pose3d[0])
@@ -230,18 +245,15 @@ private void logCameraData(int cameraIndex, List<Pose3d> tagPoses, List<Pose3d>
230245
);
231246
}
232247

233-
234248
// Logs summary data for all cameras.
235-
private void logSummaryData(List<Pose3d> allTagPoses, List<Pose3d> allRobotPoses,
236-
List<Pose3d> allRobotPosesAccepted, List<Pose3d> allRobotPosesRejected) {
237-
Logger.recordOutput(
238-
"Vision/Summary/TagPoses",
239-
allTagPoses.toArray(new Pose3d[0])
240-
);
241-
Logger.recordOutput(
242-
"Vision/Summary/RobotPoses",
243-
allRobotPoses.toArray(new Pose3d[0])
244-
);
249+
private void logSummaryData(
250+
List<Pose3d> allTagPoses,
251+
List<Pose3d> allRobotPoses,
252+
List<Pose3d> allRobotPosesAccepted,
253+
List<Pose3d> allRobotPosesRejected
254+
) {
255+
Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[0]));
256+
Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[0]));
245257
Logger.recordOutput(
246258
"Vision/Summary/RobotPosesAccepted",
247259
allRobotPosesAccepted.toArray(new Pose3d[0])

‎src/main/java/frc/robot/Subsystems/Vision/VisionConstants.java‎

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -50,8 +50,10 @@ public class VisionConstants {
5050

5151
// Camera settings
5252
public static final double CAMERA_DEBOUNCE_TIME = 0.5;
53-
public static final CameraResolution FRONT_RIGHT_CAMERA_RESOLUTION = CameraResolution.HIGH_RESOLUTION;
54-
public static final CameraResolution FRONT_LEFT_CAMERA_RESOLUTION = CameraResolution.HIGH_RESOLUTION;
53+
public static final CameraResolution FRONT_RIGHT_CAMERA_RESOLUTION =
54+
CameraResolution.HIGH_RESOLUTION;
55+
public static final CameraResolution FRONT_LEFT_CAMERA_RESOLUTION =
56+
CameraResolution.HIGH_RESOLUTION;
5557

5658
// Field layout configuration
5759
public static final boolean USE_WELDED_FIELD = false; // Adjust based on competition setup
@@ -62,18 +64,18 @@ public class VisionConstants {
6264
);
6365

6466
// Camera properties
65-
public static final int CAMERA_WIDTH = 1200;
66-
public static final int CAMERA_HEIGHT = 800;
67+
public static final int CAMERA_WIDTH = 1200;
68+
public static final int CAMERA_HEIGHT = 800;
6769
public static final Rotation2d CAMERA_FOV = Rotation2d.fromDegrees(84.47);
6870
public static final double CALIB_ERROR_AVG = 0.25;
6971
public static final double CALIB_ERROR_STD_DEV = 0.08;
7072
public static final int CAMERA_FPS = 40;
71-
public static final int AVG_LATENCY_MS = 40;
73+
public static final int AVG_LATENCY_MS = 40;
7274
public static final int LATENCY_STD_DEV_MS = 10;
7375

7476
// Filtering thresholds for pose observations
75-
public static final double maxAmbiguity = 0.3;
76-
public static final double maxZError = 0.75;
77+
public static final double maxAmbiguity = 0.3;
78+
public static final double maxZError = 0.75;
7779

7880
public static final double linearStdDevBaseline = 0.02;
7981
public static final double angularStdDevBaseline = 0.06;

‎src/main/java/frc/robot/Subsystems/Vision/VisionIO.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ public static class VisionIOInputs {
2020
// Represents the angle to a simple target, not used for pose estimation.
2121
public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
2222

23-
// Represents a robot pose sample used for pose estimation.
23+
// Represents a robot pose sample used for pose estimation.
2424
public static record PoseObservation(
2525
double timestamp,
2626
Pose3d pose,

0 commit comments

Comments
 (0)