diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index fb9ca7f..cc5af40 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos0; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3; @@ -371,7 +372,7 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = bPickupGateBY; pickupGateBH = bPickupGateBH; - obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0; + obeliskTurrPosAutoStart = turrDefault + blueObeliskTurrPos0; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 10d140f..ccfb8d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -7,8 +7,8 @@ import com.acmerobotics.roadrunner.Pose2d; public class Front_Poses { - public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; - public static double bx1 = 20, by1 = -0.5, bh1 = -0.1; + public static double rx1 = 30, ry1 = 5, rh1 = 0.1; + public static double bx1 = 30, by1 = -5, bh1 = -0.1; public static double rx2a = 41, ry2a = 18, rh2a = 140; public static double bx2a = 41, by2a = -18, bh2a = -140; @@ -33,8 +33,8 @@ public class Front_Poses { public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; - public static double rShootX = 40, rShootY = 10, rShootH = 50; - public static double bShootX = 40, bShootY = -10, bShootH = -50; + public static double rShootX = 60, rShootY = 10, rShootH = 50; + public static double bShootX = 60, bShootY = -10, bShootH = -50; public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; public static double bxPrep = 45, byPrep = -10, bhPrep = -50; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 3a05cdd..465d608 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -16,10 +16,10 @@ public class ServoPositions { public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6; public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4; - public static double spinStartPos = 0.10; + public static double spinStartPos = 0.05; public static double spinEndPos = 0.95; - public static double shootAllSpindexerSpeedIncrease = 0.013; + public static double shootAllSpindexerSpeedIncrease = 0.008; public static double transferServo_out = 0.15; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 6dc4a9b..a519e0a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -361,8 +361,8 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData("robotInchesY", targeting.robotInchesY); // TELE.addData("Targeting Interpolate", turretInterpolate); -// TELE.addData("Targeting GridX", targeting.robotGridX); -// TELE.addData("Targeting GridY", targeting.robotGridY); + TELE.addData("Targeting GridX", targeting.robotGridX); + TELE.addData("Targeting GridY", targeting.robotGridY); // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); @@ -376,7 +376,7 @@ public class TeleopV3 extends LinearOpMode { TELE.update(); - //light.update(); + light.update(); ticker++; } 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 b738fac..bec1745 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,35 +87,47 @@ public class Targeting { public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); + int gridX; + int gridY; if (!redAlliance){ sin54 = Math.sin(Math.toRadians(54)); + double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; + double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; + + // Convert robot coordinates to inches + robotInchesX = rotatedX * unitConversionFactor + 20; + robotInchesY = rotatedY * unitConversionFactor + 20; + + // Find approximate location in the grid + gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize)); + gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); } else { sin54 = Math.sin(Math.toRadians(-54)); + double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; + double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; + + // Convert robot coordinates to inches + robotInchesX = rotatedX * unitConversionFactor; + robotInchesY = rotatedY * unitConversionFactor; + + // Find approximate location in the grid + gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); + gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); } - // TODO: test these values determined from the fmap - double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; - double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; - // Convert robot coordinates to inches - robotInchesX = rotatedX * unitConversionFactor; - robotInchesY = rotatedY * unitConversionFactor; - - // Find approximate location in the grid - int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); - int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); int remX = Math.floorMod((int) robotInchesX, tileSize); int remY = Math.floorMod((int) robotInchesY, tileSize); //clamp - //if (redAlliance) { + 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)); - //} + } 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.