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 b66c0be..f4073ac 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 @@ -109,32 +109,32 @@ public class Auton { .build(); grabSet2Path = follower.pathBuilder() - .addPath(new BezierLine(new Pose(41.758, 61.439), new Pose(23.620, 59.830))) + .addPath(new BezierLine(new Pose(41.758, 61.439), new Pose(16, 59.830))) .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) .build(); scoreSet2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(23.620, 59.830), new Pose(55.542, 82.280), new Pose(54, 90))) + .addPath(new BezierCurve(new Pose(16, 59.830), new Pose(55.542, 82.280), new Pose(54, 90))) .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(144)) .build(); grabGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(54, 90), new Pose(40.119, 55.595), new Pose(11.400, 59.600))) + .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)) .build(); scoreGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(11.400, 59.600), new Pose(41.918, 62.187), new Pose(54, 90))) + .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)) .build(); grabGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(54, 90), new Pose(42.081, 61.914), new Pose(11.400, 59.600))) + .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)) .build(); scoreGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(11.400, 59.600), new Pose(43.404, 100.236), new Pose(54, 90))) + .addPath(new BezierCurve(new Pose(12, 58.3), new Pose(44, 62), new Pose(54, 90))) .setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144)) .build(); @@ -144,12 +144,12 @@ public class Auton { .build(); grabSet3Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(38.515, 47.156), new Pose(29.622, 37.842), new Pose(16.419, 35.945))) + .addPath(new BezierCurve(new Pose(38.515, 47.156), new Pose(29.622, 37.842), new Pose(12, 36))) .setLinearHeadingInterpolation(Math.toRadians(220), Math.toRadians(185)) .build(); scoreSet3Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(16.419, 35.945), new Pose(9.594, 52.541), new Pose(54, 90))) + .addPath(new BezierCurve(new Pose(12, 36), new Pose(9.594, 52.541), new Pose(54, 90))) .setLinearHeadingInterpolation(Math.toRadians(185), Math.toRadians(144)) .build(); @@ -264,32 +264,32 @@ public class Auton { .build(); grabSet2Path = follower.pathBuilder() - .addPath(new BezierLine(new Pose(102.242, 61.439), new Pose(120.380, 59.830))) + .addPath(new BezierLine(new Pose(102.242, 61.439), new Pose(128, 59.830))) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)) .build(); scoreSet2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(120.380, 59.830), new Pose(88.458, 82.280), new Pose(90, 90))) + .addPath(new BezierCurve(new Pose(128, 59.830), new Pose(88.458, 82.280), new Pose(90, 90))) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(36)) .build(); grabGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(90, 90), new Pose(103.881, 55.595), new Pose(132.600, 59.600))) + .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)) .build(); scoreGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(132.600, 59.600), new Pose(102.082, 62.187), new Pose(90, 90))) + .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)) .build(); grabGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(90, 90), new Pose(101.919, 61.914), new Pose(132.600, 59.600))) + .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)) .build(); scoreGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(132.600, 59.600), new Pose(100.596, 100.236), new Pose(90, 90))) + .addPath(new BezierCurve(new Pose(132, 58.3), new Pose(100, 62), new Pose(90, 90))) .setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36)) .build(); @@ -299,12 +299,12 @@ public class Auton { .build(); grabSet3Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(105.485, 47.156), new Pose(114.378, 37.842), new Pose(127.581, 35.945))) + .addPath(new BezierCurve(new Pose(105.485, 47.156), new Pose(114.378, 37.842), new Pose(132, 36))) .setLinearHeadingInterpolation(Math.toRadians(-40), Math.toRadians(-5)) .build(); scoreSet3Path = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(127.581, 35.945), new Pose(134.406, 52.541), new Pose(90, 90))) + .addPath(new BezierCurve(new Pose(132, 36), new Pose(134.406, 52.541), new Pose(90, 90))) .setLinearHeadingInterpolation(Math.toRadians(-5), Math.toRadians(36)) .build(); @@ -345,8 +345,8 @@ public class Auton { protected final boolean isNet; protected final boolean useTimer; - public static double timerTimeout = 4.5; - public static double gateWaitTime = 4.0; + public static double timerTimeout = 2.8; + public static double gateWaitTime = 3.0; protected Pose startPose; 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 48fd4dc..a409780 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 @@ -149,7 +149,7 @@ public class Turret { public void idleLaunch() { - targetVelocity = 1300; + targetVelocity = 1600; hardware.setTurretVelocity(targetVelocity); double calculatedHoodPos = getHoodPOS(65); @@ -394,7 +394,7 @@ public class Turret { double error = Math.abs(absCurrent - absTarget); double tolerance = absTarget * 0.05; - return error <= tolerance; + return error <= tolerance || absCurrent > absTarget; } return false; } @@ -493,7 +493,7 @@ public class Turret { + (0.00636447 * x3) + (-0.593959 * x2) + (26.08276 * x) - + 1218.12895; + + 1340.12895; return Range.clip(velocity, 1300.0, 2450.0); }