diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/KOdometryDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/KOdometryDrive.kt index 5772ce207dde..f902c4773b3c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/KOdometryDrive.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/KOdometryDrive.kt @@ -45,9 +45,9 @@ class KOdometryDrive( // previous version of this curve @ 9405a9d1f89cb164c81c14ca659724933698ae92 val StrafingCurve = QuadraticDownRamps( .1, - .1, // TODO: is this too low? + .1, StrafingMaxSpeed, - 0.0, + .5, // in SECONDS, not inches 8.0, ) @@ -206,7 +206,7 @@ class KOdometryDrive( val lCorrect = kpStr * lErr + kiStr * sumErrorLeft val rCorrect = kpStr * rErr + kiStr * sumErrorRight - val speed = StrafingCurve.ramp(sDist, target - sDist) * switcher + val speed = StrafingCurve.ramp(timeoutT.time(), target - sDist) * switcher Log.i("Encoders", String.format("L %+.4f R %+.4f P %+.4f", lErr, rErr, sDist)) if (abs(lCorrect) > 1 || abs(rCorrect) > 1) { // uh oh!!! diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java index 9e32b525ed82..410a0ee1b562 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java @@ -3,6 +3,7 @@ import static org.firstinspires.ftc.teamcode.AprilTagToPoseKt.detectSingleToPose; import static org.firstinspires.ftc.teamcode.opmodes.TeleOp.kvoid; +import android.util.Log; import android.util.Size; import androidx.annotation.Nullable; @@ -19,7 +20,6 @@ import org.firstinspires.ftc.teamcode.abstractions.ApproachObject2; import org.firstinspires.ftc.teamcode.abstractions.Dumper; import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration; -import org.firstinspires.ftc.teamcode.odo.DriveForwardPID; import org.firstinspires.ftc.teamcode.odo.KOdometryDrive; import org.firstinspires.ftc.teamcode.odo.SyncFail; import org.firstinspires.ftc.teamcode.odo.TurnPID; @@ -48,7 +48,6 @@ public abstract class TwentyAuto extends LinearOpMode { private VisionPortal portal; private AdvSphereProcess sphere; private AprilTagProcessor aprilTag; - private DriveForwardPID forwardPID; private TurnPID turnPID; private SyncFail why; private RobotConfiguration robot; @@ -78,12 +77,29 @@ public abstract class TwentyAuto extends LinearOpMode { */ protected abstract AdvSphereProcess.Mode modeConf(); + protected abstract AllianceColor allianceColor(); + + protected abstract FieldGlobalPosition globalPosition(); + /** * Robot starting position * * @return Left of truss Or Right of truss */ - protected abstract StartPosition positionConf(); + protected StartPosition positionConf() { + if (allianceColor() == AllianceColor.Red) { + if (globalPosition() == FieldGlobalPosition.Backstage) + return StartPosition.RightOfTruss; + else + return StartPosition.LeftOfTruss; + } else { + if (globalPosition() == FieldGlobalPosition.Backstage) { + return StartPosition.LeftOfTruss; + } else { + return StartPosition.RightOfTruss; + } + } + } /** * Robot parking options @@ -223,6 +239,7 @@ private void scoreYellowPixel(MultitaskScheduler scheduler, RobotConfiguration r * @param targetID target tag id. */ private void alignTag(AprilTagProcessor aprilTag, int targetID) { + Log.i("20auto", "Aligning with tag " + targetID); // Rotate right by theta + 90deg List detections = aprilTag.getDetections(); @Nullable AprilTagDetection primary = null; @@ -240,6 +257,7 @@ private void alignTag(AprilTagProcessor aprilTag, int targetID) { } Pose posed = detectSingleToPose(primary).transform(cameraAdjustment); + Log.i("20auto", " align: pose estimate: " + posed); double turnDegs = posed.getTheta() .plus(new DegreeUnit(90)) @@ -264,7 +282,6 @@ public void runOpMode() throws InterruptedException { setupVision(hardwareMap.get(CameraName.class, "Webcam 1")); telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap); - forwardPID = new DriveForwardPID(robot); turnPID = new TurnPID(robot); KOdometryDrive newOdo = new KOdometryDrive(scheduler, robot); why = new SyncFail(scheduler, newOdo); @@ -314,8 +331,6 @@ public void runOpMode() throws InterruptedException { Runnable right = posLeft ? this::rightLeft : this::rightRight; Runnable undoRight = posLeft ? this::unRightLeft : this::unRightRight; - boolean shouldUndo = parkingConf() != Parking.NoParking; - int targetTag = 0; switch (result) { case Left: @@ -333,9 +348,11 @@ public void runOpMode() throws InterruptedException { break; } - if (!posLeft) targetTag += 3; + if (allianceColor() == AllianceColor.Red) targetTag += 3; + + if (globalPosition() == FieldGlobalPosition.Backstage) { + // Return to common position. Not amazing efficiency but it's what we had. - if (shouldUndo) { switch (result) { case Left: undoLeft.run(); @@ -348,57 +365,13 @@ public void runOpMode() throws InterruptedException { undoCenter.run(); break; } - // PUT IF STATEMENT TO BRANCH HERE - - if (parkingConf() == Parking.OtherParking) - throw new IllegalStateException("Don't know how to handle OtherParking here"); - double modifier = parkingConf() == Parking.MoveLeft ? 1 : -1; - turnPID.TurnRobot(modifier * 90.0, telemetry); - sleep(100); - why.DriveReverse(19.0, telemetry, 5.0); - // Left: 9.5", Center: 15", Right: 21.5" - - int nearMedFar; - switch (result) { - case Left: - nearMedFar = 0; - break; - case None: - case Center: - default: - nearMedFar = 1; - break; - case Right: - nearMedFar = 2; - break; - } - - if (!posLeft) nearMedFar = 2 - nearMedFar; Consumer awayFromWall = posLeft ? newOdo::strafeLeft : newOdo::strafeRight; BiConsumer toWallWithTimeout = posLeft ? newOdo::strafeRight : newOdo::strafeLeft; - switch (nearMedFar) { - case 0: - awayFromWall.accept( - new InchUnit(11.0).plus(Var.AutoPositions.RobotWidth.div(2)) - ); - break; - case 1: - default: - awayFromWall.accept( - new InchUnit(16.5).plus(Var.AutoPositions.RobotWidth.div(2)) - ); - break; - case 2: - awayFromWall.accept( - new InchUnit(23.0).plus(Var.AutoPositions.RobotWidth.div(2)) - ); - break; - } - scheduler.runToCompletion(this::opModeIsActive); + navBackstage(scheduler, result, awayFromWall); sleep(200); @@ -407,8 +380,10 @@ public void runOpMode() throws InterruptedException { alignTag(aprilTag, targetTag); scoreYellowPixel(scheduler, robot, theXButton, newOdo); + //noinspection DataFlowIssue i am going to shove you into a box :)))) LengthUnit strafeMotion = new InchUnit(tagXPositions.getOrDefault(targetTag, 35.5)); - if (!posLeft) strafeMotion = new FootUnit(12).minus(strafeMotion); + if (allianceColor() == AllianceColor.Red) + strafeMotion = new FootUnit(12).minus(strafeMotion); strafeMotion = strafeMotion .minus(Var.AutoPositions.RobotWidth.div(2)) .minus(new InchUnit(2)); @@ -441,6 +416,36 @@ public void runOpMode() throws InterruptedException { return kvoid; }); + scheduler.runToCompletion(this::opModeIsActive); + } else { // Frontstage scripts + navFrontstage(scheduler, result); + + alignTag(aprilTag, targetTag); + sleep(200); + alignTag(aprilTag, targetTag); + scoreYellowPixel(scheduler, robot, theXButton, newOdo); + + // back off More + newOdo.driveForward(new InchUnit(2), 2.0) // this would be a REALLY BAD place to get stuck + .then(e -> { + e.onStart(() -> { + robot.dumperRotate().setPosition(Var.Box.idleRotate); + return kvoid; + }); + TimingKt.maxDuration(e, 300); + return kvoid; + }).then(e -> { + e.require(robot.getLiftLock()); + e.onStart(() -> { + robot.liftLeft().setTargetPosition(0); + robot.liftRight().setTargetPosition(0); + return kvoid; + }); + e.isCompleted(() -> + !(robot.liftLeft().isBusy() || robot.liftRight().isBusy())); + return kvoid; + }); + scheduler.runToCompletion(this::opModeIsActive); } @@ -451,6 +456,102 @@ public void runOpMode() throws InterruptedException { } } + private void navBackstage( + MultitaskScheduler scheduler, + AdvSphereProcess.Result cvResult, + Consumer doAwayFromWall + ) { + double modifier = parkingConf() == Parking.MoveLeft ? 1 : -1; + turnPID.TurnRobot(modifier * 90.0, telemetry); + sleep(100); + why.DriveReverse(19.0, telemetry, 5.0); + // Left: 9.5", Center: 15", Right: 21.5" + + int nearMedFar; + switch (cvResult) { + case Left: + nearMedFar = 0; + break; + case None: + case Center: + default: + nearMedFar = 1; + break; + case Right: + nearMedFar = 2; + break; + } + + if (allianceColor() == AllianceColor.Red) nearMedFar = 2 - nearMedFar; + + switch (nearMedFar) { + case 0: + doAwayFromWall.accept( + new InchUnit(11.0).plus(Var.AutoPositions.RobotWidth.div(2)) + ); + break; + case 1: + default: + doAwayFromWall.accept( + new InchUnit(16.5).plus(Var.AutoPositions.RobotWidth.div(2)) + ); + break; + case 2: + doAwayFromWall.accept( + new InchUnit(23.0).plus(Var.AutoPositions.RobotWidth.div(2)) + ); + break; + } + scheduler.runToCompletion(this::opModeIsActive); + } + + private void frontstageLeft() { + turnPID.TurnRobot(-45.0, telemetry); + why.StrafeLeft(2); + why.DriveReverse(27); + sleep(250); + turnPID.TurnRobot(95, telemetry); + why.DriveReverse(80); + why.StrafeRight(29); + } + + private void frontstageCenter() { + why.StrafeLeft(7); + why.DriveReverse(24); + turnPID.TurnRobot(95, telemetry); + why.DriveReverse(87); + why.StrafeRight(22); + } + + private void frontstageRight() { + turnPID.TurnRobot(60, telemetry); + why.StrafeRight(4.5); + why.DriveReverse(34); + sleep(250); + turnPID.TurnRobot(90, telemetry); + why.DriveReverse(78); + why.StrafeRight(20.5); + } + + private void navFrontstage( + MultitaskScheduler scheduler, + AdvSphereProcess.Result cvResult + ) { + switch (cvResult) { + case Left: + frontstageLeft(); + break; + case Center: + case None: + default: + frontstageCenter(); + break; + case Right: + frontstageRight(); + break; + } + } + void setupVision(CameraName camera) { sphere = new AdvSphereProcess(modeConf(), positionConf() == StartPosition.LeftOfTruss); aprilTag = new AprilTagProcessor.Builder() @@ -479,4 +580,14 @@ public enum Parking { NoParking, OtherParking } + + public enum AllianceColor { + Red, + Blue + } + + public enum FieldGlobalPosition { + Frontstage, + Backstage + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoBlueLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoBlueLeft.java index c89c27f12a60..883943e76a35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoBlueLeft.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoBlueLeft.java @@ -12,8 +12,13 @@ protected AdvSphereProcess.Mode modeConf() { } @Override - protected StartPosition positionConf() { - return StartPosition.LeftOfTruss; + protected AllianceColor allianceColor() { + return AllianceColor.Blue; + } + + @Override + protected FieldGlobalPosition globalPosition() { + return FieldGlobalPosition.Backstage; } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoBlueRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoBlueRight.java index 49dff97d81a0..ce7219b12997 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoBlueRight.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoBlueRight.java @@ -12,8 +12,13 @@ protected AdvSphereProcess.Mode modeConf() { } @Override - protected StartPosition positionConf() { - return StartPosition.RightOfTruss; + protected AllianceColor allianceColor() { + return AllianceColor.Blue; + } + + @Override + protected FieldGlobalPosition globalPosition() { + return FieldGlobalPosition.Frontstage; } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoRedLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoRedLeft.java index ae12e4c94002..666b2772a159 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoRedLeft.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoRedLeft.java @@ -12,8 +12,13 @@ protected AdvSphereProcess.Mode modeConf() { } @Override - protected StartPosition positionConf() { - return StartPosition.LeftOfTruss; + protected AllianceColor allianceColor() { + return AllianceColor.Red; + } + + @Override + protected FieldGlobalPosition globalPosition() { + return FieldGlobalPosition.Frontstage; } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoRedRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoRedRight.java index b5779049ade8..471665968cd2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoRedRight.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAutoRedRight.java @@ -12,8 +12,13 @@ protected AdvSphereProcess.Mode modeConf() { } @Override - protected StartPosition positionConf() { - return StartPosition.RightOfTruss; + protected AllianceColor allianceColor() { + return AllianceColor.Red; + } + + @Override + protected FieldGlobalPosition globalPosition() { + return FieldGlobalPosition.Backstage; } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyBlueRightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyBlueRightTest.java index 0357fe980c8f..a07de43b90da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyBlueRightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyBlueRightTest.java @@ -7,8 +7,12 @@ import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration; import org.firstinspires.ftc.teamcode.odo.DriveForwardPID; +import org.firstinspires.ftc.teamcode.odo.KOdometryDrive; +import org.firstinspires.ftc.teamcode.odo.SyncFail; import org.firstinspires.ftc.teamcode.odo.TurnPID; +import dev.aether.collaborative_multitasking.MultitaskScheduler; + @Autonomous public class TwentyBlueRightTest extends LinearOpMode { private DriveForwardPID forwardPID; @@ -18,42 +22,48 @@ public class TwentyBlueRightTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { RobotConfiguration robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap); + MultitaskScheduler scheduler = new MultitaskScheduler(); + KOdometryDrive odo2 = new KOdometryDrive(scheduler, robot); robot.clearEncoders(); DriveForwardPID drivePID = new DriveForwardPID(robot); TurnPID turnPID = new TurnPID(robot); + SyncFail why = new SyncFail(scheduler, odo2); telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); waitForStart(); if (!opModeIsActive()) return; - /*Left spike marker BlueRightAuto - /drivePID.DriveReverse(21.0, telemetry); + // Ctrl-Slash to comment lines + /* Left spike marker BlueRightAuto */ + drivePID.DriveReverse(21.0, telemetry); turnPID.TurnRobot(45.0, telemetry); drivePID.DriveReverse(3.0, telemetry); + turnPID.TurnRobot(-45.0, telemetry); drivePID.strafeLeft(2, telemetry); drivePID.DriveReverse(25, telemetry); sleep(250); turnPID.TurnRobot(95, telemetry); drivePID.DriveReverse(80, telemetry); - drivePID.strafeRight(27, telemetry);*/ + drivePID.strafeRight(27, telemetry); - /*Center spike marker BlueRightAuto + /* Center spike marker BlueRightAuto */ drivePID.DriveReverse(27, telemetry); + drivePID.strafeLeft(7, telemetry); drivePID.DriveReverse(24, telemetry); turnPID.TurnRobot(95, telemetry); drivePID.DriveReverse(87, telemetry); - drivePID.strafeRight(22, telemetry);*/ + drivePID.strafeRight(22, telemetry); //Right spike marker BlueRightAuto - drivePID.DriveReverse(16.0, telemetry); + why.DriveReverse(16.0, telemetry); turnPID.TurnRobot(-60.0, telemetry); turnPID.TurnRobot(60, telemetry); - drivePID.strafeRight(4.5, telemetry); - drivePID.DriveReverse(34, telemetry); + why.StrafeRight(4.5, telemetry); + why.DriveReverse(34, telemetry); sleep(250); turnPID.TurnRobot(90, telemetry); - drivePID.DriveReverse(78, telemetry); - drivePID.strafeRight(20.5, telemetry); + why.DriveReverse(78, telemetry); + why.StrafeRight(20.5, telemetry); while (opModeIsActive()) { telemetry.addData("LEFT ", drivePID.LeftOdoDist());