Skip to content

Fix Poses (o-ωｑ)).oO #189

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
+45 −30
Open

Fix Poses (o-ωｑ)).oO #189

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
7ea8eb4
Update Field.java
Wu-Fan-529 Oct 17, 2025
3254b68
Update Constants.java
Wu-Fan-529 Oct 17, 2025
f2c4635
quick fix
Wu-Fan-529 Oct 18, 2025
1bfb030
Merge branch 'main' into pose-fixes
EliUva Oct 18, 2025
1a1a109
Merge branch 'main' into pose-fixes
EliUva Oct 18, 2025
3c562d7
10/18, 21:52
EliUva Oct 19, 2025
68643cf
Update Constants.java
EliUva Oct 21, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -142,7 +142,7 @@ public static class constDrivetrain {
public static final AngularVelocity TURN_SPEED = Units.DegreesPerSecond.of(360);
public static final double SLOW_MODE_MULTIPLIER = 0.5;

public static final boolean INVERT_ROTATION = true;
public static final boolean INVERT_ROTATION = !Robot.isSimulation();
Copy link

Copilot AI Oct 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[nitpick] To reduce coupling between Constants and your Robot class, prefer WPILib's RobotBase utility: import edu.wpi.first.wpilibj.RobotBase; and use public static final boolean INVERT_ROTATION = !RobotBase.isSimulation();

Copilot uses AI. Check for mistakes.

// -- Motor Configurations --
static {
Expand Down Expand Up @@ -671,8 +671,8 @@ public static class PoseDriveGroup {
public boolean lockX;
public boolean lockY;
public boolean backwardsAllowed;
public Distance distanceTolerance = Units.Inches.of(1);
public Angle rotationTolerance = Units.Degrees.of(2);
public Distance distanceTolerance = Units.Inches.of(0.5);
public Angle rotationTolerance = Units.Degrees.of(0.5);
}

public static class constPoseDrive {
Expand Down
49 changes: 31 additions & 18 deletions src/main/java/frc/robot/Field.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,18 +29,18 @@ public class Field {
private static class FieldElements {
private static final Pose2d REEF_CENTER = new Pose2d(4.5, 4.0, Rotation2d.kZero);
// Branch Poses
private static final Pose2d REEF_A = new Pose2d(3.171, 4.189, Rotation2d.kZero);
private static final Pose2d REEF_B = new Pose2d(3.171, 3.863, Rotation2d.kZero);
private static final Pose2d REEF_C = new Pose2d(3.688, 2.968, Rotation2d.fromDegrees(60));
private static final Pose2d REEF_D = new Pose2d(3.975, 2.803, Rotation2d.fromDegrees(60));
private static final Pose2d REEF_E = new Pose2d(5.001, 2.804, Rotation2d.fromDegrees(120));
private static final Pose2d REEF_F = new Pose2d(5.285, 2.964, Rotation2d.fromDegrees(120));
private static final Pose2d REEF_G = new Pose2d(5.805, 3.863, Rotation2d.k180deg);
private static final Pose2d REEF_H = new Pose2d(5.805, 4.189, Rotation2d.k180deg);
private static final Pose2d REEF_I = new Pose2d(5.288, 5.083, Rotation2d.fromDegrees(-120));
private static final Pose2d REEF_J = new Pose2d(5.002, 5.248, Rotation2d.fromDegrees(-120));
private static final Pose2d REEF_K = new Pose2d(3.972, 5.247, Rotation2d.fromDegrees(-60));
private static final Pose2d REEF_L = new Pose2d(3.693, 5.079, Rotation2d.fromDegrees(-60));
private static final Pose2d REEF_A = new Pose2d(3.206, 4.189, Rotation2d.kZero);
private static final Pose2d REEF_B = new Pose2d(3.206, 3.863, Rotation2d.kZero);
private static final Pose2d REEF_C = new Pose2d(3.715, 3.005, Rotation2d.fromDegrees(60));
private static final Pose2d REEF_D = new Pose2d(3.983, 2.846, Rotation2d.fromDegrees(60));
private static final Pose2d REEF_E = new Pose2d(4.971, 2.847, Rotation2d.fromDegrees(120));
private static final Pose2d REEF_F = new Pose2d(5.275, 3.017, Rotation2d.fromDegrees(120));
private static final Pose2d REEF_G = new Pose2d(5.775, 3.858, Rotation2d.k180deg);
private static final Pose2d REEF_H = new Pose2d(5.775, 4.199, Rotation2d.k180deg);
private static final Pose2d REEF_I = new Pose2d(5.263, 5.041, Rotation2d.fromDegrees(-120));
private static final Pose2d REEF_J = new Pose2d(5.007, 5.187, Rotation2d.fromDegrees(-120));
private static final Pose2d REEF_K = new Pose2d(3.995, 5.211, Rotation2d.fromDegrees(-60));
private static final Pose2d REEF_L = new Pose2d(3.712, 5.046, Rotation2d.fromDegrees(-60));

// L2 Backwards
private static final Pose2d REEF_A_L2_BACKWARDS = new Pose2d(2.848, 4.189, Rotation2d.k180deg);
Expand All @@ -65,12 +65,25 @@ private static class FieldElements {
Rotation2d.fromDegrees(180));

// algae poses
private static final Pose2d ALGAE_AB = REEF_A.interpolate(REEF_B, 0.5);
private static final Pose2d ALGAE_CD = REEF_C.interpolate(REEF_D, 0.5);
private static final Pose2d ALGAE_EF = REEF_E.interpolate(REEF_F, 0.5);
private static final Pose2d ALGAE_GH = REEF_G.interpolate(REEF_H, 0.5);
private static final Pose2d ALGAE_IJ = REEF_I.interpolate(REEF_J, 0.5);
private static final Pose2d ALGAE_KL = REEF_K.interpolate(REEF_L, 0.5);
private static final Pose2d ALGAE_A = new Pose2d(3.171, 4.189, Rotation2d.kZero);
private static final Pose2d ALGAE_B = new Pose2d(3.171, 3.863, Rotation2d.kZero);
private static final Pose2d ALGAE_C = new Pose2d(3.688, 2.968, Rotation2d.fromDegrees(60));
private static final Pose2d ALGAE_D = new Pose2d(3.975, 2.803, Rotation2d.fromDegrees(60));
private static final Pose2d ALGAE_E = new Pose2d(5.001, 2.804, Rotation2d.fromDegrees(120));
private static final Pose2d ALGAE_F = new Pose2d(5.285, 2.964, Rotation2d.fromDegrees(120));
private static final Pose2d ALGAE_G = new Pose2d(5.805, 3.863, Rotation2d.k180deg);
private static final Pose2d ALGAE_H = new Pose2d(5.805, 4.189, Rotation2d.k180deg);
private static final Pose2d ALGAE_I = new Pose2d(5.288, 5.083, Rotation2d.fromDegrees(-120));
private static final Pose2d ALGAE_J = new Pose2d(5.002, 5.248, Rotation2d.fromDegrees(-120));
private static final Pose2d ALGAE_K = new Pose2d(3.972, 5.247, Rotation2d.fromDegrees(-60));
private static final Pose2d ALGAE_L = new Pose2d(3.693, 5.079, Rotation2d.fromDegrees(-60));

private static final Pose2d ALGAE_AB = ALGAE_A.interpolate(ALGAE_B, 0.5);
private static final Pose2d ALGAE_CD = ALGAE_C.interpolate(ALGAE_D, 0.5);
private static final Pose2d ALGAE_EF = ALGAE_E.interpolate(ALGAE_F, 0.5);
private static final Pose2d ALGAE_GH = ALGAE_G.interpolate(ALGAE_H, 0.5);
private static final Pose2d ALGAE_IJ = ALGAE_I.interpolate(ALGAE_J, 0.5);
private static final Pose2d ALGAE_KL = ALGAE_K.interpolate(ALGAE_L, 0.5);

// Coral Station
private static final Pose2d LEFT_CORAL_STATION_FAR = new Pose2d(1.64, 7.33, Rotation2d.fromDegrees(125));
Expand Down
7 changes: 5 additions & 2 deletions src/main/java/frc/robot/commands/driver_states/PoseDriving.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,12 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants.PoseDriveGroup;
import frc.robot.Constants.constField;
import frc.robot.Field;
import frc.robot.Robot;
Copy link

Copilot AI Oct 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This import appears unused in this file. Please remove it to keep imports clean, unless you plan to use it in this class.

Suggested change
import frc.robot.Robot;

Copilot uses AI. Check for mistakes.
import frc.robot.Field.FieldElementGroups;
import frc.robot.subsystems.DriverStateMachine;
import frc.robot.subsystems.Drivetrain;
Expand All @@ -29,7 +31,8 @@ public class PoseDriving extends Command {
private boolean isPoseAligned = false;

public PoseDriving(Drivetrain subDrivetrain, DriverStateMachine subDriverStateMachine, StateMachine subStateMachine,
DoubleSupplier xAxis, DoubleSupplier yAxis, DoubleSupplier rotationAxis, BooleanSupplier slowMode, PoseDriveGroup poseGroup) {
DoubleSupplier xAxis, DoubleSupplier yAxis, DoubleSupplier rotationAxis, BooleanSupplier slowMode,
PoseDriveGroup poseGroup) {
this.subDrivetrain = subDrivetrain;
this.subStateMachine = subStateMachine;
this.subDriverStateMachine = subDriverStateMachine;
Expand Down Expand Up @@ -104,6 +107,6 @@ public void end(boolean interrupted) {
public boolean isFinished() {
isPoseAligned = subDrivetrain.isAtPosition(closestPose, poseGroup.distanceTolerance) &&
subDrivetrain.isAtRotation(closestPose.getRotation(), poseGroup.rotationTolerance);
return isPoseAligned;
return isPoseAligned & DriverStation.isAutonomous();
Copy link

Copilot AI Oct 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use the short-circuit logical AND (&&) instead of bitwise AND (&) for booleans to avoid unnecessary evaluation and match Java conventions: return isPoseAligned && DriverStation.isAutonomous();

Suggested change
return isPoseAligned & DriverStation.isAutonomous();
return isPoseAligned && DriverStation.isAutonomous();

Copilot uses AI. Check for mistakes.
}
}