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 8abe784..f33fd1c 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 @@ -13,16 +13,22 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferSe 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.PoseVelocity2d; import com.acmerobotics.roadrunner.SequentialAction; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; 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.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -64,13 +70,13 @@ 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 = 30; + public static double pickupStackGateSpeed = 13; public static double intake2TimeGate = 3; public static double shoot2GateTime = 1.7; public static double endGateTime = 22; - public static double waitToPickupGateWithPartner = 0.7; + public static double waitToPickupGateWithPartner = 0.01; public static double waitToPickupGateSolo = 0.01; - public static double intakeGateTime = 8; + public static double intakeGateTime = 5; public static double shootGateTime = 1.7; public static double shoot1GateTime = 1.7; public static double intake1GateTime = 3.3; @@ -167,6 +173,7 @@ public class Auto_LT_Close extends LinearOpMode { while (opModeInInit()) { + if (gateCycle) { servos.setHoodPos(hoodGate0Start); } else { @@ -195,6 +202,10 @@ public class Auto_LT_Close extends LinearOpMode { } if (gamepad2.squareWasPressed()) { + + drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); + + if (!gateCycle) { turret.pipelineSwitch(1); } else if (redAlliance) { @@ -203,6 +214,9 @@ public class Auto_LT_Close extends LinearOpMode { turret.pipelineSwitch(2); } robot.limelight.start(); + + + gamepad2.rumble(500); } @@ -320,37 +334,14 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); + .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0)); } else { shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0)); + .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); } if (gateCycle) { - pickup1 = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate))) - .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) - .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), - new TranslationalVelConstraint(pickupStackGateSpeed)); - } else { - pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) - .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) - .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), - new TranslationalVelConstraint(pickup1Speed)); - } - - if (gateCycle) { - shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) - .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate)); - } else if (ballCycles < 2) { - shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) - .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); - } else { - shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); - } - - if (gateCycle) { - pickup2 = drive.actionBuilder(new Pose2d(xShoot0, yShoot0, Math.toRadians(hShoot0))) + pickup2 = shoot0.endTrajectory().fresh() .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), new TranslationalVelConstraint(pickupStackGateSpeed)); @@ -362,7 +353,7 @@ public class Auto_LT_Close extends LinearOpMode { } if (gateCycle) { - shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) + shoot2 = pickup2.endTrajectory().fresh() .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); } else if (ballCycles < 3) { shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) @@ -372,6 +363,43 @@ public class Auto_LT_Close extends LinearOpMode { .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); } + gateCyclePickup = shoot2.endTrajectory().fresh() + .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH)) + .waitSeconds(waitToPickupGate) + .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH)); + + gateCycleShoot = gateCyclePickup.endTrajectory().fresh() + .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + + + if (gateCycle) { + pickup1 = gateCycleShoot.endTrajectory().fresh() + .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) + .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), + new TranslationalVelConstraint(pickupStackGateSpeed)); + } else { + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) + .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) + .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), + new TranslationalVelConstraint(pickup1Speed)); + } + + + if (gateCycle) { + shoot1 = pickup1.endTrajectory().fresh() + .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate)); + } else if (ballCycles < 2) { + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + } else { + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + } + + + + + pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) .strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a)) .strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b), @@ -386,14 +414,10 @@ public class Auto_LT_Close extends LinearOpMode { waitToPickupGate = waitToPickupGateSolo; } - gateCyclePickup = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate))) - .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH)) - .waitSeconds(waitToPickupGate) - .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH)); - gateCycleShoot = drive.actionBuilder(new Pose2d(pickupGateBX, pickupGateBY, Math.toRadians(pickupGateBH))) - .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + + teleStart = drive.localizer.getPose(); TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Ball Cycles", ballCycles); @@ -411,6 +435,7 @@ public class Auto_LT_Close extends LinearOpMode { robot.transfer.setPower(1); + if (gateCycle) { startAutoGate(); shoot(); @@ -421,6 +446,7 @@ public class Auto_LT_Close extends LinearOpMode { cycleGateIntake(); if (getRuntime() - stamp < lastShootTime) { cycleGateShoot(); + shoot(); } } cycleStackCloseIntakeGate(); @@ -487,6 +513,8 @@ public class Auto_LT_Close extends LinearOpMode { ); } + + void startAutoGate() { assert shoot0 != null; @@ -498,7 +526,7 @@ public class Auto_LT_Close extends LinearOpMode { xShoot0, yShoot0, hShoot0, - false + true ) ) ); @@ -656,6 +684,11 @@ public class Auto_LT_Close extends LinearOpMode { } void cycleStackMiddleGate() { + drive.updatePoseEstimate(); + pickup2 = drive.actionBuilder(drive.localizer.getPose()) + .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) + .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), + new TranslationalVelConstraint(pickupStackGateSpeed)); Actions.runBlocking( new ParallelAction( pickup2.build(), @@ -684,6 +717,12 @@ 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(), @@ -698,7 +737,13 @@ public class Auto_LT_Close extends LinearOpMode { } void cycleGateShoot() { + + drive.updatePoseEstimate(); servos.setSpinPos(spinStartPos); + + gateCycleShoot = drive.actionBuilder(drive.localizer.getPose()) + .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + Actions.runBlocking( new ParallelAction( gateCycleShoot.build(), @@ -714,6 +759,14 @@ public class Auto_LT_Close extends LinearOpMode { } void cycleStackCloseIntakeGate() { + drive.updatePoseEstimate(); + + pickup1 = drive.actionBuilder(drive.localizer.getPose()) + .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) + .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), + new TranslationalVelConstraint(pickupStackGateSpeed)); + + Actions.runBlocking( new ParallelAction( pickup1.build(), @@ -729,6 +782,11 @@ public class Auto_LT_Close extends LinearOpMode { void cycleStackCloseShootGate(){ servos.setSpinPos(spinStartPos); + drive.updatePoseEstimate(); + + shoot1 = drive.actionBuilder(drive.localizer.getPose()) + .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate)); + Actions.runBlocking( new ParallelAction( shoot1.build(), 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 c124a2d..949a23c 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 @@ -10,10 +10,10 @@ 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 rx2a = 41, ry2a = 18, rh2a = 140; + public static double rx2a = 41, ry2a = 18, rh2a = 155; public static double bx2a = 41, by2a = -18, bh2a = -140; - public static double rx2b = 23, ry2b = 36, rh2b = 140.1; + public static double rx2b = 21, ry2b = 34, rh2b = 155.1; public static double bx2b = 23, by2b = -36, bh2b = -140.1; public static double rx3a = 55, ry3a = 39, rh3a = 140; @@ -42,8 +42,8 @@ public class Front_Poses { public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55; public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55; - public static double rShoot0X = 40, rShoot0Y = 0.1, rShoot0H = 0.1; - public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1; + public static double rShoot0X = 53, rShoot0Y = 10.1, rShoot0H = 80.1; + public static double bShoot0X = 53, bShoot0Y = -10.1, bShoot0H = -80.1; public static double rShootGateX = 50, rShootGateY = 10, rShootGateH = 90; public static double bShootGateX = 50, bShootGateY = -10, bShootGateH = -90; @@ -51,10 +51,10 @@ 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 = 24, rPickupGateAY = 50, rPickupGateAH = 140; + public static double rPickupGateAX = 26, rPickupGateAY = 48, rPickupGateAH = 140; public static double bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -140; - public static double rPickupGateBX = 38, rPickupGateBY = 68, rPickupGateBH = 180; + public static double rPickupGateBX = 35, rPickupGateBY = 65, rPickupGateBH = 180; public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180; public static Pose2d teleStart = new Pose2d(0, 0, 0); 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 39e45b8..f678d0a 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 @@ -5,18 +5,18 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.13; //0.13; + public static double spindexer_intakePos1 = 0.22; //0.13; - public static double spindexer_intakePos2 = 0.32; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.41; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.5; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.60; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.78; //0.65; //0.24; - public static double spindexer_outtakeBall3b = 0.22; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.88; //0.65; //0.24; + public static double spindexer_outtakeBall3b = 0.31; //0.65; //0.24; public static double spindexer_outtakeBall2 = 0.7; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.42; //0.27; //0.4; - public static double spinStartPos = 0.05; + public static double spindexer_outtakeBall1 = 0.51; //0.27; //0.4; + public static double spinStartPos = 0.14; public static double spinEndPos = 0.95; public static double shootAllSpindexerSpeedIncrease = 0.014; 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 9e5e180..d474760 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 @@ -210,9 +210,9 @@ public final class MecanumDrive { public double maxAngAccel = 2 * Math.PI; // path controller gains - public double axialGain = 6; - public double lateralGain = 6; - public double headingGain = 6; // shared with turn + public double axialGain = 6.0; + public double lateralGain = 6.0; + public double headingGain = 6.0; // shared with turn public double axialVelGain = 0.0; public double lateralVelGain = 0.0; @@ -352,9 +352,9 @@ public final class MecanumDrive { Pose2d error = txWorldTarget.value().minusExp(localizer.getPose()); - if ((t >= timeTrajectory.duration && error.position.norm() < 2 + if ((t >= timeTrajectory.duration && error.position.norm() < 1 && robotVelRobot.linearVel.norm() < 0.5) - || t >= timeTrajectory.duration + 0.5) { + || t >= timeTrajectory.duration + 0.25) { leftFront.setPower(0); leftBack.setPower(0); rightBack.setPower(0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java index 8d6471a..8546dde 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java @@ -49,6 +49,8 @@ public final class PinpointLocalizer implements Localizer { txWorldPinpoint = initialPose; } + + @Override public void setPose(Pose2d pose) { txWorldPinpoint = pose.times(txPinpointRobot.inverse()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index fb13310..0c6f178 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -187,7 +187,7 @@ public class Spindexer { distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); // Position 1 - if (distanceRearCenter < 65) { + if (distanceRearCenter < 48) { // Mark Ball Found newPos1Detection = true; @@ -209,7 +209,7 @@ public class Spindexer { // Position 2 // Find which ball position this is in the spindexer spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]; - if (distanceFrontDriver < 56) { + if (distanceFrontDriver < 50) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) {