Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 08:48:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Robot Reveal

Browse Source
This commit is contained in:
C4llSiqn
2025-02-21 16:54:18 -07:00
parent c5c6dd2eed
commit c9a947d3b8
6 changed files with 47 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/utility/ReefPositionHelper.java
+15 -2
View File
@@ -12,7 +12,8 @@ import frc4388.robot.Constants.FieldConstants;
public class ReefPositionHelper {
public enum Side {
LEFT,
RIGHT
RIGHT,
CENTER
}
public static final Pose2d[] RED_TAGS = {
@@ -74,10 +75,22 @@ public class ReefPositionHelper {
public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
return offset(getNearestTag(position),
(side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim,
getSide(side) + xtrim,
ydistance);
}
public static double getSide(Side side){
switch(side) {
case LEFT:
return -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
case RIGHT:
return (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
case CENTER:
return 0;
}
return 0;
}
public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
Translation2d oldTranslation = oldPose.getTranslation();