Skip to content

Commit 644fbb3

Browse files
KaraParikh1KaraParikh1
committed
spotless apply
1 parent 48c7dbe commit 644fbb3

5 files changed

Lines changed: 91 additions & 81 deletions

File tree

‎src/main/java/frc/robot/commands/DrivetrainCommand.java‎

Lines changed: 45 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -9,9 +9,7 @@
99
import edu.wpi.first.math.geometry.Rotation2d;
1010
import edu.wpi.first.math.kinematics.SwerveModuleState;
1111
import edu.wpi.first.math.trajectory.TrapezoidProfile;
12-
import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint;
1312
import edu.wpi.first.wpilibj2.command.Command;
14-
import edu.wpi.first.wpilibj2.command.Subsystem;
1513
import frc.robot.config.TunerConstants;
1614
import frc.robot.helpers.ApplyModuleStates;
1715
import frc.robot.helpers.ShotCalculator;
@@ -24,7 +22,7 @@ public static enum Position {
2422
TELEOP,
2523
STILL_SHOT,
2624
SOTM,
27-
AUTO_ALIGN_HUB
25+
AUTO_ALIGN_HUB
2826
}
2927

3028
private CommandSwerveDrivetrain subsystem;
@@ -72,19 +70,21 @@ public DrivetrainCommand(
7270
double alignI = 1;
7371
double alignD = 1;
7472
this.autoAlignPidController =
75-
new ProfiledPIDController(
76-
alignP,
77-
alignI,
78-
alignD,
79-
new TrapezoidProfile.Constraints(MaxAngularRate, MaxAngularRate / 0.2));
80-
this.autoAlignPidController.enableContinuousInput(-Math.PI, Math.PI); // Swerve angles are continuous (-180 to 180 deg)
73+
new ProfiledPIDController(
74+
alignP,
75+
alignI,
76+
alignD,
77+
new TrapezoidProfile.Constraints(MaxAngularRate, MaxAngularRate / 0.2));
78+
this.autoAlignPidController.enableContinuousInput(
79+
-Math.PI, Math.PI); // Swerve angles are continuous (-180 to 180 deg)
8180
// Don't compute hub-facing target in the constructor (DriverStation alliance may be
8281
// unavailable during initialization). Initialize to current heading; execute() will
8382
// recompute the actual hub-facing target each loop.
8483
this.m_targetAngle = subsystem.getState().Pose.getRotation();
8584

86-
addRequirements(subsystem);
85+
addRequirements(subsystem);
8786
}
87+
8888
public double getAutoAlignRotationalOutput() {
8989
Rotation2d currentAngle = subsystem.getState().Pose.getRotation();
9090
double current = currentAngle.getRadians();
@@ -97,6 +97,7 @@ public double getAutoAlignRotationalOutput() {
9797
}
9898
return output;
9999
}
100+
100101
@Override
101102
public void execute() {
102103
switch (pose) {
@@ -127,33 +128,38 @@ public void execute() {
127128
System.out.println("Drivetrain: SOTM Drive");
128129
break;
129130

130-
case AUTO_ALIGN_HUB:
131-
// Recompute the desired heading toward the hub each loop
132-
m_targetAngle =
133-
ShotCalculator.getRotationTowardsHub(
134-
ShotCalculator.calculateHubPosition(), subsystem.getState().Pose.getTranslation());
135-
System.out.println("Auto Align Current Pose: "+subsystem.getState().Pose.getX()+" "+subsystem.getState().Pose.getY());
136-
137-
double currentRad = subsystem.getState().Pose.getRotation().getRadians();
138-
double targetRad = m_targetAngle.getRadians();
139-
// Normalize error to [-pi, pi]
140-
double error = Math.atan2(Math.sin(targetRad - currentRad), Math.cos(targetRad - currentRad));
141-
double absError = Math.abs(error);
142-
double angleTolerance = Math.toRadians(1.0); // stop within 1 degree
143-
144-
double rotOutput = getAutoAlignRotationalOutput();
145-
146-
if (absError < angleTolerance) {
147-
// Aligned: stop rotating (and hold position)
148-
subsystem.setControl(drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(0.0));
149-
} else {
150-
// Not aligned: apply rotational output
151-
System.out.println("new rotational output: "+rotOutput);
152-
subsystem.setControl(
153-
drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(rotOutput));
154-
}
155-
156-
break;
131+
case AUTO_ALIGN_HUB:
132+
// Recompute the desired heading toward the hub each loop
133+
m_targetAngle =
134+
ShotCalculator.getRotationTowardsHub(
135+
ShotCalculator.calculateHubPosition(), subsystem.getState().Pose.getTranslation());
136+
System.out.println(
137+
"Auto Align Current Pose: "
138+
+ subsystem.getState().Pose.getX()
139+
+ " "
140+
+ subsystem.getState().Pose.getY());
141+
142+
double currentRad = subsystem.getState().Pose.getRotation().getRadians();
143+
double targetRad = m_targetAngle.getRadians();
144+
// Normalize error to [-pi, pi]
145+
double error =
146+
Math.atan2(Math.sin(targetRad - currentRad), Math.cos(targetRad - currentRad));
147+
double absError = Math.abs(error);
148+
double angleTolerance = Math.toRadians(1.0); // stop within 1 degree
149+
150+
double rotOutput = getAutoAlignRotationalOutput();
151+
152+
if (absError < angleTolerance) {
153+
// Aligned: stop rotating (and hold position)
154+
subsystem.setControl(drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(0.0));
155+
} else {
156+
// Not aligned: apply rotational output
157+
System.out.println("new rotational output: " + rotOutput);
158+
subsystem.setControl(
159+
drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(rotOutput));
160+
}
161+
162+
break;
157163

158164
default:
159165
break;
@@ -174,7 +180,8 @@ public boolean isFinished() {
174180
return false;
175181
}
176182

177-
edu.wpi.first.math.geometry.Translation2d currentTrans = subsystem.getState().Pose.getTranslation();
183+
edu.wpi.first.math.geometry.Translation2d currentTrans =
184+
subsystem.getState().Pose.getTranslation();
178185
if (currentTrans == null) {
179186
return false;
180187
}

‎src/main/java/frc/robot/config/VisionConfig.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,19 +2,19 @@
22

33
import edu.wpi.first.apriltag.AprilTagFieldLayout;
44
import edu.wpi.first.apriltag.AprilTagFields;
5-
import edu.wpi.first.math.geometry.Rotation3d;
6-
import edu.wpi.first.math.geometry.Transform3d;
7-
import edu.wpi.first.math.util.Units;
85
import edu.wpi.first.math.Matrix;
96
import edu.wpi.first.math.VecBuilder;
7+
import edu.wpi.first.math.geometry.Rotation3d;
8+
import edu.wpi.first.math.geometry.Transform3d;
109
import edu.wpi.first.math.numbers.N1;
1110
import edu.wpi.first.math.numbers.N3;
11+
import edu.wpi.first.math.util.Units;
1212
import org.photonvision.PhotonPoseEstimator;
1313
import org.photonvision.targeting.PhotonPipelineResult;
1414

1515
public class VisionConfig {
1616
public static PhotonPipelineResult result = new PhotonPipelineResult();
17-
public static final String FRONT_CAMERA_NAME = "forwardAprilTag";
17+
public static final String FRONT_CAMERA_NAME = "forwardAprilTag";
1818
// Per-camera measurement standard deviations: [x (m), y (m), theta (rad)]
1919
public static final Matrix<N3, N1> FRONT_VISION_STDDEVS =
2020
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(3.0));
@@ -31,7 +31,7 @@ public class VisionConfig {
3131
Units.inchesToMeters(-11.55),
3232
Units.inchesToMeters(14.88),
3333
new Rotation3d(0.0, Units.degreesToRadians(16), 0.0));
34-
public static final Transform3d REAR_CAMERA_POSITION =
34+
public static final Transform3d REAR_CAMERA_POSITION =
3535
new Transform3d(
3636
Units.inchesToMeters(0.0),
3737
Units.inchesToMeters(0.0),

‎src/main/java/frc/robot/helpers/ShotCalculator.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -41,13 +41,13 @@ public static Translation2d calculateHubPosition() {
4141
return null;
4242
}
4343

44-
public static Rotation2d getRotationTowardsHub(Translation2d hubpose, Translation2d currentpose){
45-
double deltaX = hubpose.getX() - currentpose.getX();
46-
double deltaY = hubpose.getY() - currentpose.getY();
44+
public static Rotation2d getRotationTowardsHub(Translation2d hubpose, Translation2d currentpose) {
45+
double deltaX = hubpose.getX() - currentpose.getX();
46+
double deltaY = hubpose.getY() - currentpose.getY();
4747

48-
double angleRadians = Math.atan2(deltaY, deltaX);
48+
double angleRadians = Math.atan2(deltaY, deltaX);
4949

50-
return new Rotation2d(angleRadians);
50+
return new Rotation2d(angleRadians);
5151
}
5252

5353
// Determines active based on current match time

‎src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java‎

Lines changed: 34 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -232,18 +232,20 @@ private void initVisionPoseEstimator() {
232232
this.m_FrontPhotonPoseEstimator =
233233
new PhotonPoseEstimator(fieldLayout, VisionConfig.FORWARD_CAMERA_POSITION);
234234
} catch (Exception e) {
235-
DriverStation.reportError("Failed to create front PhotonPoseEstimator: " + e.getMessage(), true);
235+
DriverStation.reportError(
236+
"Failed to create front PhotonPoseEstimator: " + e.getMessage(), true);
236237
this.m_FrontPhotonPoseEstimator = null;
237238
}
238-
try {
239-
AprilTagFieldLayout fieldLayout =
240-
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
241-
this.m_RearPhotonPoseEstimator =
242-
new PhotonPoseEstimator(fieldLayout, VisionConfig.REAR_CAMERA_POSITION);
243-
} catch (Exception e) {
244-
DriverStation.reportError("Failed to create rear PhotonPoseEstimator: " + e.getMessage(), true);
245-
this.m_RearPhotonPoseEstimator = null;
246-
}
239+
try {
240+
AprilTagFieldLayout fieldLayout =
241+
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
242+
this.m_RearPhotonPoseEstimator =
243+
new PhotonPoseEstimator(fieldLayout, VisionConfig.REAR_CAMERA_POSITION);
244+
} catch (Exception e) {
245+
DriverStation.reportError(
246+
"Failed to create rear PhotonPoseEstimator: " + e.getMessage(), true);
247+
this.m_RearPhotonPoseEstimator = null;
248+
}
247249
}
248250

249251
/**
@@ -325,32 +327,32 @@ public void periodic() {
325327
});
326328
}
327329
}
328-
if (m_RearPhotonPoseEstimator != null) {
329-
var rearResults = Vision.RearCameraApril.getAllUnreadResults();
330-
331-
for (var result : rearResults) {
332-
// 2. Use the 2026 explicit methods to calculate pose
333-
var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result);
330+
if (m_RearPhotonPoseEstimator != null) {
331+
var rearResults = Vision.RearCameraApril.getAllUnreadResults();
334332

335-
// Fallback to single tag if multi-tag isn't available
336-
if (visionEst.isEmpty()) {
337-
var bestTarget = result.getBestTarget();
338-
if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) {
339-
visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result);
340-
}
341-
}
333+
for (var result : rearResults) {
334+
// 2. Use the 2026 explicit methods to calculate pose
335+
var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result);
342336

343-
// 3. Apply the successful estimation to the CTRE odometry and log to file
344-
visionEst.ifPresent(
345-
est -> {
346-
Pose2d pose = est.estimatedPose.toPose2d();
347-
double ts = est.timestampSeconds;
348-
SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose);
349-
SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds");
350-
addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS);
351-
});
337+
// Fallback to single tag if multi-tag isn't available
338+
if (visionEst.isEmpty()) {
339+
var bestTarget = result.getBestTarget();
340+
if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) {
341+
visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result);
352342
}
343+
}
344+
345+
// 3. Apply the successful estimation to the CTRE odometry and log to file
346+
visionEst.ifPresent(
347+
est -> {
348+
Pose2d pose = est.estimatedPose.toPose2d();
349+
double ts = est.timestampSeconds;
350+
SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose);
351+
SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds");
352+
addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS);
353+
});
353354
}
355+
}
354356
}
355357

356358
private void startSimThread() {

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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,5 +7,6 @@
77
public class Vision extends SubsystemBase {
88
public static final PhotonCamera FrontCameraApril =
99
new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME);
10-
public static final PhotonCamera RearCameraApril = new PhotonCamera(VisionConfig.REAR_CAMERA_NAME);
10+
public static final PhotonCamera RearCameraApril =
11+
new PhotonCamera(VisionConfig.REAR_CAMERA_NAME);
1112
}

0 commit comments

Comments
 (0)