diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java index aeb0d7b..86dbb2a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java @@ -73,6 +73,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Poses_V2; @@ -84,6 +85,7 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; import org.firstinspires.ftc.teamcode.utils.Turret; +@Disabled @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close_12Ball extends LinearOpMode { 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 1b38734..41c736f 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 @@ -96,7 +96,7 @@ import java.util.Objects; public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.022; + public static double shoot0SpinSpeedIncrease = 0.015; public static double spindexerSpeedIncrease = 0.03; @@ -106,12 +106,12 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double normalIntakeTime = 3.0; public static double shoot1Turr = 0.57; public static double shoot0XTolerance = 1.0; - public static double turretShootPos = 0.53; + public static double turretShootPos = 0.52; public static double shootAllTime = 1.8; public static double shoot0Time = 1.6; public static double intake1Time = 3.0; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 17; + public static double pickup1Speed = 19; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shoot1Hood = 0.93; @@ -128,12 +128,12 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double obelisk1YTolerance = 2.0; public static double shoot1ToleranceX = 2.0; public static double shoot1ToleranceY = 2.0; - public static double shoot1Time = 2.5; - public static double shoot2Time = 2.5; - public static double shoot3Time = 2.5; - public static double colorSenseTime = 0.8; + public static double shoot1Time = 2; + public static double shoot2Time = 2; + public static double shoot3Time = 2; + public static double colorSenseTime = 1.2; - public static double firstShootTime = 0.5; + public static double firstShootTime = 0.3; public int motif = 0; Robot robot; @@ -488,8 +488,13 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { } ticker++; - + robot.limelight.pipelineSwitch(1); motif = turret.detectObelisk(); + if (redAlliance){ + robot.limelight.pipelineSwitch(4); + } else { + robot.limelight.pipelineSwitch(2); + } robot.turr1.setPosition(turrPos); robot.turr2.setPosition(1 - turrPos); 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 458e63b..d8c2bbd 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 @@ -23,8 +23,8 @@ public class Poses { public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); public static double rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1); - public static double rx4a = 72, ry4a = 65, rh4a = Math.toRadians(145); - public static double rx4b = 37, ry4b = 85, rh4b = Math.toRadians(145.1); + public static double rx4a = 72, ry4a = 50, rh4a = Math.toRadians(145); + public static double rx4b = 42, ry4b = 80, rh4b = Math.toRadians(145.1); public static double bx1 = 40, by1 = 7, bh1 = 0; public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140);