preuil
This commit is contained in:
@@ -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;
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -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());
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user