diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java index 0a739a9..bc1a211 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java @@ -100,13 +100,23 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double spindexerSpeedIncrease = 0.03; - public static double obeliskTurrPos1 = 0.52; - public static double obeliskTurrPos2 = 0.53; - public static double obeliskTurrPos3 = 0.54; + public static double redObeliskTurrPos1 = 0.52; + public static double redObeliskTurrPos2 = 0.53; + public static double redObeliskTurrPos3 = 0.54; + + public static double blueObeliskTurrPos1 = 0.28; + public static double blueObeliskTurrPos2 = 0.27; + public static double blueObeliskTurrPos3 = 0.26; + double obeliskTurrPos1 = 0.0; + double obeliskTurrPos2 = 0.0; + double obeliskTurrPos3 = 0.0; public static double normalIntakeTime = 3.0; public static double shoot1Turr = 0.57; public static double shoot0XTolerance = 1.0; - public static double turretShootPos = 0.52; + public static double redTurretShootPos = 0.52; + public static double blueTurretShootPos = 0.26; + double turretShootPos = 0.0; + public static double shootAllTime = 1.8; public static double shoot0Time = 1.6; public static double intake1Time = 3.0; @@ -799,6 +809,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { yShoot = rShootY; hShoot = rShootH; + obeliskTurrPos1 = redObeliskTurrPos1; + obeliskTurrPos2 = redObeliskTurrPos2; + obeliskTurrPos3 = redObeliskTurrPos3; + turretShootPos = redTurretShootPos; + } else { robot.light.setPosition(0.6); @@ -834,6 +849,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { yShoot = bShootY; hShoot = bShootH; + obeliskTurrPos1 = blueObeliskTurrPos1; + obeliskTurrPos2 = blueObeliskTurrPos2; + obeliskTurrPos3 = blueObeliskTurrPos3; + turretShootPos = blueTurretShootPos; + } shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index 100cc66..f52fb10 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -26,23 +26,23 @@ public class Poses { public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140); public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1); - public static double bx1 = 40, by1 = 7, bh1 = 0; - public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140); - public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140); + public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1); + public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140); + public static double bx2b = 23, by2b = -36, bh2b = Math.toRadians(-140.1); public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140); - public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140); - public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140); + public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140); + public static double bx3b = 38, by3b = -56, bh3b = Math.toRadians(-140.1); - public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140); - public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140); + public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140); + public static double bx4b = 45, by4b = -83, bh4b = Math.toRadians(-140.1); public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50); public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50); - public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140); - public static double bxPrep = 50, byPrep = -10, bhPrep = Math.toRadians(140); + public static double bShootX = 40, bShootY = 7, bShootH = Math.toRadians(-50); + public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50); public static Pose2d teleStart = new Pose2d(0, 0, 0);