diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java index f4073ac..7ee0450 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java @@ -52,18 +52,6 @@ public class Auton { .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118)) .build(); - grabSet2Path = follower.pathBuilder() - .addPath(new BezierLine(new Pose(59, 15), new Pose(41, 60))) - .setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180)) - .addPath(new BezierCurve(new Pose(41, 60), new Pose(17.711, 56.837), new Pose(25.306, 62.677), new Pose(16, 63))) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .build(); - - scoreSet2Path = follower.pathBuilder() - .addPath(new BezierLine(new Pose(16, 63), new Pose(59, 15))) - .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118)) - .build(); - grabEndGamePath = follower.pathBuilder() .addPath(new BezierCurve(new Pose(59, 15), new Pose(37.157, 26.830), new Pose(10.400, 8))) .setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180)) @@ -81,7 +69,13 @@ public class Auton { .setConstantHeadingInterpolation(Math.toRadians(180)) .addPath(new BezierLine(new Pose(10.439, 17.195), new Pose(59, 15))) .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(135)) - .addPath(new BezierLine(new Pose(59, 15), new Pose(54.614, 19.545))) + .addPath(new BezierCurve(new Pose(59, 15), new Pose(35.728, 26.614), new Pose(10, 27))) + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(180)) + .addPath(new BezierLine(new Pose(10, 27), new Pose(10.219, 17.664))) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath(new BezierLine(new Pose(10.219, 17.664), new Pose(59, 15))) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(135)) + .addPath(new BezierLine(new Pose(59, 15), new Pose(55, 20))) .setConstantHeadingInterpolation(Math.toRadians(135)) .build(); } @@ -110,7 +104,7 @@ public class Auton { grabSet2Path = follower.pathBuilder() .addPath(new BezierLine(new Pose(41.758, 61.439), new Pose(16, 59.830))) - .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + .setConstantHeadingInterpolation(Math.toRadians(180)) .build(); scoreSet2Path = follower.pathBuilder() @@ -119,23 +113,27 @@ public class Auton { .build(); grabGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(54, 90), new Pose(40.119, 55.595), new Pose(12, 58.3))) - .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(138)) + .addPath(new BezierCurve(new Pose(54, 90), new Pose(40.119, 55.595), new Pose(15.300, 64))) + .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(90)) + .addPath(new BezierCurve(new Pose(15.300, 64), new Pose(17.445, 52.820), new Pose(7.500, 50))) + .setConstantHeadingInterpolation(Math.toRadians(90)) .build(); scoreGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(12, 58.3), new Pose(41.918, 62.187), new Pose(54, 90))) - .setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144)) + .addPath(new BezierCurve(new Pose(7.500, 50), new Pose(41.918, 62.187), new Pose(54, 90))) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(144)) .build(); grabGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(54, 90), new Pose(42.081, 61.914), new Pose(12, 58.3))) - .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(138)) + .addPath(new BezierCurve(new Pose(54, 90), new Pose(42.081, 61.914), new Pose(14.600, 63.600))) + .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(90)) + .addPath(new BezierCurve(new Pose(14.600, 63.600), new Pose(17.545, 53.278), new Pose(7.500, 50))) + .setConstantHeadingInterpolation(Math.toRadians(90)) .build(); scoreGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(12, 58.3), new Pose(44, 62), new Pose(54, 90))) - .setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144)) + .addPath(new BezierCurve(new Pose(7.500, 50), new Pose(44, 62), new Pose(54, 90))) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(144)) .build(); driveToSet3Path = follower.pathBuilder() @@ -207,18 +205,6 @@ public class Auton { .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62)) .build(); - grabSet2Path = follower.pathBuilder() - .addPath(new BezierLine(new Pose(85, 15), new Pose(103, 60))) - .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0)) - .addPath(new BezierCurve(new Pose(103, 60), new Pose(126.289, 56.837), new Pose(118.694, 62.677), new Pose(128, 63))) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .build(); - - scoreSet2Path = follower.pathBuilder() - .addPath(new BezierLine(new Pose(128, 63), new Pose(85, 15))) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62)) - .build(); - grabEndGamePath = follower.pathBuilder() .addPath(new BezierCurve(new Pose(85, 15), new Pose(106.843, 26.830), new Pose(133.600, 8))) .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0)) @@ -236,7 +222,13 @@ public class Auton { .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath(new BezierLine(new Pose(133.561, 17.195), new Pose(85, 15))) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(45)) - .addPath(new BezierLine(new Pose(85, 15), new Pose(89.386, 19.545))) + .addPath(new BezierCurve(new Pose(85, 15), new Pose(108.272, 26.614), new Pose(134, 27))) + .setLinearHeadingInterpolation(Math.toRadians(45), Math.toRadians(0)) + .addPath(new BezierLine(new Pose(134, 27), new Pose(133.781, 17.664))) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath(new BezierLine(new Pose(133.781, 17.664), new Pose(85, 15))) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(45)) + .addPath(new BezierLine(new Pose(85, 15), new Pose(89, 20))) .setConstantHeadingInterpolation(Math.toRadians(45)) .build(); } @@ -265,7 +257,7 @@ public class Auton { grabSet2Path = follower.pathBuilder() .addPath(new BezierLine(new Pose(102.242, 61.439), new Pose(128, 59.830))) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)) + .setConstantHeadingInterpolation(Math.toRadians(0)) .build(); scoreSet2Path = follower.pathBuilder() @@ -274,23 +266,27 @@ public class Auton { .build(); grabGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(90, 90), new Pose(103.881, 55.595), new Pose(132, 58.3))) - .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(42)) + .addPath(new BezierCurve(new Pose(90, 90), new Pose(103.881, 55.595), new Pose(128.700, 64))) + .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(90)) + .addPath(new BezierCurve(new Pose(128.700, 64), new Pose(126.555, 52.820), new Pose(136.500, 50))) + .setConstantHeadingInterpolation(Math.toRadians(90)) .build(); scoreGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(132, 58.3), new Pose(102.082, 62.187), new Pose(90, 90))) - .setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36)) + .addPath(new BezierCurve(new Pose(136.500, 50), new Pose(102.082, 62.187), new Pose(90, 90))) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(36)) .build(); grabGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(90, 90), new Pose(101.919, 61.914), new Pose(132, 58.3))) - .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(42)) + .addPath(new BezierCurve(new Pose(90, 90), new Pose(101.919, 61.914), new Pose(129.400, 63.600))) + .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(90)) + .addPath(new BezierCurve(new Pose(129.400, 63.600), new Pose(126.455, 53.278), new Pose(136.500, 50))) + .setConstantHeadingInterpolation(Math.toRadians(90)) .build(); scoreGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(132, 58.3), new Pose(100, 62), new Pose(90, 90))) - .setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36)) + .addPath(new BezierCurve(new Pose(136.500, 50), new Pose(100, 62), new Pose(90, 90))) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(36)) .build(); driveToSet3Path = follower.pathBuilder() @@ -504,35 +500,6 @@ public class Auton { break; case SCORE_SET_1: - if (!follower.isBusy()) { - systems.setAutonState(pedroStateMachine.AutonState.SCORING); - if (checkScoringCondition()) { - setPathState(PathState.DRIVE_TO_GRAB_SET_2); - } - } - break; - - // --- SET 2 --- - case DRIVE_TO_GRAB_SET_2: - follower.followPath(grabSet2Path, true); - systems.setAutonState(pedroStateMachine.AutonState.INTAKING); - setPathState(PathState.GRAB_SET_2); - break; - - case GRAB_SET_2: - if (!follower.isBusy()) { - systems.setAutonState(pedroStateMachine.AutonState.IDLE); - setPathState(PathState.DRIVE_TO_SCORE_SET_2); - } - break; - - case DRIVE_TO_SCORE_SET_2: - follower.followPath(scoreSet2Path, true); - systems.initiateTurretSpinUp(); - setPathState(PathState.SCORE_SET_2); - break; - - case SCORE_SET_2: if (!follower.isBusy()) { systems.setAutonState(pedroStateMachine.AutonState.SCORING); if (checkScoringCondition()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java index 2f4cf03..3837379 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java @@ -128,7 +128,7 @@ public class DriveTrain { if (isAnchorActive) { isAnchorActive = false; stop(); - this.currentState = DriveState.NORMAL; + this.currentState = DriveState.TURBO; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java index a409780..e1c27e7 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java @@ -287,6 +287,7 @@ public class Turret { LLResult result = hardware.getLimelightResult(); if (result != null && result.isValid()) { + offsetTicks = 0; double tx = result.getTx(); double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER); double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES; @@ -481,7 +482,7 @@ public class Turret { } if (dist > 145.0) { - return 2300.0; + return 2400.0; } double x = dist; @@ -493,7 +494,7 @@ public class Turret { + (0.00636447 * x3) + (-0.593959 * x2) + (26.08276 * x) - + 1340.12895; + + 1350.12895; return Range.clip(velocity, 1300.0, 2450.0); } @@ -565,6 +566,9 @@ public class Turret { distanceOffset = offset; } + public double getDistanceOffset() { + return distanceOffset; + } public void setLaunching(boolean state) { isLaunching = state; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java index e0fd4a6..a817be2 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java @@ -57,7 +57,7 @@ public class Constants { public static DcMotor.ZeroPowerBehavior INTAKE_ZERO_POWER_BEHAVIOR = DcMotor.ZeroPowerBehavior.BRAKE; public static DcMotor.RunMode INTAKE_RUNMODE = DcMotor.RunMode.RUN_WITHOUT_ENCODER; - public static final double INTAKE_POWER = 1; + public static final double INTAKE_POWER = 0.9; public static final double EXTAKE_POWER = -0.8; public static final double LAUNCH_POWER = 1; public static final double IDLE_POWER = 0; @@ -113,7 +113,7 @@ public class Constants { public static final double MAX_POWER = 0.8; public static final double MAX_INTEGRAL = 0.5; // PIDF Values P I D F - public static double[] PIDF_NORMAL = {1.3, 0, 2, 13.5}; + public static double[] PIDF_NORMAL = {1.3, 0, 2, 16}; public static double[] PIDF_Shooting = {27, 0, 2, 2.4}; @@ -183,7 +183,7 @@ public class Constants { public static double ARTIFACT_1_WINDOW = 0.35; public static double ARTIFACT_2_WINDOW = 0.55; - public static double SHOT_TIME_OF_FLIGHT = 0.83; + public static double SHOT_TIME_OF_FLIGHT = 0.43; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java index a6777c9..1cfbb0a 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java @@ -199,7 +199,7 @@ public class StateMachine { private void handleRobotStateEntry(RobotState newState) { switch (newState) { case TELEOP: - m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL); + m_driveTrain.setDriveState(DriveTrain.DriveState.TURBO); m_intake.setIntakeState(Intake.IntakeState.IDLE); break; case ESTOP: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java index 33fed4b..a9777bc 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java @@ -239,11 +239,16 @@ public class finalTeleOp { if (gamepad2.dpadUpWasPressed()) { stateMachine.getTurret().updateDistanceOffset(7); } else if (gamepad2.dpadDownWasPressed()) { - stateMachine.getTurret().updateDistanceOffset(7); + stateMachine.getTurret().updateDistanceOffset(-7); } else if (gamepad2.dpadLeftWasPressed()) { stateMachine.getTurret().setDistanceOffset(0); } + if (gamepad2.dpadRightWasPressed()) { + org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.ARTIFACT_1 = -90; + } else if (gamepad2.dpadLeftWasReleased()) { + org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.ARTIFACT_1 = -150; + } } @@ -258,6 +263,7 @@ public class finalTeleOp { telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE"); telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE"); telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState()); + telemetry.addData("Distance offset", stateMachine.getTurret().getDistanceOffset()); telemetry.addLine("▰▰▰▰▰▰▰ TRACKING ▰▰▰▰▰▰▰"); telemetry.addData("Pose", "X:%.1f Y:%.1f H:%.1f°", p.getX(), p.getY(), Math.toDegrees(p.getHeading()));