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 f105e1d..b66c0be 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 @@ -31,80 +31,58 @@ public class Auton { @Override public void buildPaths() { - // Start Pose startPose = new Pose(59, 9, Math.toRadians(90)); follower.setStartingPose(startPose); - // Poses - Pose scorePosePreload = new Pose(59, 15, Math.toRadians(118)); - Pose scorePoseGeneric = new Pose(59, 15, Math.toRadians(118)); - Pose set1Inter = new Pose(44, 36, Math.toRadians(180)); - Pose set1Pick = new Pose(17, 36, Math.toRadians(180)); - Pose set2Inter = new Pose(41, 60, Math.toRadians(180)); - Pose set2Pick = new Pose(16, 63, Math.toRadians(180)); - Pose set2Control1 = new Pose(17.711, 56.837); - Pose set2Control2 = new Pose(25.306, 62.677); - Pose scoreSet1Control = new Pose(36.621, 19.900); - - // Corner Specific Poses - Pose cornerInter = new Pose(9.444, 24.561, Math.toRadians(-90)); - Pose cornerInterControl = new Pose(37.157, 26.830); - Pose cornerArtifact1 = new Pose(12.598, 18.008, Math.toRadians(-135)); - Pose cornerArtifact1Control = new Pose(11.997, 24.364); - Pose cornerArtifact2 = new Pose(10.551, 13.380, Math.toRadians(-160)); - Pose cornerArtifact3 = new Pose(9.712, 8.993, Math.toRadians(-160)); - Pose parkPose = new Pose(54.614, 19.545, Math.toRadians(135)); - // --- PATH DEFINITIONS --- scorePreloadPath = follower.pathBuilder() - .addPath(new BezierLine(startPose, scorePosePreload)) - .setLinearHeadingInterpolation(startPose.getHeading(), scorePosePreload.getHeading()) + .addPath(new BezierLine(new Pose(59, 9), new Pose(59, 15))) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(118)) .build(); grabSet1Path = follower.pathBuilder() - .addPath(new BezierLine(scorePosePreload, set1Inter)) - .setLinearHeadingInterpolation(scorePosePreload.getHeading(), Math.toRadians(180)) - .addPath(new BezierLine(set1Inter, set1Pick)) + .addPath(new BezierLine(new Pose(59, 15), new Pose(44, 36))) + .setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180)) + .addPath(new BezierLine(new Pose(44, 36), new Pose(17, 36))) .setConstantHeadingInterpolation(Math.toRadians(180)) .build(); scoreSet1Path = follower.pathBuilder() - .addPath(new BezierCurve(set1Pick, scoreSet1Control, scorePoseGeneric)) - .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading()) + .addPath(new BezierCurve(new Pose(17, 36), new Pose(36.621, 19.900), new Pose(59, 15))) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118)) .build(); grabSet2Path = follower.pathBuilder() - .addPath(new BezierLine(scorePoseGeneric, set2Inter)) - .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), Math.toRadians(180)) - .addPath(new BezierCurve(set2Inter, set2Control1, set2Control2, set2Pick)) + .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(set2Pick, scorePoseGeneric)) - .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading()) + .addPath(new BezierLine(new Pose(16, 63), new Pose(59, 15))) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118)) .build(); - // Corner Sequence grabEndGamePath = follower.pathBuilder() - .addPath(new BezierCurve(scorePoseGeneric, cornerInterControl, cornerInter)) - .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), Math.toRadians(-90)) - .addPath(new BezierCurve(cornerInter, cornerArtifact1Control, cornerArtifact1)) - .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-135)) - .addPath(new BezierLine(cornerArtifact1, cornerArtifact2)) - .setLinearHeadingInterpolation(Math.toRadians(-135), Math.toRadians(-160)) - .addPath(new BezierLine(cornerArtifact2, cornerArtifact3)) - .setConstantHeadingInterpolation(Math.toRadians(-160)) + .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)) .build(); scoreEndGamePath = follower.pathBuilder() - .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric)) - .setLinearHeadingInterpolation(Math.toRadians(-160), scorePoseGeneric.getHeading()) + .addPath(new BezierLine(new Pose(10.400, 8), new Pose(59, 15))) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118)) .build(); parkPath = follower.pathBuilder() - .addPath(new BezierLine(scorePoseGeneric, parkPose)) - .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), parkPose.getHeading()) + .addPath(new BezierCurve(new Pose(59, 15), new Pose(37.181, 26.706), new Pose(10.400, 27))) + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(180)) + .addPath(new BezierLine(new Pose(10.400, 27), new Pose(10.439, 17.195))) + .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))) + .setConstantHeadingInterpolation(Math.toRadians(135)) .build(); } } @@ -120,106 +98,79 @@ public class Auton { startPose = new Pose(21, 124, Math.toRadians(143.5)); follower.setStartingPose(startPose); - // Waypoints - Pose preloadScorePose = new Pose(54, 90, Math.toRadians(144)); - Pose set2DrivePose = new Pose(39, 60, Math.toRadians(180)); - Pose set2PickPose = new Pose(14, 60, Math.toRadians(180)); // tangential - Pose set2ScorePose = new Pose(54, 90, Math.toRadians(144)); - Pose gate1PickPose = new Pose(9, 60, Math.toRadians(143.5)); - Pose gate1ScorePose = new Pose(54, 90, Math.toRadians(144)); - Pose gate2PickPose = new Pose(9, 60, Math.toRadians(143.5)); - Pose gate2ScorePose = new Pose(54, 90, Math.toRadians(144)); - Pose set3DrivePose = new Pose(38.515, 47.156, Math.toRadians(220)); - Pose set3PickPose = new Pose(16.419, 35.945, Math.toRadians(185)); - Pose set3ScorePose = new Pose(54, 90, Math.toRadians(144)); - Pose set1DrivePose = new Pose(39, 84, Math.toRadians(180)); - Pose set1PickPose = new Pose(29.689, 106.279, Math.toRadians(144)); - Pose set1ScorePose = new Pose(44, 100, Math.toRadians(148)); - Pose parkTargetPose = new Pose(36.219, 93.170, Math.toRadians(148)); - - // Controls - Pose set2ScoreControl = new Pose(50.386, 78.923); - Pose gate1PickControl = new Pose(40.119, 55.595); - Pose gate1ScoreControl = new Pose(41.918, 62.187); - Pose gate2PickControl = new Pose(42.081, 61.914); - Pose gate2ScoreControl = new Pose(42.318, 61.514); - Pose set3PickControl = new Pose(29.622, 37.842); - Pose set3ScoreControl = new Pose(9.594, 52.541); - Pose set1PickControl = new Pose(14.863, 77.692); - scorePreloadPath = follower.pathBuilder() - .addPath(new BezierLine(startPose, preloadScorePose)) - .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading()) + .addPath(new BezierLine(new Pose(21, 124), new Pose(54, 90))) + .setLinearHeadingInterpolation(Math.toRadians(143.5), Math.toRadians(144)) .build(); driveToSet2Path = follower.pathBuilder() - .addPath(new BezierLine(preloadScorePose, set2DrivePose)) - .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading()) + .addPath(new BezierLine(new Pose(54, 90), new Pose(41.758, 61.439))) + .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(180)) .build(); grabSet2Path = follower.pathBuilder() - .addPath(new BezierLine(set2DrivePose, set2PickPose)) - .setTangentHeadingInterpolation() + .addPath(new BezierLine(new Pose(41.758, 61.439), new Pose(23.620, 59.830))) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) .build(); scoreSet2Path = follower.pathBuilder() - .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose)) - .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading()) + .addPath(new BezierCurve(new Pose(23.620, 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(set2ScorePose, gate1PickControl, gate1PickPose)) - .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading()) + .addPath(new BezierCurve(new Pose(54, 90), new Pose(40.119, 55.595), new Pose(11.400, 59.600))) + .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(138)) .build(); scoreGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose)) - .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading()) + .addPath(new BezierCurve(new Pose(11.400, 59.600), new Pose(41.918, 62.187), new Pose(54, 90))) + .setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144)) .build(); grabGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose)) - .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading()) + .addPath(new BezierCurve(new Pose(54, 90), new Pose(42.081, 61.914), new Pose(11.400, 59.600))) + .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(138)) .build(); scoreGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose)) - .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading()) + .addPath(new BezierCurve(new Pose(11.400, 59.600), new Pose(43.404, 100.236), new Pose(54, 90))) + .setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144)) .build(); driveToSet3Path = follower.pathBuilder() - .addPath(new BezierLine(gate2ScorePose, set3DrivePose)) - .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading()) + .addPath(new BezierLine(new Pose(54, 90), new Pose(38.515, 47.156))) + .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(220)) .build(); grabSet3Path = follower.pathBuilder() - .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose)) - .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading()) + .addPath(new BezierCurve(new Pose(38.515, 47.156), new Pose(29.622, 37.842), new Pose(16.419, 35.945))) + .setLinearHeadingInterpolation(Math.toRadians(220), Math.toRadians(185)) .build(); scoreSet3Path = follower.pathBuilder() - .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose)) - .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading()) + .addPath(new BezierCurve(new Pose(16.419, 35.945), new Pose(9.594, 52.541), new Pose(54, 90))) + .setLinearHeadingInterpolation(Math.toRadians(185), Math.toRadians(144)) .build(); driveToSet1Path = follower.pathBuilder() - .addPath(new BezierLine(set3ScorePose, set1DrivePose)) - .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading()) + .addPath(new BezierLine(new Pose(54, 90), new Pose(39, 84))) + .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(180)) .build(); grabSet1Path = follower.pathBuilder() - .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose)) - .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading()) + .addPath(new BezierCurve(new Pose(39, 84), new Pose(14.863, 77.692), new Pose(29.689, 106.279))) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(144)) .build(); scoreSet1Path = follower.pathBuilder() - .addPath(new BezierLine(set1PickPose, set1ScorePose)) - .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading()) + .addPath(new BezierLine(new Pose(29.689, 106.279), new Pose(44, 100))) + .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(148)) .build(); parkPath = follower.pathBuilder() - .addPath(new BezierLine(set1ScorePose, parkTargetPose)) - .setConstantHeadingInterpolation(set1ScorePose.getHeading()) + .addPath(new BezierLine(new Pose(44, 100), new Pose(36.219, 93.170))) + .setConstantHeadingInterpolation(Math.toRadians(148)) .build(); } } @@ -239,74 +190,54 @@ public class Auton { startPose = new Pose(85, 9, Math.toRadians(90)); follower.setStartingPose(startPose); - Pose scorePosePreload = new Pose(85, 15, Math.toRadians(62)); - Pose scorePoseGeneric = new Pose(85, 15, Math.toRadians(62)); - Pose set1Inter = new Pose(100, 36, Math.toRadians(0)); - Pose set1Pick = new Pose(127, 36, Math.toRadians(0)); - Pose set2Inter = new Pose(103, 60, Math.toRadians(0)); - Pose set2Pick = new Pose(128, 63, Math.toRadians(0)); - Pose set2Control1 = new Pose(126.289, 56.837); - Pose set2Control2 = new Pose(118.694, 62.677); - Pose scoreSet1Control = new Pose(107.379, 19.900); - - // Corner Poses - Pose cornerInter = new Pose(134.556, 24.561, Math.toRadians(270)); - Pose cornerInterControl = new Pose(106.843, 26.830); - Pose cornerArtifact1 = new Pose(131.402, 18.008, Math.toRadians(315)); - Pose cornerArtifact1Control = new Pose(132.003, 24.364); - Pose cornerArtifact2 = new Pose(133.449, 13.380, Math.toRadians(340)); - Pose cornerArtifact3 = new Pose(134.288, 8.993, Math.toRadians(340)); - Pose parkPose = new Pose(89.386, 19.545, Math.toRadians(45)); - scorePreloadPath = follower.pathBuilder() - .addPath(new BezierLine(startPose, scorePosePreload)) - .setLinearHeadingInterpolation(startPose.getHeading(), Math.toRadians(62)) + .addPath(new BezierLine(new Pose(85, 9), new Pose(85, 15))) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(62)) .build(); grabSet1Path = follower.pathBuilder() - .addPath(new BezierLine(scorePosePreload, set1Inter)) + .addPath(new BezierLine(new Pose(85, 15), new Pose(100, 36))) .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0)) - .addPath(new BezierLine(set1Inter, set1Pick)) + .addPath(new BezierLine(new Pose(100, 36), new Pose(127, 36))) .setConstantHeadingInterpolation(Math.toRadians(0)) .build(); scoreSet1Path = follower.pathBuilder() - .addPath(new BezierCurve(set1Pick, scoreSet1Control, scorePoseGeneric)) + .addPath(new BezierCurve(new Pose(127, 36), new Pose(107.379, 19.900), new Pose(85, 15))) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62)) .build(); grabSet2Path = follower.pathBuilder() - .addPath(new BezierLine(scorePoseGeneric, set2Inter)) + .addPath(new BezierLine(new Pose(85, 15), new Pose(103, 60))) .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0)) - .addPath(new BezierCurve(set2Inter, set2Control1, set2Control2, set2Pick)) + .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(set2Pick, scorePoseGeneric)) + .addPath(new BezierLine(new Pose(128, 63), new Pose(85, 15))) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62)) .build(); - // Corner Sequence grabEndGamePath = follower.pathBuilder() - .addPath(new BezierCurve(scorePoseGeneric, cornerInterControl, cornerInter)) - .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(270)) - .addPath(new BezierCurve(cornerInter, cornerArtifact1Control, cornerArtifact1)) - .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(315)) - .addPath(new BezierLine(cornerArtifact1, cornerArtifact2)) - .setLinearHeadingInterpolation(Math.toRadians(315), Math.toRadians(340)) - .addPath(new BezierLine(cornerArtifact2, cornerArtifact3)) - .setConstantHeadingInterpolation(Math.toRadians(340)) + .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)) .build(); scoreEndGamePath = follower.pathBuilder() - .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric)) - .setLinearHeadingInterpolation(Math.toRadians(340), Math.toRadians(62)) + .addPath(new BezierLine(new Pose(133.600, 8), new Pose(85, 15))) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62)) .build(); parkPath = follower.pathBuilder() - .addPath(new BezierLine(scorePoseGeneric, parkPose)) - .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(45)) + .addPath(new BezierCurve(new Pose(85, 15), new Pose(106.819, 26.706), new Pose(133.600, 27))) + .setLinearHeadingInterpolation(Math.toRadians(45), Math.toRadians(0)) + .addPath(new BezierLine(new Pose(133.600, 27), new Pose(133.561, 17.195))) + .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))) + .setConstantHeadingInterpolation(Math.toRadians(45)) .build(); } } @@ -319,110 +250,82 @@ public class Auton { @Override public void buildPaths() { - startPose = new Pose(122, 125.5, Math.toRadians(36)); + startPose = new Pose(123, 124, Math.toRadians(36.5)); follower.setStartingPose(startPose); - // Waypoints - Pose preloadScorePose = new Pose(90, 90, Math.toRadians(36)); - Pose set2DrivePose = new Pose(105, 60, Math.toRadians(0)); - Pose set2PickPose = new Pose(130, 60, Math.toRadians(0)); // tangential - Pose set2ScorePose = new Pose(90, 90, Math.toRadians(36)); - Pose gate1PickPose = new Pose(132.7, 56, Math.toRadians(36)); - Pose gate1ScorePose = new Pose(90, 90, Math.toRadians(36)); - Pose gate2PickPose = new Pose(132.7, 56, Math.toRadians(36)); - Pose gate2ScorePose = new Pose(90, 90, Math.toRadians(36)); - Pose set3DrivePose = new Pose(105.485, 47.156, Math.toRadians(-40)); - Pose set3PickPose = new Pose(127.581, 35.945, Math.toRadians(-5)); - Pose set3ScorePose = new Pose(90, 90, Math.toRadians(36)); - Pose set1DrivePose = new Pose(105, 84, Math.toRadians(0)); - Pose set1PickPose = new Pose(114.311, 106.279, Math.toRadians(36)); - Pose set1ScorePose = new Pose(100, 100, Math.toRadians(32)); - Pose parkTargetPose = new Pose(107.781, 93.170, Math.toRadians(32)); - - // Controls - Pose preloadControl = new Pose(116.600, 115.541); - Pose set2ScoreControl = new Pose(93.614, 78.923); - Pose gate1PickControl = new Pose(103.881, 55.595); - Pose gate1ScoreControl = new Pose(102.082, 62.187); - Pose gate2PickControl = new Pose(101.919, 61.914); - Pose gate2ScoreControl = new Pose(101.682, 61.514); - Pose set3PickControl = new Pose(114.378, 37.842); - Pose set3ScoreControl = new Pose(134.406, 52.541); - Pose set1PickControl = new Pose(129.137, 77.692); - scorePreloadPath = follower.pathBuilder() - .addPath(new BezierCurve(startPose, preloadControl, preloadScorePose)) - .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading()) + .addPath(new BezierLine(new Pose(123, 124), new Pose(90, 90))) + .setLinearHeadingInterpolation(Math.toRadians(36.5), Math.toRadians(36)) .build(); driveToSet2Path = follower.pathBuilder() - .addPath(new BezierLine(preloadScorePose, set2DrivePose)) - .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading()) + .addPath(new BezierLine(new Pose(90, 90), new Pose(102.242, 61.439))) + .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(0)) .build(); grabSet2Path = follower.pathBuilder() - .addPath(new BezierLine(set2DrivePose, set2PickPose)) - .setTangentHeadingInterpolation() + .addPath(new BezierLine(new Pose(102.242, 61.439), new Pose(120.380, 59.830))) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)) .build(); scoreSet2Path = follower.pathBuilder() - .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose)) - .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading()) + .addPath(new BezierCurve(new Pose(120.380, 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(set2ScorePose, gate1PickControl, gate1PickPose)) - .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading()) + .addPath(new BezierCurve(new Pose(90, 90), new Pose(103.881, 55.595), new Pose(132.600, 59.600))) + .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(42)) .build(); scoreGate1Path = follower.pathBuilder() - .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose)) - .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading()) + .addPath(new BezierCurve(new Pose(132.600, 59.600), new Pose(102.082, 62.187), new Pose(90, 90))) + .setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36)) .build(); grabGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose)) - .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading()) + .addPath(new BezierCurve(new Pose(90, 90), new Pose(101.919, 61.914), new Pose(132.600, 59.600))) + .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(42)) .build(); scoreGate2Path = follower.pathBuilder() - .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose)) - .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading()) + .addPath(new BezierCurve(new Pose(132.600, 59.600), new Pose(100.596, 100.236), new Pose(90, 90))) + .setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36)) .build(); driveToSet3Path = follower.pathBuilder() - .addPath(new BezierLine(gate2ScorePose, set3DrivePose)) - .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading()) + .addPath(new BezierLine(new Pose(90, 90), new Pose(105.485, 47.156))) + .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(-40)) .build(); grabSet3Path = follower.pathBuilder() - .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose)) - .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading()) + .addPath(new BezierCurve(new Pose(105.485, 47.156), new Pose(114.378, 37.842), new Pose(127.581, 35.945))) + .setLinearHeadingInterpolation(Math.toRadians(-40), Math.toRadians(-5)) .build(); scoreSet3Path = follower.pathBuilder() - .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose)) - .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading()) + .addPath(new BezierCurve(new Pose(127.581, 35.945), new Pose(134.406, 52.541), new Pose(90, 90))) + .setLinearHeadingInterpolation(Math.toRadians(-5), Math.toRadians(36)) .build(); driveToSet1Path = follower.pathBuilder() - .addPath(new BezierLine(set3ScorePose, set1DrivePose)) - .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading()) + .addPath(new BezierLine(new Pose(90, 90), new Pose(105, 84))) + .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(0)) .build(); grabSet1Path = follower.pathBuilder() - .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose)) - .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading()) + .addPath(new BezierCurve(new Pose(105, 84), new Pose(129.137, 77.692), new Pose(114.311, 106.279))) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(36)) .build(); scoreSet1Path = follower.pathBuilder() - .addPath(new BezierLine(set1PickPose, set1ScorePose)) - .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading()) + .addPath(new BezierLine(new Pose(114.311, 106.279), new Pose(100, 100))) + .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(32)) .build(); parkPath = follower.pathBuilder() - .addPath(new BezierLine(set1ScorePose, parkTargetPose)) - .setConstantHeadingInterpolation(set1ScorePose.getHeading()) + .addPath(new BezierLine(new Pose(100, 100), new Pose(107.781, 93.170))) + .setConstantHeadingInterpolation(Math.toRadians(32)) .build(); } } @@ -633,7 +536,7 @@ public class Auton { if (!follower.isBusy()) { systems.setAutonState(pedroStateMachine.AutonState.SCORING); if (checkScoringCondition()) { - setPathState(PathState.DRIVE_TO_PARK); + setPathState(PathState.DRIVE_TO_ENDGAME_COLLECTION); } } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java index ebe2917..908a3af 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java @@ -13,6 +13,7 @@ import org.firstinspires.ftc.teamcode.subsystems.TransferSys.TransferState; import org.firstinspires.ftc.teamcode.subsystems.Intake.IntakeState; import org.firstinspires.ftc.teamcode.util.AutoTransfer; +import org.firstinspires.ftc.teamcode.teleOp.Constants; public class pedroStateMachine { @@ -28,6 +29,10 @@ public class pedroStateMachine { private final Turret m_turret; + private boolean PIDFShooting = false; + private boolean lastPIDFShooting = false; + + private int alliance = 0; @@ -36,6 +41,9 @@ public class pedroStateMachine { this.m_transfer = new TransferSys(h_transfer); this.m_turret = new Turret(h_turret); + m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL); + m_turret.setLaunching(false); + setAutonState(AutonState.IDLE); AutoTransfer.reset(); } @@ -58,26 +66,28 @@ public class pedroStateMachine { switch (oldState) { case INTAKING: m_intake.setIntakeState(IntakeState.IDLE); - // Ensure gate is closed when we stop intaking to hold rings m_transfer.setTransferState(TransferState.IDLE); break; case SCORING: m_transfer.setTransferState(TransferState.IDLE); m_turret.setTurretState(Turret.TurretState.IDLE); m_intake.setIntakeState(Intake.IntakeState.IDLE); + PIDFShooting = false; + m_turret.setLaunching(false); + m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL); break; } } + private void handleStateEntry(AutonState newState) { switch (newState) { case INTAKING: m_intake.setIntakeState(IntakeState.INTAKE); - m_turret.setTurretState(TurretState.INTAKING); // Optional: position turret for intake stability - m_transfer.closeLimiter(); // Ensure rings don't fly out back + m_turret.setTurretState(TurretState.INTAKING); + m_transfer.closeLimiter(); break; case SCORING: - m_intake.setIntakeState(IntakeState.LAUNCH); // Feed rings m_turret.setTurretState(TurretState.LAUNCH); m_transfer.setTransferState(TransferState.LAUNCHING); break; @@ -94,10 +104,27 @@ public class pedroStateMachine { if (currentState == AutonState.SCORING) { m_transfer.updateTurretReady(m_turret.hasReachedVelocity()); + + if (m_turret.hasReachedVelocity() || PIDFShooting) { + m_intake.setIntakeState(IntakeState.LAUNCH); + PIDFShooting = true; + } + } else { + PIDFShooting = false; } - m_transfer.update(); + if (PIDFShooting != lastPIDFShooting) { + if (PIDFShooting) { + m_turret.setLaunching(true); // Freeze Vision Offsets + m_turret.setPIDF(Constants.TurretConstants.PIDF_Shooting); // High-power recovery PID + } else { + m_turret.setLaunching(false); // Resume Vision Tracking + m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL); // Precise tracking PID + } + } + lastPIDFShooting = PIDFShooting; + m_transfer.update(); if (currentState == AutonState.SCORING) { if (m_transfer.getTransferState() != TransferSys.TransferState.LAUNCHING) { 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 b6b57fb..550bb30 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 @@ -4,6 +4,7 @@ import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; + import org.firstinspires.ftc.teamcode.Hware.hwMap; import org.firstinspires.ftc.teamcode.teleOp.Constants; @@ -31,10 +32,13 @@ public class Turret { public double offsetHood = 0; private Pose robotPose; - private Vector robotVelocity = new Vector(0,0); // NEW + private Vector robotVelocity = new Vector(0, 0); // NEW private double kinematicDistanceOffset = 0.0; // NEW private double kinematicAngularOffsetTicks = 0.0; // NEW + private double distanceOffset = 0.0; + private double visionDistanceOffset = 0.0; + public double targetTick; private final ElapsedTime loopTimer = new ElapsedTime(); @@ -44,12 +48,15 @@ public class Turret { public boolean lastTurretModeHold = false; + private boolean isLaunching = false; + public enum TurretState { LAUNCH, IDLE, EXTAKE, INTAKING } + private TurretState currentState = TurretState.IDLE; public Turret(hwMap.TurretHwMap hardware) { @@ -124,7 +131,7 @@ public class Turret { double rawTicks = hardware.getTurretRotationPosition(); double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE; - distance = getDistance() + kinematicDistanceOffset; + distance = getDistance() + kinematicDistanceOffset + distanceOffset; if (!manualTracking) { calculateHybridTarget(currentPose, currentDegrees, rawTicks); @@ -135,16 +142,15 @@ public class Turret { if (currentState == TurretState.LAUNCH) { handleLaunchLogic(elapsedTime); - } - else { + } else { idleLaunch(); } } - public void idleLaunch() { - hardware.setTurretVelocity(1300); + targetVelocity = 1300; + hardware.setTurretVelocity(targetVelocity); double calculatedHoodPos = getHoodPOS(65); setHoodPos(calculatedHoodPos); @@ -170,7 +176,7 @@ public class Turret { calculateHybridTarget(currentPose, currentDegrees, rawTicks); runPIDControl(rawTicks, dt); - handleLaunchLogicOffset(0); + handleLaunchLogicOffset(offset); } private void handleLaunchLogic(double elapsedTime) { @@ -230,9 +236,7 @@ public class Turret { } - - - private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) { + /*private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) { double odoTargetTicks = calculateOdometryTargetTicks(robotPose); LLResult result = hardware.getLimelightResult(); @@ -253,7 +257,7 @@ public class Turret { // 4. Normalize the error for wrapping (360 degrees) double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE; - while (correctionError > (ticksPerRev / 2)) correctionError -= ticksPerRev; + while (correctionError > (ticksPerRev / 2)) correctionError -= ticksPerRev; while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev; // 5. Apply the error to the persistent offset (Bias-Shift) @@ -265,8 +269,53 @@ public class Turret { double fullWrapTicks = Constants.TurretConstants.TURRET_FULL_WRAP_TICKS; double halfWrapTicks = fullWrapTicks / 2.0; - if (rawTarget > halfWrapTicks) {rawTarget -= fullWrapTicks;} - else if (rawTarget < -halfWrapTicks) {rawTarget += fullWrapTicks;} + if (rawTarget > halfWrapTicks) { + rawTarget -= fullWrapTicks; + } else if (rawTarget < -halfWrapTicks) { + rawTarget += fullWrapTicks; + } + + fusedTargetTicks = Range.clip(rawTarget, Constants.TurretConstants.SOFT_LIMIT_NEG, Constants.TurretConstants.SOFT_LIMIT_POS); + targetTick = fusedTargetTicks; + }*/ // <-- Old Logic (relies on both LL and ODO [Even while shooting] + private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) { + double odoTargetTicks = calculateOdometryTargetTicks(robotPose); + + boolean isActivelyShooting = isLaunching(); + + if (!isActivelyShooting) { + LLResult result = hardware.getLimelightResult(); + + if (result != null && result.isValid()) { + double tx = result.getTx(); + double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER); + double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES; + double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE; + + visionTargetTicks += kinematicAngularOffsetTicks; + + double visionTargetWithManualOffset = visionTargetTicks + offsetTicks; + double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks; + double correctionError = visionTargetWithManualOffset - currentFusedTarget; + + double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE; + while (correctionError > (ticksPerRev / 2)) correctionError -= ticksPerRev; + while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev; + + targetCorrectionOffsetTicks += (correctionError * Constants.TurretConstants.OFFSET_SMOOTHING); + } + } + + double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks; + + double fullWrapTicks = Constants.TurretConstants.TURRET_FULL_WRAP_TICKS; + double halfWrapTicks = fullWrapTicks / 2.0; + + if (rawTarget > halfWrapTicks) { + rawTarget -= fullWrapTicks; + } else if (rawTarget < -halfWrapTicks) { + rawTarget += fullWrapTicks; + } fusedTargetTicks = Range.clip(rawTarget, Constants.TurretConstants.SOFT_LIMIT_NEG, Constants.TurretConstants.SOFT_LIMIT_POS); targetTick = fusedTargetTicks; @@ -313,8 +362,7 @@ public class Turret { hardware.setTurretRotationPower(Constants.TurretConstants.TURRET_HOLD_POWER); // Set a holding power } hardware.runToPos((int) fusedTargetTicks); // Keep target updated even in hold mode - } - else { + } else { if (lastTurretModeHold != turretModeHold) { hardware.disableTurretHold(); } @@ -358,13 +406,9 @@ public class Turret { } - public void updatePose(Pose robotPose) { // Fallback if called somewhere else without velocity - updatePose(robotPose, new Vector(0,0)); - } - public void updatePose(Pose robotPose, Vector robotVelocity) { this.robotPose = robotPose; - this.robotVelocity = robotVelocity != null ? robotVelocity : new Vector(0,0); + this.robotVelocity = robotVelocity != null ? robotVelocity : new Vector(0, 0); calculateKinematicOffsets(); } @@ -401,20 +445,28 @@ public class Turret { } public double getDistance() { - if (hardware.hasTarget()) { + double targetX = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_X : Constants.TurretConstants.GOAL_BLUE_X; + double targetY = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_Y : Constants.TurretConstants.GOAL_BLUE_Y; + + double odoDistance = 0.0; + if (robotPose != null) { + odoDistance = Math.hypot(targetX - robotPose.getX(), targetY - robotPose.getY()); + } + + boolean isActivelyShooting = isLaunching(); + + if (!isActivelyShooting && hardware.hasTarget()) { double ty = hardware.getTargetTY(); double angleToGoalDegrees = Constants.TurretConstants.CAMERA_MOUNT_ANGLE_DEGREES + ty; double angleToGoalRadians = Math.toRadians(angleToGoalDegrees); - return (Constants.TurretConstants.GOAL_HEIGHT_INCHES - Constants.TurretConstants.CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians); - } - else if (robotPose != null) { - double targetX = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_X : Constants.TurretConstants.GOAL_BLUE_X; - double targetY = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_Y : Constants.TurretConstants.GOAL_BLUE_Y; + double rawVisionDistance = (Constants.TurretConstants.GOAL_HEIGHT_INCHES - Constants.TurretConstants.CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians); - return Math.hypot(targetX - robotPose.getX(), targetY - robotPose.getY()); + double distanceError = rawVisionDistance - odoDistance; + + visionDistanceOffset += (distanceError - visionDistanceOffset) * 0.1; } - return 0.0; + return odoDistance + visionDistanceOffset; } private double calculateHoodOffset(double target, double current) { @@ -422,15 +474,49 @@ public class Turret { } private double getFlywheelVelocity(double dist) { - return (-0.0000144121 * Math.pow(dist, 4) - + 0.00398607 * Math.pow(dist, 3) - - 0.342198 * Math.pow(dist, 2) - + (16.43014 * dist) - + 1198.24434); - } + if (dist <= 0) return 0; + if (dist < 19.0) { + return 1300.0; + } + + if (dist > 145.0) { + return 2400.0; + } + + double x = dist; + double x2 = x * x; + double x3 = x2 * x; + double x4 = x3 * x; + + double velocity = (-0.00000898301 * x4) + + (0.00302546 * x3) + + (-0.300962 * x2) + + (16.04247 * x) + + 1327.9972; + + return Range.clip(velocity, 1300.0, 2450.0); + } private double getHoodPOS(double dist) { - return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859); + if (dist < 20.0) { + return 0.36; + } + + if (dist > 125.0) { + return 0.09; + } + double x = dist; + double x2 = x * x; + double x3 = x2 * x; + double x4 = x3 * x; + + double hoodPos = (5.34037e-9 * x4) + + (-1.70158e-6 * x3) + + (2.04794e-4 * x2) + + (-0.013153 * x) + + 0.55256; + + return Range.clip(hoodPos, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX); } public void setHoodPos(double pos) { @@ -454,6 +540,11 @@ public class Turret { return hardware.getFlywheelVelocities()[1]; } + public void setPIDF(double[] PIDF) { + hardware.setPIDF(PIDF[0], PIDF[1], PIDF[2], PIDF[3]); + } + + public double getTargetVelocity() { return targetVelocity; } @@ -466,4 +557,21 @@ public class Turret { public boolean isTurretRotationLocked() { return turretModeHold && Math.abs(lastError) <= 10; } + + public void updateDistanceOffset(double offset) { + distanceOffset += offset; + } + public void setDistanceOffset(double offset) { + distanceOffset = offset; + } + + + public void setLaunching(boolean state) { + isLaunching = state; + } + + public boolean isLaunching() { + return isLaunching; + } + } \ No newline at end of file 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 9fcc721..e0fd4a6 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 @@ -42,8 +42,8 @@ public class Constants { public static final String LED = "prism"; // TODO: Tune these! - public static final double COLOR_RED = 0.28; - public static final double COLOR_GREEN = 0.45; + public static final double COLOR_RED = 0.301; + public static final double COLOR_GREEN = 0.5; public static final double COLOR_ORANGE = 0.32; public static final double COLOR_YELLOW = 0.38; } @@ -107,11 +107,15 @@ public class Constants { public static double EXTERNAL_GEAR_RATIO = 9.179875; public static final double TICKS_PER_DEGREE = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0; - public static double KP = 0.0048; + public static double KP = 0.0043; public static double KI = 0.00801; - public static double KD = 0.00083; + public static double KD = 0.00076; 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_Shooting = {27, 0, 2, 2.4}; public static double GOAL_RED_X = 140; public static double GOAL_RED_Y = 140; @@ -129,9 +133,9 @@ public class Constants { public static double SOFT_LIMIT_NEG = -370; public static double SOFT_LIMIT_POS = 370; - public static final double HOOD_POS_CLOSE = 0.3; - public static final double HOOD_POS_FAR = 0.7; - public static final double HOOD_POS_MID = 0.5; + public static final double HOOD_POS_CLOSE = 0.1; + public static final double HOOD_POS_FAR = 0.5; + public static final double HOOD_POS_MID = 0.3; public static double HOOD_OFFSET = 0.3; @@ -140,7 +144,7 @@ public class Constants { public static final double TURRET_POWER_MID = -0.84; public static final double TURRET_POWER_MAX = -1; public static final double TURRET_POWER_LOW = -0.7; - public static final double TURRET_HOLD_POWER = 0.75; + public static final double TURRET_HOLD_POWER = 1.0; public static final double EXTAKE_POWER = 0.3; public static final double INTAKE_POWER = -0.04; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java index 25f0faa..946f7dc 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java @@ -146,7 +146,7 @@ public class SOLOTeleOP { private void configureStartSettings() { stateMachine.setRobotState(RobotState.TELEOP); - stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL); + stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO); stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID); stateMachine.getTransfer().closeLimiter(); stateMachine.initUpdate(); @@ -168,7 +168,7 @@ public class SOLOTeleOP { } if (stateMachine.getCurrentGameState() != GameState.SCORING) { - if (!stateMachine.getTurret().hasReachedVelocity() && (gamepad1.timestamp % 100 < 10)) { + if (!stateMachine.getTurret().hasReachedVelocity() && (System.currentTimeMillis() % 100 < 10)) { gamepad1.rumble(50); } } 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 c498763..a6777c9 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 @@ -39,6 +39,10 @@ public class StateMachine { public boolean isIntakeLaunching = false; private boolean isForceLaunch = false; + private boolean PIDFShooting = false; + + private boolean lastPIDFShooting = PIDFShooting; + public StateMachine(hwMap.DriveHwMap h_driveTrain, hwMap.IntakeHwMap h_intake, hwMap.LiftHwMap h_lift, hwMap.TransferHwMap h_transfer, hwMap.TurretHwMap h_turret, hwMap.LedHwMap h_led, int initialMotif, int alliance) { this.m_driveTrain = new DriveTrain(h_driveTrain); this.m_intake = new Intake(h_intake); @@ -48,6 +52,10 @@ public class StateMachine { this.m_led = new LED(h_led); + m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL); + m_turret.setLaunching(false); + + setRobotState(RobotState.INIT); setGameState(GameState.IDLE); } @@ -93,11 +101,29 @@ public class StateMachine { m_transfer.updateTurretReady(m_turret.hasReachedVelocity()); } - if (m_turret.hasReachedVelocity() || isForceLaunch) { + if (m_turret.hasReachedVelocity() || isForceLaunch || PIDFShooting) { isIntakeLaunching = true; m_intake.setIntakeState(Intake.IntakeState.LAUNCH); + PIDFShooting = true; + } + } else { + PIDFShooting = false; + } + + if (PIDFShooting != lastPIDFShooting) { + if (PIDFShooting) { + + m_turret.setLaunching(true); + m_turret.setPIDF(Constants.TurretConstants.PIDF_Shooting); + } else { + + m_turret.setLaunching(false); + m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL); } } + lastPIDFShooting = PIDFShooting; + + m_transfer.update(); 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 bec8a2b..33fed4b 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 @@ -10,6 +10,7 @@ import org.firstinspires.ftc.teamcode.Hware.hwMap; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.subsystems.DriveTrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.TransferSys; import org.firstinspires.ftc.teamcode.util.AutoTransfer; import org.firstinspires.ftc.teamcode.util.FPSCounter; import com.qualcomm.hardware.lynx.LynxModule; @@ -67,7 +68,7 @@ public class finalTeleOp { if (AutoTransfer.isAutonRan) { currentAlliance = AutoTransfer.alliance; if (currentAlliance != initialAlliance) { - telemetry.addLine("WARNING: AUTON ALLIANCE DOES NOT MATCH TELEOP ALLIANCE!"); + telemetry.addLine("!!! WARNING: AUTON ALLIANCE DOES NOT MATCH TELEOP ALLIANCE!"); } follower.setStartingPose(AutoTransfer.transferPose); telemetry.addLine("AUTON DATA LOADED"); @@ -75,6 +76,7 @@ public class finalTeleOp { currentAlliance = initialAlliance; if (currentAlliance == 2) follower.setStartingPose(DEFAULT_RED_START); else follower.setStartingPose(DEFAULT_BLUE_START); + stateMachine.getTurret().resetTurret(); telemetry.addLine("MANUAL START - NO AUTON DATA"); } @@ -112,18 +114,8 @@ public class finalTeleOp { stateMachine.update(manualSAM, manualTracking, currentPose, currentVelocity); - if (gamepad2.a) { - stateMachine.getTransfer().openLimiter(); - } - else { - if (stateMachine.getCurrentGameState() != GameState.SCORING) { - stateMachine.getTransfer().closeLimiter(); - } - } - updateTelemetry(currentPose); - sleep(fpsCounter.getSyncTime(100)); } stateMachine.setRobotState(RobotState.ESTOP); @@ -148,15 +140,14 @@ public class finalTeleOp { currentAlliance ); - - stateMachine.getTransfer().closeLimiter(); + stateMachine.getTransfer().setTransferState(TransferSys.TransferState.STOP); stateMachine.getTurret().setAlliance(currentAlliance); } private void configureStartSettings() { stateMachine.setRobotState(RobotState.TELEOP); - stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL); + stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO); stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID); stateMachine.getTransfer().closeLimiter(); stateMachine.initUpdate(); @@ -168,8 +159,10 @@ public class finalTeleOp { stateMachine.emergencyStop(); } - if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity() && (gamepad2.timestamp % 100 < 10)) { - gamepad2.rumble(50); + if (stateMachine.getCurrentGameState() != GameState.SCORING) { + if (!stateMachine.getTurret().hasReachedVelocity() && (System.currentTimeMillis() % 100 < 10)) { + gamepad1.rumble(50); + } } if (gamepad1.psWasPressed()) { @@ -217,8 +210,8 @@ public class finalTeleOp { - if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2); - if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.2); + if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(1); + if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(-1); } private void handleOperatorInput() { @@ -239,30 +232,22 @@ public class finalTeleOp { manualTracking = !manualTracking; } - if (manualSAM) { - handleManualSAM(); - } - } - - private void handleManualSAM() { - if (gamepad2.dpad_up) { - stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MAX); - stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_FAR); - } else if (gamepad2.dpad_left) { - stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID); - stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_MID); - } else if (gamepad2.dpad_down) { - stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_LOW); - stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_CLOSE); + if (gamepad2.aWasPressed()) { + stateMachine.forceLaunchSequence(); } - if (Math.abs(gamepad2.left_stick_y) > 0.13) { - hoodPosition += -gamepad2.left_stick_y * 0.05; - hoodPosition = Range.clip(hoodPosition, 0.0, 1.0); + if (gamepad2.dpadUpWasPressed()) { + stateMachine.getTurret().updateDistanceOffset(7); + } else if (gamepad2.dpadDownWasPressed()) { + stateMachine.getTurret().updateDistanceOffset(7); + } else if (gamepad2.dpadLeftWasPressed()) { + stateMachine.getTurret().setDistanceOffset(0); } - stateMachine.getTurret().setHoodPos(hoodPosition); + + } + private void updateTelemetry(Pose p) { telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java index a6350e3..82c685a 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.tuning; import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.OpMode; @@ -15,6 +16,8 @@ import com.qualcomm.robotcore.util.Range; // Make sure your import points to your actual Constants file import org.firstinspires.ftc.teamcode.teleOp.Constants; +import com.bylazar.telemetry.TelemetryManager; + @Configurable @TeleOp public class flywheelTuning extends OpMode { @@ -51,16 +54,20 @@ public class flywheelTuning extends OpMode { double snapshotAutoHood = 0.0; double calculatedGain = 0.0; - public static double P = 5; + public static double P = 1.3; public static double I = 0; - public static double D = 11; - public static double F = 14.3; + public static double D = 2; + public static double F = 13.5; private double lastP = P; private double lastI = I; private double lastD = D; private double lastF = F; + public static double distanceOffset = 0.0; + + private TelemetryManager telemetryM; + @Override public void init() { @@ -90,7 +97,7 @@ public class flywheelTuning extends OpMode { limiter.setPosition(1); - + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); telemetry.addLine("Init complete"); } @@ -103,7 +110,7 @@ public class flywheelTuning extends OpMode { if (result != null && result.isValid()) { double targetY = result.getTy(); telemetry.addData("Ty", targetY); - distance = getTrigBasedDistance(targetY); + distance = getTrigBasedDistance(targetY) - distanceOffset; } boolean currentX = gamepad1.x; @@ -220,6 +227,8 @@ public class flywheelTuning extends OpMode { telemetry.addData("Distance", "%.2f inches", distance); telemetry.addLine(); + telemetryM.addData("Current Velocity", currentVel); + if (autoMode) { telemetry.addData("Raw Auto Target Vel", "%.2f", autoTargetVel); telemetry.addData("Raw Auto Target Hood", "%.4f", autoHoodPos); @@ -232,6 +241,7 @@ public class flywheelTuning extends OpMode { } telemetry.update(); + telemetryM.update(telemetry); } private double getTrigBasedDistance(double targetOffsetAngleVertical) { @@ -240,14 +250,48 @@ public class flywheelTuning extends OpMode { } private double getFlywheelVelocity(double dist) { - return (-0.0000144121 * Math.pow(dist, 4) - + 0.00398607 * Math.pow(dist, 3) - - 0.342198 * Math.pow(dist, 2) - + (16.43014 * dist) - + 1198.24434); - } + if (dist <= 0) return 0; + if (dist < 19.0) { + return 1300.0; + } + + if (dist > 145.0) { + return 2400.0; + } + + double x = dist; + double x2 = x * x; + double x3 = x2 * x; + double x4 = x3 * x; + + double velocity = (-0.00000898301 * x4) + + (0.00302546 * x3) + + (-0.300962 * x2) + + (16.04247 * x) + + 1327.9972; + + return Range.clip(velocity, 1300.0, 2450.0); + } private double getHoodPOS(double dist) { - return (((3.55794 * Math.pow(10, -8)) * Math.pow(dist, 4)) + (-0.00000995887 * Math.pow(dist, 3)) - (0.000924672 * Math.pow(dist, 2)) + (-0.037262 * dist) + 0.943067); + if (dist < 20.0) { + return 0.36; + } + + if (dist > 125.0) { + return 0.09; + } + double x = dist; + double x2 = x * x; + double x3 = x2 * x; + double x4 = x3 * x; + + double hoodPos = (5.34037e-9 * x4) + + (-1.70158e-6 * x3) + + (2.04794e-4 * x2) + + (-0.013153 * x) + + 0.55256; + + return Range.clip(hoodPos, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX); } } \ No newline at end of file