From f9013f4d792b96b8c1e662498d331771e0c7cd57 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 27 Feb 2026 23:34:22 -0600 Subject: [PATCH] stash --- .../teamcode/autonomous/Auto_LT_Close.java | 1 + .../ftc/teamcode/autonomous/Auto_LT_Far.java | 268 ++++-------------- .../autonomous/actions/AutoActions.java | 42 ++- .../ftc/teamcode/constants/Back_Poses.java | 30 +- .../teamcode/constants/ServoPositions.java | 2 +- .../ftc/teamcode/teleop/TeleopV3.java | 29 +- .../ftc/teamcode/tests/SortingTest.java | 85 +++--- .../ftc/teamcode/utils/Drivetrain.java | 34 ++- .../ftc/teamcode/utils/Light.java | 12 +- .../ftc/teamcode/utils/Spindexer.java | 36 ++- .../ftc/teamcode/utils/Turret.java | 28 +- 11 files changed, 251 insertions(+), 316 deletions(-) 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 36050fb..1ac064d 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 @@ -180,6 +180,7 @@ public class Auto_LT_Close extends LinearOpMode { servos.setTransferPos(transferServo_out); limelightUsed = false; + Spindexer.teleop = false; robot.light.setPosition(1); 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 eb7dd17..5d7a25d 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 @@ -1,6 +1,11 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAY; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBX; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBY; 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.blueObeliskTurrPos1; @@ -12,9 +17,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObelisk import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; -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_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; @@ -60,19 +62,14 @@ public class Auto_LT_Far extends LinearOpMode { AutoActions autoActions; Light light; double xShoot, yShoot, hShoot; - double pickupGateXA = 0, pickupGateYA = 0, pickupGateHA = 0; - double pickupGateXB = 0, pickupGateYB = 0, pickupGateHB = 0; - double pickupGateXC = 0, pickupGateYC = 0, pickupGateHC = 0; + double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; public static double flywheel0Time = 1.5; boolean gatePickup = true; boolean stack3 = true; - boolean stack2 = true; - double xStackPickupFarA, yStackPickupFarA, hStackPickupFarA; - double xStackPickupFarB, yStackPickupFarB, hStackPickupFarB; - double xStackPickupMiddleA, yStackPickupMiddleA, hStackPickupMiddleA; - double xStackPickupMiddleB, yStackPickupMiddleB, hStackPickupMiddleB; - public static int pickupStackSpeed = 17; - public static int pickupGateSpeed = 25; + double xStackPickupA, yStackPickupA, hStackPickupA; + double xStackPickupB, yStackPickupB, hStackPickupB; + public static int pickupStackSpeed = 12; + public static int pickupGateSpeed = 30; int prevMotif = 0; public static double spindexerSpeedIncrease = 0.014; public static double shootAllTime = 2; @@ -82,8 +79,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 = 4.5; - public static double intakeGateTime = 8; + public static double intakeStackTime = 2.5; + public static double intakeGateTime = 2; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; @@ -96,7 +93,6 @@ public class Auto_LT_Far extends LinearOpMode { TrajectoryActionBuilder shoot3 = null; TrajectoryActionBuilder pickupGate = null; TrajectoryActionBuilder shootGate = null; - TrajectoryActionBuilder pickup2 = null; Pose2d autoStart = new Pose2d(0,0,0); @Override @@ -127,46 +123,16 @@ public class Auto_LT_Far extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.setTurret(turrDefault); - - servos.setSpinPos(spinStartPos); - - servos.setTransferPos(transferServo_out); - - limelightUsed = false; + Spindexer.teleop = false; while (opModeInInit()) { - if (limelightUsed){ - Actions.runBlocking( - autoActions.detectObelisk( - 0.1, - 0.501, - 0.501, - 0.501, - 0.501, - turrDefault - ) - ); - motif = turret.getObeliskID(); - if (motif == 21){ - AutoActions.firstSpindexShootPos = spindexer_outtakeBall1; - } else if (motif == 22){ - AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b; - } else { - AutoActions.firstSpindexShootPos = spindexer_outtakeBall2; - } - } - - if (gamepad2.triangleWasPressed()){ + if (gamepad2.leftBumperWasPressed()){ gatePickup = !gatePickup; } if (gamepad2.rightBumperWasPressed()){ stack3 = !stack3; } - if (gamepad2.leftBumperWasPressed()){ - stack2 = !stack2; - } turret.setTurret(turretShootPos); @@ -199,33 +165,17 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = rShootY; hShoot = rShootH; - xStackPickupFarA = rStackPickupAX; - yStackPickupFarA = rStackPickupAY; - hStackPickupFarA = rStackPickupAH; + xStackPickupA = rStackPickupFarAX; + yStackPickupA = rStackPickupFarAY; + hStackPickupA = rStackPickupFarAH; - xStackPickupFarB = rStackPickupBX; - yStackPickupFarB = rStackPickupBY; - hStackPickupFarB = rStackPickupBH; + xStackPickupB = rStackPickupFarBX; + yStackPickupB = rStackPickupFarBY; + hStackPickupB = rStackPickupFarBH; - xStackPickupMiddleA = rStackPickupAX; - yStackPickupMiddleA = rStackPickupAY; - hStackPickupMiddleA = rStackPickupAH; - - xStackPickupMiddleB = rStackPickupBX; - yStackPickupMiddleB = rStackPickupBY; - hStackPickupMiddleB = rStackPickupBH; - - pickupGateXA = rPickupGateXA; - pickupGateYA = rPickupGateYA; - pickupGateHA = rPickupGateHA; - - pickupGateXB = rPickupGateXB; - pickupGateYB = rPickupGateYB; - pickupGateHB = rPickupGateHB; - - pickupGateXC = rPickupGateXC; - pickupGateYC = rPickupGateYC; - pickupGateHC = rPickupGateHC; + pickupGateX = rPickupGateXA; + pickupGateY = rPickupGateYA; + pickupGateH = rPickupGateHA; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -233,10 +183,15 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + redTurretShootPos; if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(1); + turret.pipelineSwitch(4); robot.limelight.start(); - limelightUsed = true; gamepad2.rumble(500); + sleep(1000); + turret.setTurret(turrDefault); + + servos.setSpinPos(spinStartPos); + + servos.setTransferPos(transferServo_out); } } else { robot.light.setPosition(0.6); @@ -253,33 +208,17 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = bShootY; hShoot = bShootH; - xStackPickupFarA = bStackPickupAX; - yStackPickupFarA = bStackPickupAY; - hStackPickupFarA = bStackPickupAH; + xStackPickupA = bStackPickupFarAX; + yStackPickupA = bStackPickupFarAY; + hStackPickupA = bStackPickupFarAH; - xStackPickupFarB = bStackPickupBX; - yStackPickupFarB = bStackPickupBY; - hStackPickupFarB = bStackPickupBH; + xStackPickupB = bStackPickupFarBX; + yStackPickupB = bStackPickupFarBY; + hStackPickupB = bStackPickupFarBH; - xStackPickupMiddleA = bStackPickupAX; - yStackPickupMiddleA = bStackPickupAY; - hStackPickupMiddleA = bStackPickupAH; - - xStackPickupMiddleB = bStackPickupBX; - yStackPickupMiddleB = bStackPickupBY; - hStackPickupMiddleB = bStackPickupBH; - - pickupGateXA = bPickupGateXA; - pickupGateYA = bPickupGateYA; - pickupGateHA = bPickupGateHA; - - pickupGateXB = bPickupGateXB; - pickupGateYB = bPickupGateYB; - pickupGateHB = bPickupGateHB; - - pickupGateXC = bPickupGateXC; - pickupGateYC = bPickupGateYC; - pickupGateHC = bPickupGateHC; + pickupGateX = bPickupGateXA; + pickupGateY = bPickupGateYA; + pickupGateH = bPickupGateHA; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; @@ -287,10 +226,15 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + blueTurretShootPos; if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(5); + turret.pipelineSwitch(2); robot.limelight.start(); - limelightUsed = true; gamepad2.rumble(500); + sleep(1000); + turret.setTurret(turrDefault); + + servos.setSpinPos(spinStartPos); + + servos.setTransferPos(transferServo_out); } } @@ -301,33 +245,26 @@ public class Auto_LT_Far extends LinearOpMode { .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(xStackPickupFarA, yStackPickupFarA), Math.toRadians(hStackPickupFarA)) - .strafeToLinearHeading(new Vector2d(xStackPickupFarB, yStackPickupFarB), Math.toRadians(hStackPickupFarB), + .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA)) + .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB), new TranslationalVelConstraint(pickupStackSpeed)); - pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(xStackPickupMiddleA, yStackPickupMiddleA), Math.toRadians(hStackPickupMiddleA)) - .strafeToLinearHeading(new Vector2d(xStackPickupMiddleB, yStackPickupMiddleB), Math.toRadians(hStackPickupMiddleB), - new TranslationalVelConstraint(pickupStackSpeed)); - - shoot3 = drive.actionBuilder(new Pose2d(xStackPickupFarB, yStackPickupFarB, Math.toRadians(hStackPickupFarB))) + 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(pickupGateXA, pickupGateYA), Math.toRadians(pickupGateHA)) - .waitSeconds(0.2) - .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB)) - .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC), + .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH), new TranslationalVelConstraint(pickupGateSpeed)); - shootGate = drive.actionBuilder(new Pose2d(pickupGateXC, pickupGateYC, Math.toRadians(pickupGateHC))) + shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + limelightUsed = true; + TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Gate Cycle?", gatePickup); - TELE.addData("Pickup Stack Far?", stack3); - TELE.addData("Pickup Stack Middle?", stack2); + TELE.addData("Pickup Stack?", stack3); TELE.addData("Start Position", autoStart); TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.update(); @@ -351,11 +288,6 @@ public class Auto_LT_Far extends LinearOpMode { shoot(); } - if (stack2){ - cycleStackMiddle(); - shoot(); - } - while (gatePickup && getRuntime() - stamp < endAutoTime){ cycleGatePickupBalls(); if (getRuntime() - stamp > endAutoTime){ @@ -432,79 +364,16 @@ public class Auto_LT_Far extends LinearOpMode { pickup3.build(), autoActions.intake( intakeStackTime, - xStackPickupFarB, - yStackPickupFarB, - hStackPickupFarB - ), - autoActions.detectObelisk( - intakeStackTime, - xStackPickupFarB, - yStackPickupFarB, - posXTolerance, - posYTolerance, - obeliskTurrPos3 + xStackPickupB, + yStackPickupB, + hStackPickupB ) - ) ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = 22; - prevMotif = motif; - + servos.setSpinPos(spinStartPos); Actions.runBlocking( new ParallelAction( - shoot3.build(), - autoActions.prepareShootAll( - colorSenseTime, - shootStackTime, - motif, - xShoot, - yShoot, - hShoot) - ) - ); - } - - - void cycleStackMiddle(){ - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - autoActions.intake( - intakeStackTime, - xStackPickupMiddleB, - yStackPickupMiddleB, - hStackPickupMiddleB - ), - autoActions.detectObelisk( - intakeStackTime, - xStackPickupMiddleB, - yStackPickupMiddleB, - posXTolerance, - posYTolerance, - obeliskTurrPos2 - ) - - ) - ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; - - Actions.runBlocking( - new ParallelAction( - shoot3.build(), - autoActions.prepareShootAll( - colorSenseTime, - shootStackTime, - motif, - xShoot, - yShoot, - hShoot) + shoot3.build() ) ); } @@ -515,25 +384,12 @@ public class Auto_LT_Far extends LinearOpMode { pickupGate.build(), autoActions.intake( intakeStackTime, - pickupGateXC, - pickupGateYC, - pickupGateHC - ), - autoActions.detectObelisk( - intakeGateTime, - pickupGateXC, - pickupGateYC, - posXTolerance, - posYTolerance, - obeliskTurrPos3 + pickupGateX, + pickupGateY, + pickupGateH ) ) ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; } void cycleGatePrepareShoot(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 3f07acd..e4db014 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -55,7 +55,7 @@ public class AutoActions { private boolean shootForward = true; public int motif = 0; double spinEndPos = ServoPositions.spinEndPos; - + private boolean intaking = false; public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig) { this.robot = rob; this.drive = dri; @@ -427,8 +427,10 @@ public class AutoActions { if ((System.currentTimeMillis() - stamp) > (time * 1000)) { servos.setSpinPos(spindexer_intakePos1); + intaking = false; return false; } else { + intaking = true; return true; } } @@ -672,6 +674,44 @@ public class AutoActions { } }; } + + public Action ShakeDrivetrain( + double time + ){ + return new Action() { + int ticker = 0; + double stamp = 0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0){ + stamp = System.currentTimeMillis(); + } + ticker++; + + double currentStamp = System.currentTimeMillis(); + if (currentStamp - stamp < time*1000 && (intaking || ticker < 50)) { + if (ticker % 10000 < 5000) { + robot.frontLeft.setPower(0.5); + robot.backLeft.setPower(0.5); + robot.frontRight.setPower(0.5); + robot.backRight.setPower(0.5); + } else { + robot.frontLeft.setPower(-0.5); + robot.backLeft.setPower(-0.5); + robot.frontRight.setPower(-0.5); + robot.backRight.setPower(-0.5); + } + return true; + } else { + robot.frontLeft.setPower(0); + robot.backLeft.setPower(0); + robot.frontRight.setPower(0); + robot.backRight.setPower(0); + return false; + } + } + }; + } } 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 1b1af7e..05190f4 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 @@ -7,23 +7,29 @@ public class Back_Poses { public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1; public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50; - public static double rShootX = 100, rShootY = 55, rShootH = 90; - public static double bShootX = 100, bShootY = -55, bShootH = -90; + public static double rShootX = 100, rShootY = 60, rShootH = 145.2; + public static double bShootX = 100, bShootY = -55, bShootH = -145.2; - public static double rStackPickupAX = 73, rStackPickupAY = 51, rStackPickupAH = 140; - public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; + public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145; + public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145; - public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1; - public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1; + public static double rStackPickupFarBX = 53, rStackPickupFarBY = 71, rStackPickupFarBH = 145.1; + public static double bStackPickupFarBX = 55, bStackPickupFarBY = -71, bStackPickupFarBH = -145.1; - public static double rPickupGateXA = 50, rPickupGateYA = 83, rPickupGateHA = 140; - public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -140; - public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 140; - public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -140; + public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145; + public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145; + + public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1; + public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1; + + public static double rPickupGateXA = 60, rPickupGateYA = 83, rPickupGateHA = 145; + public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -145; + public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145; + public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145; public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190; - public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; - public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50; + public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 54; + public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -54; } 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 775b583..3a05cdd 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 @@ -19,7 +19,7 @@ public class ServoPositions { public static double spinStartPos = 0.10; public static double spinEndPos = 0.95; - public static double shootAllSpindexerSpeedIncrease = 0.014; + public static double shootAllSpindexerSpeedIncrease = 0.013; public static double transferServo_out = 0.15; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 46b7949..40b3784 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -39,7 +39,7 @@ public class TeleopV3 extends LinearOpMode { public static double spinPow = 0.09; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; public static double spinSpeedIncrease = 0.03; - public static int resetSpinTicks = 4; + public static int resetSpinTicks = 0; public static double hoodSpeedOffset = 0.01; public static double turretSpeedOffset = 0.01; public double vel = 3000; @@ -76,7 +76,7 @@ public class TeleopV3 extends LinearOpMode { int intakeTicker = 0; private boolean shootAll = false; - boolean relocalize = false; + public static boolean relocalize = false; @Override public void runOpMode() throws InterruptedException { @@ -112,6 +112,7 @@ public class TeleopV3 extends LinearOpMode { light.setState(StateEnums.LightState.MANUAL); limelightUsed = true; + Spindexer.teleop = true; while (opModeInInit()) { robot.limelight.start(); @@ -154,13 +155,13 @@ public class TeleopV3 extends LinearOpMode { shootAll = false; servo.setTransferPos(transferServo_out); - light.setState(StateEnums.LightState.BALL_COUNT); + //light.setState(StateEnums.LightState.BALL_COUNT); } else if (gamepad2.triangle){ - light.setState(StateEnums.LightState.BALL_COLOR); + //light.setState(StateEnums.LightState.BALL_COLOR); } else { - light.setState(StateEnums.LightState.GOAL_LOCK); + //light.setState(StateEnums.LightState.GOAL_LOCK); } //TURRET TRACKING @@ -354,14 +355,14 @@ public class TeleopV3 extends LinearOpMode { // // TELE.addData("shootall commanded", shootAll); // Targeting Debug - TELE.addData("robotX", robotX); - TELE.addData("robotY", robotY); - TELE.addData("robot H", robotHeading); +// TELE.addData("robotX", robotX); +// TELE.addData("robotY", robotY); +// TELE.addData("robot H", robotHeading); // TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData("robotInchesY", targeting.robotInchesY); // TELE.addData("Targeting Interpolate", turretInterpolate); - TELE.addData("Targeting GridX", targeting.robotGridX); - TELE.addData("Targeting GridY", targeting.robotGridY); +// TELE.addData("Targeting GridX", targeting.robotGridX); +// TELE.addData("Targeting GridY", targeting.robotGridY); // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); @@ -369,13 +370,13 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); - TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); - TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); +// TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); +// TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); +// TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); TELE.update(); - light.update(); + //light.update(); ticker++; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java index 0e30dbb..de6189d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java @@ -65,46 +65,51 @@ public class SortingTest extends LinearOpMode { waitForStart(); if (isStopRequested()) return; - while (opModeIsActive()){ - spindexer.setIntakePower(1); - robot.transfer.setPower(1); - - if (gamepad1.crossWasPressed()){ - motif = 21; - } else if (gamepad1.squareWasPressed()){ - motif = 22; - } else if (gamepad1.triangleWasPressed()){ - motif = 23; - } - flywheel.manageFlywheel(2500); - - if (gamepad1.leftBumperWasPressed()){ - intaking = false; - Actions.runBlocking( - autoActions.prepareShootAll( - 3, - 5, - motif, - 0.501, - 0.501, - 0.501 - ) - ); - } else if (gamepad1.rightBumperWasPressed()){ - intaking = false; - Actions.runBlocking( - autoActions.shootAllAuto( - 3.5, - 0.014, - 0.501, - 0.501, - 0.501 - ) - ); - intaking = true; - } else if (intaking){ - spindexer.processIntake(); - } + if (opModeIsActive()){ + Actions.runBlocking( + autoActions.ShakeDrivetrain( + 100 + ) + ); +// spindexer.setIntakePower(1); +// robot.transfer.setPower(1); +// +// if (gamepad1.crossWasPressed()){ +// motif = 21; +// } else if (gamepad1.squareWasPressed()){ +// motif = 22; +// } else if (gamepad1.triangleWasPressed()){ +// motif = 23; +// } +// flywheel.manageFlywheel(2500); +// +// if (gamepad1.leftBumperWasPressed()){ +// intaking = false; +// Actions.runBlocking( +// autoActions.prepareShootAll( +// 3, +// 5, +// motif, +// 0.501, +// 0.501, +// 0.501 +// ) +// ); +// } else if (gamepad1.rightBumperWasPressed()){ +// intaking = false; +// Actions.runBlocking( +// autoActions.shootAllAuto( +// 3.5, +// 0.014, +// 0.501, +// 0.501, +// 0.501 +// ) +// ); +// intaking = true; +// } else if (intaking){ +// spindexer.processIntake(); +// } } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java index accdc8b..f843d89 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java @@ -31,18 +31,40 @@ public class Drivetrain { } + private double prevY = 0; + private double prevX = 0; + private double prevRX = 0; + private double prevBrake = 0; public void drive(double y, double x, double rx, double brake){ + int countConstant = 0; + boolean brakeChange = false; + if (Math.abs(prevY - y) > 0.05){ + prevY = y; + countConstant++; + } + if (Math.abs(prevX - x) > 0.05){ + prevX = x; + countConstant++; + } + if (Math.abs(prevRX - rx) > 0.05){ + prevRX = rx; + countConstant++; + } + if (Math.abs(prevBrake - brake) > 0.05){ + prevBrake = brake; + brakeChange = true; + } - if (!autoDrive) { + if (!autoDrive && countConstant > 0) { x = x* 1.1; // Counteract imperfect strafing - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; + double denominator = Math.max(Math.abs(prevY) + Math.abs(prevX) + Math.abs(prevRX), 1); + double frontLeftPower = (prevY + prevX + prevRX) / denominator; + double backLeftPower = (prevY - prevX + prevRX) / denominator; + double frontRightPower = (prevY - prevX - prevRX) / denominator; + double backRightPower = (prevY + prevX - prevRX) / denominator; robot.frontLeft.setPower(frontLeftPower); robot.backLeft.setPower(backLeftPower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java index 6a5f09d..6fdd2cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java @@ -64,17 +64,17 @@ public final class Light { break; case BALL_COLOR: - - if ((System.currentTimeMillis() % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) { + double currentTime = System.currentTimeMillis(); + if ((currentTime % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) { lightColor = spindexer.getRearCenterLight(); - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime / 3)) { + } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime / 3)) { lightColor = 0; - } else if ((System.currentTimeMillis() % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) { + } else if ((currentTime % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) { lightColor = spindexer.getDriverLight(); - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) { + } else if ((currentTime % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) { lightColor = 0; - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime - restingTime)) { + } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime - restingTime)) { lightColor = spindexer.getPassengerLight(); } else { 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 43c8a7d..f7d707d 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 @@ -173,10 +173,23 @@ public class Spindexer { // Detects if a ball is found and what color. // Returns true is there was a new ball found in Position 1 // FIXIT: Reduce number of times that we read the color sensors for loop times. + public static boolean teleop = false; public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) { boolean newPos1Detection = false; int spindexerBallPos = 0; + double rearDistance; + double frontDriverDistance; + double frontPassengerDistance; + if (teleop){ + rearDistance = 48; + frontDriverDistance = 50; + frontPassengerDistance = 29; + } else { + rearDistance = 48; + frontDriverDistance = 56; + frontPassengerDistance = 29; + } // Read Distances double dRearCenter = robot.color1.getDistance(DistanceUnit.MM); @@ -187,7 +200,7 @@ public class Spindexer { distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); // Position 1 - if (distanceRearCenter < 48) { + if (distanceRearCenter < rearDistance) { // Mark Ball Found newPos1Detection = true; @@ -209,7 +222,7 @@ public class Spindexer { // Position 2 // Find which ball position this is in the spindexer spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]; - if (distanceFrontDriver < 50) { + if (distanceFrontDriver < frontDriverDistance) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { @@ -235,7 +248,7 @@ public class Spindexer { // Position 3 spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]; - if (distanceFrontPassenger < 29) { + if (distanceFrontPassenger < frontPassengerDistance) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; @@ -512,6 +525,7 @@ public class Spindexer { break; case SHOOTWAIT: + double shootWaitMax = 4; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { if (commandedIntakePosition==2) { @@ -535,33 +549,27 @@ public class Spindexer { break; case SHOOT_PREP_CONTINOUS: - if (shootTicks > waitFirstBallTicks){ + if (servos.spinEqual(spinStartPos)){ currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; - shootTicks++; - } else if (servos.spinEqual(spinStartPos)){ - shootTicks++; - servos.setTransferPos(transferServo_in); } else { servos.setSpinPos(spinStartPos); } break; case SHOOT_CONTINOUS: - whileShooting = true; + servos.setTransferPos(transferServo_in); ballPositions[0].isEmpty = false; ballPositions[1].isEmpty = false; ballPositions[2].isEmpty = false; if (servos.getSpinPos() > spinEndPos){ - whileShooting = false; - servos.setTransferPos(transferServo_out); - shootTicks = 0; currentIntakeState = IntakeState.FINDNEXT; + servos.setTransferPos(transferServo_out); } else { - double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; + double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; if (spinPos > spinEndPos + 0.03){ spinPos = spinEndPos + 0.03; } - servos.setSpinPos(spinPos); + moveSpindexerToPos(spinPos); } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 55df38d..471cc1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -13,6 +13,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.teleop.TeleopV3; import java.util.ArrayList; import java.util.List; @@ -79,7 +80,7 @@ public class Turret { } public double getTurrPos() { - return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; + return robot.turr1.getPosition(); } private double prevTurrPos = 0; @@ -114,23 +115,18 @@ public class Turret { if (result.isValid()) { tx = result.getTx(); ty = result.getTy(); - // MegaTag1 code for receiving position - Pose3D botpose = result.getBotpose(); - if (botpose != null) { - limelightPosX = botpose.getPosition().x; - limelightPosY = botpose.getPosition().y; - } - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - limelightTagPose = fiducial.getRobotPoseTargetSpace(); - if (limelightTagPose != null){ - xPos = limelightTagPose.getPosition().x; - yPos = limelightTagPose.getPosition().y; - zPos = limelightTagPose.getPosition().z; - hPos = limelightTagPose.getOrientation().getYaw(); + if (TeleopV3.relocalize){ + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + limelightTagPose = fiducial.getRobotPoseTargetSpace(); + if (limelightTagPose != null){ + xPos = limelightTagPose.getPosition().x; + yPos = limelightTagPose.getPosition().y; + zPos = limelightTagPose.getPosition().z; + hPos = limelightTagPose.getOrientation().getYaw(); + } } } - } } if (xPos != null){