From 72741998db1047ad10f4ab6a52baac1b43884cee Mon Sep 17 00:00:00 2001 From: HiHi3690 Date: Thu, 9 Jan 2025 02:31:39 -0500 Subject: [PATCH] Mirrors properly for alliance. Documentation in comments added. Further changes will most likely be formatting/readability only. --- src/main/java/frc/robot/Constants.java | 29 ++++--- .../java/frc/robot/util/ReefSelector.java | 78 +++++++++++++------ 2 files changed, 72 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d915867..ab72433 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -304,19 +304,26 @@ public static final class FieldConstants { public static final Translation2d kFeedingSetpoint = new Translation2d(7.137, 0.872); // Reefscape constants - public static final Translation2d kRedReef = - new Translation2d(Units.inchesToMeters(177.92), Units.inchesToMeters(159.79)); - public static final Translation2d kBlueReef = - new Translation2d(Units.inchesToMeters(514.66), Units.inchesToMeters(159.79)); + public static final Translation2d kRedReefCenter = + new Translation2d(4.50, 4.03); // unused for now + public static final Translation2d kBlueReefCenter = + new Translation2d(13.06, 4.03); // unused for now + public static final Translation2d kRedCoralTip = + new Translation2d(3.62, 3.86); // unused for now + public static final Translation2d kReefDisplacementToCenter = + new Translation2d(4.28, 0); // mathed from above + public static final Translation2d kFieldCenter = + new Translation2d(8.78, 4.03); // mathed from above public static final Translation2d kBranchOffsetXY = - new Translation2d(Units.inchesToMeters(32.38), Units.inchesToMeters(6.814)); + new Translation2d(0.81, 0.17); // mathed from above + public static final double kBranchInsetL4 = + 0.05; // 0.81 - 0.76 // 0.76 being the x component of the L4 branch offset public static final double kL1Height = Units.inchesToMeters(27.0); - public static final double kL2Height = Units.inchesToMeters(31.72); - public static final double kL3Height = Units.inchesToMeters(47.59); - public static final double kL4Height = Units.inchesToMeters(72); + public static final double kL2Height = Units.inchesToMeters(32); + public static final double kL3Height = Units.inchesToMeters(48); + public static final double kL4Height = Units.inchesToMeters(73); - public static final Rotation3d kLowWristAngle = - new Rotation3d(0, Units.degreesToRadians(145), 0); - public static final Rotation3d kL4WristAngle = new Rotation3d(0, Units.degreesToRadians(90), 0); + public static final double kLowWristPitch = Units.degreesToRadians(145); + public static final double kL4WristPitch = Units.degreesToRadians(90); } } diff --git a/src/main/java/frc/robot/util/ReefSelector.java b/src/main/java/frc/robot/util/ReefSelector.java index 99bd6fc..3248885 100644 --- a/src/main/java/frc/robot/util/ReefSelector.java +++ b/src/main/java/frc/robot/util/ReefSelector.java @@ -18,28 +18,42 @@ public ReefSelector() { } public Pose3d getPose() { - // do fancy maths and logic to determine offset from center to selected branch - Translation2d xyOffset = - FieldConstants.kBlueReef.plus( - new Translation2d( - FieldConstants.kBranchOffsetXY.getX(), - FieldConstants.kBranchOffsetXY.getY() * Math.pow(-1, m_position % 2)) - .rotateBy( - new Rotation2d( - Units.degreesToRadians(60 * Math.floor((m_position - 0.5) / 2))))); - - // do similar logic for rotation heading: - Rotation3d wristHeading = - FieldConstants.kLowWristAngle.rotateBy( - new Rotation3d(0, 0, Units.degreesToRadians(60 * Math.floor((m_position - 0.5) / 2)))); - if (m_height == 4) { - wristHeading = - FieldConstants.kL4WristAngle.rotateBy( - new Rotation3d( - 0, 0, Units.degreesToRadians(60 * Math.floor((m_position - 0.5) / 2)))); - } - // and finally, logic for the height: + // do fancy maths and logic to determine offset from center of reef to selected branch + + // Main logic for selecting branch position relative to reef center: + // 1. Start with face closer to DS, right is positon 1, CW is positive increments + // 2. For each branch, pick the correct X inset based on height (L4 is slightly closer) + // 2. For each pair, pick the correct branch based on parity of m_position + // 3. Rotate to correct branch pair based on m_position. 1,2 -> 0, 3,4 -> 60, 5,6 -> 120, etc. + // This is the job of the janky floor division + + // From here, we take the reef Position and add the branch offset, then make this relative first + // to field center, flipping based on alliance color, then mapping to origin + + Translation2d xyPosRelToCenter = + FieldConstants.kReefDisplacementToCenter + .plus( + new Translation2d( + FieldConstants.kBranchOffsetXY.getX() + - ((m_height == 4) + ? FieldConstants.kBranchInsetL4 + : 0), // pull setpoint closer to reef for L4 + FieldConstants.kBranchOffsetXY.getY() + * Math.pow(-1, m_position % 2)) // flip y pose for odd positions + .rotateBy( + new Rotation2d( + Units.degreesToRadians( + 60 + * Math.floor( + (m_position - 0.5) / 2))))) // rotate to correct branch pair + .rotateBy( + Rotation2d.fromDegrees(AllianceFlipUtil.shouldFlip() ? 180 : 0)); // flip if on red + + Translation2d xyPos = + FieldConstants.kFieldCenter.plus(xyPosRelToCenter); // translate to origin-relative + + // Pick the correct height based on m_height double height = 0; if (m_height == 1) { height = FieldConstants.kL1Height; @@ -51,10 +65,26 @@ public Pose3d getPose() { height = FieldConstants.kL4Height; } - // create and return final Pose3d - return new Pose3d(xyOffset.getX(), xyOffset.getY(), height, wristHeading); + // Calculate wrist angles based on height and position + double wristRollRad = 0; // will be 0 for all reef positions + double wristPitchRad = + m_height != 4 + ? FieldConstants.kLowWristPitch + : FieldConstants.kL4WristPitch; // adjust angle if on L4 + double wristYawRad = + Units.degreesToRadians( + 60 * Math.floor((m_position - 0.5) / 2) + + (AllianceFlipUtil.shouldFlip() ? 180 : 0)); // flip if on red + + // Create and return final Pose3d + return new Pose3d( + xyPos.getX(), + xyPos.getY(), + height, + new Rotation3d(wristRollRad, wristPitchRad, wristYawRad)); } + // Getters and setters public int getHeight() { return m_height; } @@ -91,7 +121,7 @@ public void decrementPosition() { } } - public void setPosition(int position, int height) { + public void setPoint(int position, int height) { m_position = position; m_height = height; }