diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java index 0709b81..382ecc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java @@ -116,8 +116,8 @@ public class blueDepot extends LinearOpMode { .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); }) - .lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90))) - .lineToLinearHeading(new Pose2d(-13,65, Math.toRadians(55))) + .lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(-16,65, Math.toRadians(55))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeStart(); }) @@ -143,7 +143,7 @@ public class blueDepot extends LinearOpMode { .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); }) - .lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90))) .lineToLinearHeading(new Pose2d(-18 ,65, Math.toRadians(55))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeStart(); @@ -155,7 +155,7 @@ public class blueDepot extends LinearOpMode { .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeEnd(); }) - .lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90))) .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java index 5363b23..08f8640 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java @@ -61,6 +61,9 @@ public class DriverControlsV1 extends LinearOpMode { robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); while (opModeIsActive()) { + robot.leftPTO.setPosition(0.2); + robot.rightPTO.setPosition(0.2); + double currentVoltage = robot.batteryVoltageSensor.getVoltage(); if (currentVoltage > 13.5) { pidf = new PIDFCoefficients(3, 0, 0, 13.8 @@ -74,16 +77,19 @@ public class DriverControlsV1 extends LinearOpMode { } Pose2d myPose = drive.getPoseEstimate(); - + double turrPos = robot.turret.getCurrentPosition(); result = limelight.getLatestResult(); double vision = result.getTx() * visionMult; + if((result.getTx() < 2 || result.getTy() > -2) && result.isValid()){ +// drive.setPoseEstimate(new Pose2d(drive.getPoseEstimate().getX(), drive.getPoseEstimate().getY(), turretLock.updateHeading(myPose, result, turrPos))); + } double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger); if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg); if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim // telemetry.update();\ // - double targetTicks = turretLock.update(myPose, result, vision); + double targetTicks = turretLock.update(myPose, result, vision, turrPos); robot.turret.setTargetPosition((int) targetTicks); robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.turret.setPower(1); @@ -159,8 +165,11 @@ public class DriverControlsV1 extends LinearOpMode { break; case LOWER_HOOD: - targetHood = Math.max(0.45, targetHood - 0.2); + targetHood = Math.max(0.34, targetHood - 0.2); + targetVelocity = targetVelocity + 100; robot.launchHood.setPosition(targetHood); + robot.bottomFly.setVelocity(targetVelocity); + robot.topFly.setVelocity(targetVelocity); shootStatus = ShootState.RESET; break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java index 6de05c6..f55231e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java @@ -127,7 +127,7 @@ public class TeleOpTurretLock extends LinearOpMode { if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim // telemetry.update();\ // - double targetTicks = turretLock.update(pose, result, vision); + double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition()); robot.turret.setTargetPosition((int) targetTicks); robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.turret.setPower(1); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java index 1c02f4c..e9ca896 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java @@ -168,7 +168,7 @@ public class TurretLock { /** Main update: call every loop. */ - public double update(Pose2d robotPose, LLResult result, double visionCorrect) { + public double update(Pose2d robotPose, LLResult result, double visionCorrect, double turrPos) { // 1) ODOM: compute turret angle needed to face goal (in degrees) double dx = goalX - robotPose.getX(); double dy = goalY - robotPose.getY(); @@ -193,7 +193,7 @@ public class TurretLock { filteredVisionTrimDeg = -visionCorrect; switch (TYPE){ case LIME: - finalDeg = manualTrimDeg + filteredVisionTrimDeg + turretCmdDeg; + finalDeg = manualTrimDeg + turrPos / ticksPerDegree + filteredVisionTrimDeg; if (result != null && (finalDeg > 20 || finalDeg < -20)) { TYPE = MODE.ODOMETRY; } @@ -223,6 +223,16 @@ public class TurretLock { // turret.setPower(1.0); } + public double updateHeading(Pose2d robotPose, LLResult result, double turrPos) { + double robotHeadingDeg = Math.toDegrees(robotPose.getHeading()); + + if(result.getTx() < 1 || result.getTy() > -1){ + robotHeadingDeg = - (turrPos / ticksPerDegree - 45); + } + + return robotHeadingDeg; + } + public MODE getTYPE() { return TYPE; }