diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trajectories/BlueClose/LeftSpike/BC6LeftSpikeTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trajectories/BlueClose/LeftSpike/BC6LeftSpikeTrajectory.java new file mode 100644 index 0000000..580c678 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trajectories/BlueClose/MiddleSpike/BC5MiddleSpikeTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trajectories/BlueClose/MiddleSpike/BC5MiddleSpikeTrajectory.java new file mode 100644 index 0000000..7a2eb33 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trajectories/BlueClose/RightSpike/BC6RightSpikeTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trajectories/BlueClose/RightSpike/BC6RightSpikeTrajectory.java new file mode 100644 index 0000000..1de8600 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/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; + } +}