add trajectory to move out of partner way

ftc16072 · Feb 24, 2024 · e555d27 · e555d27
1 parent 35a049b
commit e555d27
Show file tree

Hide file tree

Showing 3 changed files with 72 additions and 0 deletions.
diff --git a/...mcode/ftc16072/BehaviorTrees/Trajectories/BlueClose/LeftSpike/BC6LeftSpikeTrajectory.java b/...mcode/ftc16072/BehaviorTrees/Trajectories/BlueClose/LeftSpike/BC6LeftSpikeTrajectory.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trajectories.BlueClose.LeftSpike;
+
+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 BC6LeftSpikeTrajectory extends Node {
+    public static final int RIGHT_DISTANCE_INCHES=40;
+
+    @Override
+    public State tick(QQOpMode opMode) {
+        TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = builder.strafeRight(RIGHT_DISTANCE_INCHES).
+                build();
+        if (sequence == null){
+            return State.FAILURE;
+        }
+        opMode.board.add("TrajectorySequence", sequence);
+        opMode.board.add("SpikePosition", SpikePosition.LEFT);
+        return State.SUCCESS;
+    }
+}
diff --git a/...e/ftc16072/BehaviorTrees/Trajectories/BlueClose/MiddleSpike/BC5MiddleSpikeTrajectory.java b/...e/ftc16072/BehaviorTrees/Trajectories/BlueClose/MiddleSpike/BC5MiddleSpikeTrajectory.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trajectories.BlueClose.MiddleSpike;
+
+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 BC5MiddleSpikeTrajectory extends Node {
+    public static final int RIGHT_DISTANCE_INCHES=40;
+
+    @Override
+    public State tick(QQOpMode opMode) {
+        TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = builder.strafeRight(RIGHT_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/...ode/ftc16072/BehaviorTrees/Trajectories/BlueClose/RightSpike/BC6RightSpikeTrajectory.java b/...ode/ftc16072/BehaviorTrees/Trajectories/BlueClose/RightSpike/BC6RightSpikeTrajectory.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trajectories.BlueClose.RightSpike;
+
+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 BC6RightSpikeTrajectory extends Node {
+    public static final int RIGHT_DISTANCE_INCHES=40;
+
+    @Override
+    public State tick(QQOpMode opMode) {
+        TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = builder.strafeRight(RIGHT_DISTANCE_INCHES).
+                build();
+        if (sequence == null){
+            return State.FAILURE;
+        }
+        opMode.board.add("TrajectorySequence", sequence);
+        opMode.board.add("SpikePosition", SpikePosition.RIGHT);
+        return State.SUCCESS;
+    }
+}