Merge pull request #70 from ftc16072/AutoChanges

Auto Trajectory Organization
ftc16072 · Feb 20, 2024 · 2bc1c9d · 2bc1c9d
2 parents 5d1b5e1 + 807da79
commit 2bc1c9d
Show file tree

Hide file tree

Showing 9 changed files with 107 additions and 12 deletions.
diff --git a/...ikeTrajectory/SetLeftSpikeTrajectory.java → ...jectory/Spike/SetLeftSpikeTrajectory.java b/...ikeTrajectory/SetLeftSpikeTrajectory.java → ...jectory/Spike/SetLeftSpikeTrajectory.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory;
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory.Spike;
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;

diff --git a/...ajectory/SetLeftSpikeTrajectoryPart2.java → ...ry/Spike/SetLeftSpikeTrajectoryPart2.java b/...ajectory/SetLeftSpikeTrajectoryPart2.java → ...ry/Spike/SetLeftSpikeTrajectoryPart2.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory;
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory.Spike;
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;

diff --git a/...ees/Actions/Trajectories/LeftSpikeTrajectory/ToBackboard/SetLeftSpikeTrajectoryPart3.java b/...ees/Actions/Trajectories/LeftSpikeTrajectory/ToBackboard/SetLeftSpikeTrajectoryPart3.java
@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory.ToBackboard;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.SpikePosition;
+import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequenceBuilder;
+
+public class SetLeftSpikeTrajectoryPart3 extends Node {
+
+    //EDIT (as of 2/18)
+    public static final int FORWARD_DISTANCE_INCHES=20;
+
+    @Override
+    public State tick(QQOpMode opMode) {
+        TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = builder.back(FORWARD_DISTANCE_INCHES).
+                                            build();
+        if (sequence == null){
+            return State.FAILURE;
+        }
+        opMode.board.add("TrajectorySequence", sequence);
+        opMode.board.add("SpikePosition", SpikePosition.RIGHT);
+        return State.SUCCESS;
+    }
+}
diff --git a/...rajectories/SetMiddleSpikeTrajectory.java → ...ctory/Spike/SetMiddleSpikeTrajectory.java b/...rajectories/SetMiddleSpikeTrajectory.java → ...ctory/Spike/SetMiddleSpikeTrajectory.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories;
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.MiddleSpikeTrajectory.Spike;
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;

diff --git a/...Actions/Trajectories/MiddleSpikeTrajectory/ToBackboard/SetMiddleSpikeTrajectoryPart2.java b/...Actions/Trajectories/MiddleSpikeTrajectory/ToBackboard/SetMiddleSpikeTrajectoryPart2.java
@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.MiddleSpikeTrajectory.ToBackboard;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.SpikePosition;
+import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequenceBuilder;
+
+public class SetMiddleSpikeTrajectoryPart2 extends Node {
+    public static final int FORWARD_DISTANCE_INCHES = 24;
+    public static final int BACKWARD_DISTANCE_INCHES = 20;
+
+    @Override
+    public State tick(QQOpMode opMode) {
+        TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = builder
+                .back(FORWARD_DISTANCE_INCHES).
+                build();
+        if (sequence == null){
+            return State.FAILURE;
+        }
+        opMode.board.add("TrajectorySequence", sequence);
+        opMode.board.add("SpikePosition", SpikePosition.MIDDLE);
+        return State.SUCCESS;
+    }
+}
diff --git a/...keTrajectory/SetRightSpikeTrajectory.java → ...ectory/Spike/SetRightSpikeTrajectory.java b/...keTrajectory/SetRightSpikeTrajectory.java → ...ectory/Spike/SetRightSpikeTrajectory.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory;
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.Spike;
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;

diff --git a/...jectory/SetRightSpikeTrajectoryPart2.java → ...y/Spike/SetRightSpikeTrajectoryPart2.java b/...jectory/SetRightSpikeTrajectoryPart2.java → ...y/Spike/SetRightSpikeTrajectoryPart2.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory;
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.Spike;
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;

diff --git a/...s/Actions/Trajectories/RightSpikeTrajectory/ToBackboard/SetRightSpikeTrajectoryPart3.java b/...s/Actions/Trajectories/RightSpikeTrajectory/ToBackboard/SetRightSpikeTrajectoryPart3.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.ToBackboard;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.SpikePosition;
+import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequenceBuilder;
+
+public class SetRightSpikeTrajectoryPart3 extends Node {
+    //EDIT (as of 2/18)
+    public static final int FORWARD_DISTANCE_INCHES=20;
+
+    @Override
+    public State tick(QQOpMode opMode) {
+        TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = builder.back(FORWARD_DISTANCE_INCHES).
+                                            build();
+        if (sequence == null){
+            return State.FAILURE;
+        }
+        opMode.board.add("TrajectorySequence", sequence);
+        opMode.board.add("SpikePosition", SpikePosition.RIGHT);
+        return State.SUCCESS;
+    }
+}
diff --git a/.../main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java b/.../main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java
@@ -2,13 +2,15 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpikeLocationTelemetry;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReleaseAutoPixel;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetBackboardFromSpikeTrajectory;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.FollowTrajectory;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory.SetLeftSpikeTrajectory;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory.SetLeftSpikeTrajectoryPart2;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.SetRightSpikeTrajectoryPart2;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.SetMiddleSpikeTrajectory;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.SetRightSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory.Spike.SetLeftSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory.Spike.SetLeftSpikeTrajectoryPart2;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory.ToBackboard.SetLeftSpikeTrajectoryPart3;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.MiddleSpikeTrajectory.ToBackboard.SetMiddleSpikeTrajectoryPart2;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.Spike.SetRightSpikeTrajectoryPart2;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.MiddleSpikeTrajectory.Spike.SetMiddleSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.Spike.SetRightSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.ToBackboard.SetRightSpikeTrajectoryPart3;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsLeftSpike;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsRightSpike;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover;
@@ -33,6 +35,7 @@ public static Node root(){
                 //shows spike location
                 new SpikeLocationTelemetry(),
                 //checks and sets trajectory for each spike position
+                //goes to tape
                 new Failover(
                         new Sequence(
                                 new IsLeftSpike(),
@@ -58,7 +61,22 @@ public static Node root(){
                 //Move servo to release purple pixel
                 new ReleaseAutoPixel(),
                 //set trajectory to backboard from tape location
-                new SetBackboardFromSpikeTrajectory(),
+                new Failover(
+                        new Sequence(
+                                new IsLeftSpike(),
+                                new SetLeftSpikeTrajectoryPart3(),
+                                new FollowTrajectory()
+                        ),
+                        new Sequence(
+                                new IsRightSpike(),
+                                new SetRightSpikeTrajectoryPart3(),
+                                new FollowTrajectory()
+                        ),
+                        new Sequence(
+                                new SetMiddleSpikeTrajectoryPart2(),
+                                new FollowTrajectory()
+                        )
+                ),
                 //follow backboard trajectory
                 new FollowTrajectory()