Skip to content

Commit 1f2bc17

Browse files
pix-xlpix-xl
committed
Update AutoAlign.java
1 parent 5799d34 commit 1f2bc17

File tree

1 file changed

+68
-59
lines changed

1 file changed

+68
-59
lines changed

‎src/main/java/frc/robot/vision/commands/AutoAlign.java‎

Lines changed: 68 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,8 @@ public class AutoAlign extends Command {
6363
double xSpeed;
6464
double ySpeed;
6565
double omegaSpeed;
66+
Supplier<ReefFace> nearestReefFace;
67+
boolean isLeftAlign;
6668

6769
private final static CommandSwerveDrivetrain drivetrainSubsystem = CommandSwerveDrivetrain.getInstance();
6870
// private final FieldCentric fieldCentricSwerveRequest = new FieldCentric()
@@ -84,69 +86,16 @@ public class AutoAlign extends Command {
8486
public AutoAlign(Supplier<Pose2d> targetPose, Supplier<ReefFace> nearestReefFace, boolean isLeftAlign) {
8587
this(drivetrainSubsystem, poseProvider);
8688
this.goalPose2d = targetPose.get();
89+
this.nearestReefFace = nearestReefFace;
90+
this.isLeftAlign = isLeftAlign;
91+
8792
//we are getting ATag Pose
8893
//firstly if elevator is L4, update the AprilTag X, Y
8994
//need to do - transform for left or right wrist
9095
//else
9196
//need to do - transform for left or right wrist
92-
boolean isRightWrist = (RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight) ||
93-
(RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right) ||
94-
(RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist) ||
95-
(RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist) ||
96-
(RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist) ||
97-
(RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid) ||
98-
(RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid) ||
99-
(RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid);
100-
boolean isElevatorL4 = (ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralL4) ||
101-
(ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralL4AutonScore) ||
102-
(ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralScoreL4);
103-
104-
SmartDashboard.putBoolean("isRightWrist - AutoAlign", isRightWrist);
105-
SmartDashboard.putBoolean("isElevatorL4 - AutoAlign", isElevatorL4);
106-
SmartDashboard.putString("elevator kLastState - AutoAlign", ElevatorSubsystem.kLastState.name());
107-
SmartDashboard.putString("alliance - AutoAlign", DriverStation.getAlliance().toString());
108-
SmartDashboard.putBoolean("requesting lineup left branch - AutoAlign", isLeftAlign);
109-
SmartDashboard.putString("nearest ReefFace accessed - AutoAlign", nearestReefFace.get().name());
110-
if(isElevatorL4 == true) {
111-
System.out.println("REACHED ELEVATOR L4 IF STATEMENT IN AUTOALIGN");
112-
ReefFace newReefFace = updateReefFace(nearestReefFace.get());
113-
SmartDashboard.putString("updated ReefFace - AutoAlign", newReefFace.name());
114-
goalPose2d = new Pose2d(newReefFace.aprilTagX, newReefFace.aprilTagY, Rotation2d.fromDegrees(newReefFace.aprilTagTheta));
115-
if(isRightWrist) {
116-
if(isLeftAlign) {
117-
goalPose2d = goalPose2d.transformBy(leftBranchTransformRightWrist);
118-
}
119-
else {
120-
goalPose2d = goalPose2d.transformBy(rightBranchTransformRightWrist);
121-
}
122-
}
123-
else {
124-
if(isLeftAlign) {
125-
goalPose2d = goalPose2d.transformBy(leftBranchTransformLeftWrist);
126-
}
127-
else {
128-
goalPose2d = goalPose2d.transformBy(rightBranchTransformLeftWrist);
129-
}
130-
}
131-
}
132-
else {
133-
if(isRightWrist) {
134-
if(isLeftAlign) {
135-
goalPose2d = goalPose2d.transformBy(leftBranchTransformRightWrist);
136-
}
137-
else {
138-
goalPose2d = goalPose2d.transformBy(rightBranchTransformRightWrist);
139-
}
140-
}
141-
else {
142-
if(isLeftAlign) {
143-
goalPose2d = goalPose2d.transformBy(leftBranchTransformLeftWrist);
144-
}
145-
else {
146-
goalPose2d = goalPose2d.transformBy(rightBranchTransformLeftWrist);
147-
}
148-
}
149-
}
97+
98+
15099

151100
goalPose2d = Conversions.rotatePose(goalPose2d.transformBy(robotOffset), Rotation2d.kZero);
152101

@@ -264,6 +213,66 @@ public void initialize() {
264213
public void execute() {
265214
var robotPose = poseProvider.get();
266215

216+
boolean isRightWrist = (RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight) ||
217+
(RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right) ||
218+
(RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist) ||
219+
(RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist) ||
220+
(RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist) ||
221+
(RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid) ||
222+
(RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid) ||
223+
(RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid);
224+
boolean isElevatorL4 = (ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralL4) ||
225+
(ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralL4AutonScore) ||
226+
(ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralScoreL4);
227+
228+
SmartDashboard.putBoolean("isRightWrist - AutoAlign", isRightWrist);
229+
SmartDashboard.putBoolean("isElevatorL4 - AutoAlign", isElevatorL4);
230+
SmartDashboard.putString("elevator kLastState - AutoAlign", ElevatorSubsystem.kLastState.name());
231+
SmartDashboard.putString("alliance - AutoAlign", DriverStation.getAlliance().toString());
232+
SmartDashboard.putBoolean("requesting lineup left branch - AutoAlign", isLeftAlign);
233+
SmartDashboard.putString("nearest ReefFace accessed - AutoAlign", nearestReefFace.get().name());
234+
235+
if(isElevatorL4 == true) {
236+
System.out.println("REACHED ELEVATOR L4 IF STATEMENT IN AUTOALIGN");
237+
ReefFace newReefFace = updateReefFace(nearestReefFace.get());
238+
SmartDashboard.putString("updated ReefFace - AutoAlign", newReefFace.name());
239+
goalPose2d = new Pose2d(newReefFace.aprilTagX, newReefFace.aprilTagY, Rotation2d.fromDegrees(newReefFace.aprilTagTheta));
240+
if(isRightWrist) {
241+
if(isLeftAlign) {
242+
goalPose2d = goalPose2d.transformBy(leftBranchTransformRightWrist);
243+
}
244+
else {
245+
goalPose2d = goalPose2d.transformBy(rightBranchTransformRightWrist);
246+
}
247+
}
248+
else {
249+
if(isLeftAlign) {
250+
goalPose2d = goalPose2d.transformBy(leftBranchTransformLeftWrist);
251+
}
252+
else {
253+
goalPose2d = goalPose2d.transformBy(rightBranchTransformLeftWrist);
254+
}
255+
}
256+
}
257+
else {
258+
if(isRightWrist) {
259+
if(isLeftAlign) {
260+
goalPose2d = goalPose2d.transformBy(leftBranchTransformRightWrist);
261+
}
262+
else {
263+
goalPose2d = goalPose2d.transformBy(rightBranchTransformRightWrist);
264+
}
265+
}
266+
else {
267+
if(isLeftAlign) {
268+
goalPose2d = goalPose2d.transformBy(leftBranchTransformLeftWrist);
269+
}
270+
else {
271+
goalPose2d = goalPose2d.transformBy(rightBranchTransformLeftWrist);
272+
}
273+
}
274+
}
275+
267276
xSpeed = xController.calculate(robotPose.getX(), this.goalPose2d.getX());
268277
if (xController.atSetpoint()) {
269278
xSpeed = 0;
@@ -279,7 +288,7 @@ public void execute() {
279288
omegaSpeed = 0;
280289
}
281290

282-
ChassisSpeeds speeds = applyLimits(new ChassisSpeeds(-xSpeed, -ySpeed, omegaSpeed));
291+
ChassisSpeeds speeds = applyLimits(new ChassisSpeeds(xSpeed, ySpeed, omegaSpeed));
283292

284293
// CommandSwerveDrivetrain.getInstance().applyRequest( () ->
285294
// RobotContainer.drive.withVelocityX(speeds.vxMetersPerSecond)

0 commit comments

Comments
 (0)