diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index fb08f5a..156f0b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -3,26 +3,14 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; -import androidx.annotation.NonNull; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.ParallelAction; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; @@ -42,8 +30,6 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; import org.firstinspires.ftc.teamcode.utils.Turret; -import java.util.Objects; - @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Far extends LinearOpMode { @@ -66,13 +52,13 @@ public class Auto_LT_Far extends LinearOpMode { Light light; double xShoot, yShoot, hShoot; double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; - double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; public static double flywheel0Time = 1.5; - boolean gatePickup = false; + boolean gatePickup = true; boolean stack3 = true; double xStackPickupA, yStackPickupA, hStackPickupA; double xStackPickupB, yStackPickupB, hStackPickupB; public static int pickupStackSpeed = 12; + public static int pickupGateSpeed = 25; int prevMotif = 0; public static double spindexerSpeedIncrease = 0.008; public static double shootAllTime = 2; @@ -82,8 +68,8 @@ public class Auto_LT_Far extends LinearOpMode { public static double shootStackTime = 2; public static double shootGateTime = 2; public static double colorSenseTime = 1; - public static double intakeStackTime = 3.3; - public static double intakeGateTime = 3; + public static double intakeStackTime = 3; + public static double intakeGateTime = 2.5; public static double redObeliskTurrPos1 = 0.12; public static double redObeliskTurrPos2 = 0.13; public static double redObeliskTurrPos3 = 0.14; @@ -93,12 +79,15 @@ public class Auto_LT_Far extends LinearOpMode { double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; + public static double endAutoTime = 25; // initialize path variables here TrajectoryActionBuilder leave3Ball = null; TrajectoryActionBuilder leaveFromShoot = null; TrajectoryActionBuilder pickup3 = null; TrajectoryActionBuilder shoot3 = null; + TrajectoryActionBuilder pickupGate = null; + TrajectoryActionBuilder shootGate = null; Pose2d autoStart = new Pose2d(0,0,0); @Override @@ -183,6 +172,10 @@ public class Auto_LT_Far extends LinearOpMode { yStackPickupB = rStackPickupBY; hStackPickupB = rStackPickupBH; + pickupGateX = rPickupGateX; + pickupGateY = rPickupGateY; + pickupGateH = rPickupGateH; + obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos3 = turrDefault + redObeliskTurrPos3; @@ -216,6 +209,10 @@ public class Auto_LT_Far extends LinearOpMode { yStackPickupB = bStackPickupBY; hStackPickupB = bStackPickupBH; + pickupGateX = bPickupGateX; + pickupGateY = bPickupGateY; + pickupGateH = bPickupGateH; + obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; @@ -242,6 +239,13 @@ public class Auto_LT_Far extends LinearOpMode { shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH), + new TranslationalVelConstraint(pickupGateSpeed)); + + shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + limelightUsed = true; TELE.addData("Red?", redAlliance); @@ -259,6 +263,7 @@ public class Auto_LT_Far extends LinearOpMode { // Currently only shoots; keep this start and modify times and then add extra paths if (opModeIsActive()) { + double stamp = getRuntime(); robot.transfer.setPower(1); @@ -270,7 +275,10 @@ public class Auto_LT_Far extends LinearOpMode { shoot(); } - //TODO: insert code here for gate auto + while (gatePickup && getRuntime() - stamp < endAutoTime){ + cycleGatePickup(); + shoot(); + } if (gatePickup || stack3){ leave(); @@ -388,4 +396,51 @@ public class Auto_LT_Far extends LinearOpMode { ) ); } + + void cycleGatePickup(){ + Actions.runBlocking( + new ParallelAction( + pickupGate.build(), + autoActions.manageShooterAuto( + intakeGateTime, + pickupGateX, + pickupGateY, + posXTolerance, + posYTolerance, + pickupGateH, + true + ), + autoActions.intake(intakeStackTime), + autoActions.detectObelisk( + intakeGateTime, + pickupGateX, + pickupGateY, + posXTolerance, + posYTolerance, + obeliskTurrPos3 + ) + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + autoActions.manageShooterAuto( + shootGateTime, + xShoot, + yShoot, + posXTolerance, + posYTolerance, + hShoot, + false + ), + shootGate.build(), + autoActions.prepareShootAll(colorSenseTime, shootGateTime, motif) + ) + ); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 6ed4a94..643a917 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -14,9 +14,12 @@ public class Back_Poses { public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140; public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; - public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1; + public static double rStackPickupBX = 55, rStackPickupBY = 83, rStackPickupBH = 140.1; public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1; + public static double rPickupGateX = 65, rPickupGateY = 80, rPickupGateH = 140; + public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140; + public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;