diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index cdcb50b..07c645a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -87,6 +87,11 @@ public class Targeting { public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); + if (redAlliance){ + sin54 = Math.sin(Math.toRadians(54)); + } else { + sin54 = Math.sin(Math.toRadians(-54)) + } // TODO: test these values determined from the fmap double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; @@ -102,6 +107,16 @@ public class Targeting { int remX = Math.floorMod((int) robotInchesX, tileSize); int remY = Math.floorMod((int) robotInchesY, tileSize); + //clamp + + //if (redAlliance) { + robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); + robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); + //} else { +// robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); +// robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); + //} + // Determine if we need to interpolate based on tile position. // if near upper or lower quarter or tile interpolate with next tile. int x0 = 0; @@ -173,16 +188,6 @@ public class Targeting { interpolate = false; } - //clamp - - if (redAlliance) { - robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); - robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); - } else { - robotGridY = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); - robotGridX = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); - } - // basic search if (true) { //!interpolate) { if ((robotGridY < 6) && (robotGridX < 6)) {