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 f33fd1c..2ba0c8a 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 @@ -29,6 +29,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.acmerobotics.roadrunner.ftc.PinpointIMU; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -50,9 +51,9 @@ public class Auto_LT_Close extends LinearOpMode { public static double velGate0End = 2700, hoodGate0End = 0.35; public static double hood0MoveTime = 2; - public static double spindexerSpeedIncrease = 0.016; + public static double spindexerSpeedIncrease = 0.014; - public static double shootAllTime = 4; + public static double shootAllTime = 3.5; public static double intake1Time = 3.3; public static double intake2Time = 3.8; @@ -70,18 +71,22 @@ public class Auto_LT_Close extends LinearOpMode { public static double colorSenseTime = 1.2; public static double waitToShoot0 = 0.5; public static double waitToPickupGate2 = 0.3; - public static double pickupStackGateSpeed = 13; - public static double intake2TimeGate = 3; + public static double pickupStackGateSpeed = 19; + public static double intake2TimeGate = 5; public static double shoot2GateTime = 1.7; public static double endGateTime = 22; - public static double waitToPickupGateWithPartner = 0.01; + public static double waitToPickupGateWithPartner = 0.7; public static double waitToPickupGateSolo = 0.01; - public static double intakeGateTime = 5; - public static double shootGateTime = 1.7; + public static double intakeGateTime = 5.6; + public static double shootGateTime = 1.5; public static double shoot1GateTime = 1.7; public static double intake1GateTime = 3.3; public static double lastShootTime = 27; + public static double openGateX = 26; + public static double openGateY = 48; + public static double openGateH = Math.toRadians(155); + Robot robot; MultipleTelemetry TELE; MecanumDrive drive; @@ -171,6 +176,9 @@ public class Auto_LT_Close extends LinearOpMode { robot.light.setPosition(1); + hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); + + while (opModeInInit()) { @@ -201,8 +209,11 @@ public class Auto_LT_Close extends LinearOpMode { ballCycles--; } + + if (gamepad2.squareWasPressed()) { + drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); @@ -352,7 +363,11 @@ public class Auto_LT_Close extends LinearOpMode { new TranslationalVelConstraint(pickup1Speed)); } - if (gateCycle) { + if (gateCycle&& withPartner) { + shoot2 = pickup2.endTrajectory().fresh() + .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH)) + .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH)); + } else if (gateCycle) { shoot2 = pickup2.endTrajectory().fresh() .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); } else if (ballCycles < 3) { @@ -366,7 +381,10 @@ public class Auto_LT_Close extends LinearOpMode { gateCyclePickup = shoot2.endTrajectory().fresh() .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH)) .waitSeconds(waitToPickupGate) - .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH)); + .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH)) + .waitSeconds(0.1) + .strafeToLinearHeading(new Vector2d(pickupGateCX, pickupGateCY), Math.toRadians(pickupGateCH), + new TranslationalVelConstraint(13)); gateCycleShoot = gateCyclePickup.endTrajectory().fresh() .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); @@ -455,6 +473,8 @@ public class Auto_LT_Close extends LinearOpMode { cycleStackCloseShootGate(); } + shoot(); + } else { startAuto(); shoot(); @@ -526,7 +546,7 @@ public class Auto_LT_Close extends LinearOpMode { xShoot0, yShoot0, hShoot0, - true + false ) ) ); @@ -693,7 +713,7 @@ public class Auto_LT_Close extends LinearOpMode { new ParallelAction( pickup2.build(), autoActions.intake( - intake2Time, + intake2TimeGate, x3b, y3b, h3b @@ -711,7 +731,7 @@ public class Auto_LT_Close extends LinearOpMode { motif, xShootGate, yShootGate, - hShootGate) + pickupGateAH) ) ); } @@ -719,10 +739,6 @@ public class Auto_LT_Close extends LinearOpMode { void cycleGateIntake() { drive.updatePoseEstimate(); - gateCyclePickup = drive.actionBuilder(drive.localizer.getPose()) - .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH)) - .waitSeconds(waitToPickupGate) - .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH)); Actions.runBlocking( new ParallelAction( gateCyclePickup.build(), @@ -742,7 +758,7 @@ public class Auto_LT_Close extends LinearOpMode { servos.setSpinPos(spinStartPos); gateCycleShoot = drive.actionBuilder(drive.localizer.getPose()) - .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH)); Actions.runBlocking( new ParallelAction( @@ -751,7 +767,7 @@ public class Auto_LT_Close extends LinearOpMode { shootGateTime, xShootGate, yShootGate, - hShootGate, + pickupGateAH, false ) ) 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 68ebeec..f9697dc 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 @@ -63,7 +63,7 @@ public class Auto_LT_Far extends LinearOpMode { boolean stack3 = true; double xStackPickupA, yStackPickupA, hStackPickupA; double xStackPickupB, yStackPickupB, hStackPickupB; - public static int pickupStackSpeed = 12; + public static int pickupStackSpeed = 17; public static int pickupGateSpeed = 25; int prevMotif = 0; public static double spindexerSpeedIncrease = 0.014; @@ -74,8 +74,8 @@ public class Auto_LT_Far extends LinearOpMode { public static double shootStackTime = 2; public static double shootGateTime = 2.5; public static double colorSenseTime = 1; - public static double intakeStackTime = 2.5; - public static double intakeGateTime = 2; + public static double intakeStackTime = 4.5; + public static double intakeGateTime = 8; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; @@ -184,6 +184,8 @@ public class Auto_LT_Far extends LinearOpMode { if (gamepad2.squareWasPressed()){ turret.pipelineSwitch(4); robot.limelight.start(); + drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); + gamepad2.rumble(500); } } else { @@ -221,6 +223,8 @@ public class Auto_LT_Far extends LinearOpMode { if (gamepad2.squareWasPressed()){ turret.pipelineSwitch(2); robot.limelight.start(); + drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); + gamepad2.rumble(500); } } @@ -240,7 +244,10 @@ public class Auto_LT_Far extends LinearOpMode { .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), + .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH)) + .waitSeconds(0.2) + .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB)) + .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC), new TranslationalVelConstraint(pickupGateSpeed)); shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) 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 15f99a9..c5fcfd3 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 @@ -10,14 +10,18 @@ public class Back_Poses { public static double rShootX = 100, rShootY = 55, rShootH = 90; public static double bShootX = 100, bShootY = -55, bShootH = -90; - public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140; + public static double rStackPickupAX = 73, rStackPickupAY = 51, rStackPickupAH = 140; public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; - public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1; + public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1; public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1; - public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140; + public static double rPickupGateX = 50, rPickupGateY = 83, rPickupGateH = 140; public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140; + public static double pickupGateXB = 84, pickupGateYB = 76, pickupGateHB = 140; + public static double pickupGateXC = 50, pickupGateYC = 83, pickupGateHC = 190; + + public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50; 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 949a23c..a947943 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 @@ -51,11 +51,12 @@ public class Front_Poses { public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55; public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55; - public static double rPickupGateAX = 26, rPickupGateAY = 48, rPickupGateAH = 140; - public static double bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -140; + public static double rPickupGateAX = 31, rPickupGateAY = 53, rPickupGateAH = 150; + public static double bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -150; - public static double rPickupGateBX = 35, rPickupGateBY = 65, rPickupGateBH = 180; + public static double rPickupGateBX = 38, rPickupGateBY = 62, rPickupGateBH = 210; public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180; + public static double pickupGateCX = 34, pickupGateCY = 58, pickupGateCH = 220; public static Pose2d teleStart = new Pose2d(0, 0, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java index d474760..86ab581 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java @@ -354,7 +354,7 @@ public final class MecanumDrive { if ((t >= timeTrajectory.duration && error.position.norm() < 1 && robotVelRobot.linearVel.norm() < 0.5) - || t >= timeTrajectory.duration + 0.25) { + || t >= timeTrajectory.duration + 0.01) { leftFront.setPower(0); leftBack.setPower(0); rightBack.setPower(0);