diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java index b06cae2..418719c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java @@ -97,12 +97,12 @@ public class ProtoAutoClose_V4 extends LinearOpMode { public static double normalIntakeTime = 3.0; public static double shoot1Turr = 0.57; public static double shoot0XTolerance = 1.0; - public static double turretShootPos = 0.5; + public static double turretShootPos = 0.53; public static double shootAllTime = 1.8; public static double shoot0Time = 1.6; public static double intake1Time = 3.0; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 20; + public static double pickup1Speed = 25; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shoot1Hood = 0.93; @@ -175,6 +175,10 @@ public class ProtoAutoClose_V4 extends LinearOpMode { teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + return (System.currentTimeMillis() - stamp) < (time * 1000); } @@ -193,8 +197,8 @@ public class ProtoAutoClose_V4 extends LinearOpMode { @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); TELE.update(); flywheel.manageFlywheel(vel); @@ -258,8 +262,8 @@ public class ProtoAutoClose_V4 extends LinearOpMode { @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); TELE.update(); velo = flywheel.getVelo(); @@ -331,6 +335,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); @@ -381,6 +388,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); return !shouldFinish; @@ -430,6 +440,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); return !shouldFinish; @@ -497,6 +510,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); return !shouldFinish; @@ -563,6 +579,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); return !shouldFinish; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index 94196a8..458e63b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -14,17 +14,17 @@ public class Poses { public static Pose2d goalPose = new Pose2d(-10, 0, 0); - public static double rx1 = 20, ry1 = 0, rh1 = 0; + public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1); public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); - public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140); + public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140.1); public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140); public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); - public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140); + public static double rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1); - public static double rx4a = 68, ry4a = 59, rh4a = Math.toRadians(140); - public static double rx4b = 40, ry4b = 87, rh4b = Math.toRadians(140); + public static double rx4a = 72, ry4a = 65, rh4a = Math.toRadians(145); + public static double rx4b = 37, ry4b = 85, rh4b = Math.toRadians(145.1); public static double bx1 = 40, by1 = 7, bh1 = 0; public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140); @@ -38,8 +38,8 @@ public class Poses { public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140); public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this - public static double rShootX = 40, rShootY = 0, rShootH = Math.toRadians(50); - public static double rxPrep = 50, ryPrep = 10, rhPrep = Math.toRadians(50); + public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50); + public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50); public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140); public static double bxPrep = 50, byPrep = -10, bhPrep = Math.toRadians(140);