Merge branch 'master' into debugBehaviorTrees

ftc16072 · Feb 18, 2024 · 8c4ea2d · 8c4ea2d
2 parents 2cf8032 + 1443ad9
commit 8c4ea2d
Show file tree

Hide file tree

Showing 47 changed files with 697 additions and 165 deletions.
diff --git a/...aviorTrees/Actions/SpinInIntakeMotor.java → ...haviorTrees/Actions/PlacePurplePixel.java b/...aviorTrees/Actions/SpinInIntakeMotor.java → ...haviorTrees/Actions/PlacePurplePixel.java
@@ -3,11 +3,10 @@
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 
-public class SpinInIntakeMotor extends Node {
-
+public class PlacePurplePixel extends Node {
     @Override
     public State tick(QQOpMode opmode) {
-        opmode.robot.intake.intake();
+        opmode.robot.pixelServo.releasePixel();
         return State.SUCCESS;
     }
 }
diff --git a/.../java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReleaseAutoPixel.java b/.../java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReleaseAutoPixel.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.SetLeftSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.SetMiddleSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.SetRightSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.PixelServo;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class ReleaseAutoPixel extends Node {
+
+
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.robot.pixelServo.releasePixel();
+        return State.SUCCESS;
+    }
+
+
+}
diff --git a/...inspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetBackboardFromSpikeTrajectory.java b/...inspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetBackboardFromSpikeTrajectory.java
@@ -0,0 +1,71 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.Alliance;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.SpikePosition;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.StartPosition;
+import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequenceBuilder;
+
+public class SetBackboardFromSpikeTrajectory extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        Alliance alliance = opmode.board.get(Alliance.class, "Alliance");
+        StartPosition startPosition = opmode.board.get(StartPosition.class,"StartPosition");
+        SpikePosition spikePosition = opmode.board.get(SpikePosition.class,"SpikePosition");
+        TrajectorySequenceBuilder builder = opmode.robot.nav.trajectorySequenceBuilder(opmode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = null;
+
+        if(alliance == Alliance.RED){
+            if(startPosition == StartPosition.CLOSE){
+                if(spikePosition == SpikePosition.LEFT){
+                    //TODO: Put RedCloseLeft trajectory builder here
+                } else if (spikePosition == SpikePosition.RIGHT) {
+                    //TODO: Put REDCLOSERight trajectory builder here
+                }
+                else if (spikePosition == SpikePosition.MIDDLE){
+                    //TODO: Put REDCLOSEMIDDLE trajectory builder here
+                }
+            } else if (startPosition == StartPosition.FAR){
+                if(spikePosition == SpikePosition.LEFT){
+                    //TODO: Put RedFarLeft trajectory builder here
+                } else if (spikePosition == SpikePosition.RIGHT) {
+                    //TODO: Put REDFarRight trajectory builder here
+                }
+                else if (spikePosition == SpikePosition.MIDDLE){
+                    //TODO: Put REDFarMIDDLE trajectory builder here
+                }
+            }
+        }
+        if(alliance == Alliance.BLUE){
+            if(startPosition == StartPosition.CLOSE){
+                if(spikePosition == SpikePosition.LEFT){
+                    //TODO: Put BlueCloseLeft trajectory builder here
+                } else if (spikePosition == SpikePosition.RIGHT) {
+                    //TODO: Put BlueCLOSERight trajectory builder here
+                }
+                else if (spikePosition == SpikePosition.MIDDLE){
+                    //TODO: Put BlueCLOSEMIDDLE trajectory builder here
+                }
+            } else if (startPosition == StartPosition.FAR){
+                if(spikePosition == SpikePosition.LEFT){
+                    //TODO: Put BlueFarLeft trajectory builder here
+                } else if (spikePosition == SpikePosition.RIGHT) {
+                    //TODO: Put BlueFarRight trajectory builder here
+                }
+                else if(spikePosition == SpikePosition.MIDDLE){
+                    //TODO: Put BlueFarMIDDLE trajectory builder here
+                }
+            }
+        }
+
+        if (sequence == null){
+            return State.FAILURE;
+        }
+        opmode.board.add("TrajectorySequence", sequence);
+        return State.SUCCESS;
+    }
+}
diff --git a/...viorTrees/Actions/SpinOutIntakeMotor.java → ...Trees/Actions/SpikeLocationTelemetry.java b/...viorTrees/Actions/SpinOutIntakeMotor.java → ...Trees/Actions/SpikeLocationTelemetry.java
@@ -3,11 +3,10 @@
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 
-public class SpinOutIntakeMotor extends Node {
-
+public class SpikeLocationTelemetry extends Node {
     @Override
     public State tick(QQOpMode opmode) {
-        opmode.robot.intake.eject();
+        opmode.telemetry.addData("Prop Location: ", opmode.robot.cameraBack.getTeamPropPosition());
         return State.SUCCESS;
     }
 }
diff --git a/...n/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/StopIntakeMotor.java b/...n/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/StopIntakeMotor.java
diff --git a/...ires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetLeftSpikeTrajectory.java b/...ires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetLeftSpikeTrajectory.java
@@ -2,6 +2,7 @@
 
 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;
 
@@ -12,15 +13,16 @@ public class SetLeftSpikeTrajectory extends Node {
     @Override
     public State tick(QQOpMode opMode) {
         TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
-        TrajectorySequence sequence = builder.forward(FORWARD_DISTANCE_INCHES).
-                                            strafeLeft(LEFT_DISTANCE_INCHES).
-                                            strafeRight(LEFT_DISTANCE_INCHES).
+        TrajectorySequence sequence = builder.back(FORWARD_DISTANCE_INCHES).
+                                            //strafeLeft(LEFT_DISTANCE_INCHES).
+                                            //strafeRight(LEFT_DISTANCE_INCHES).
                                             forward(FORWARD_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/...es/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetMiddleSpikeTrajectory.java b/...es/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetMiddleSpikeTrajectory.java
@@ -2,22 +2,26 @@
 
 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 SetMiddleSpikeTrajectory extends Node {
     public static final int FORWARD_DISTANCE_INCHES = 20;
+    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.forward(FORWARD_DISTANCE_INCHES).
-                                            back(FORWARD_DISTANCE_INCHES).
-                                            build();
+        TrajectorySequence sequence = builder
+                .back(FORWARD_DISTANCE_INCHES)
+                .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/...res/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetRightSpikeTrajectory.java b/...res/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetRightSpikeTrajectory.java
@@ -2,6 +2,7 @@
 
 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;
 
@@ -21,6 +22,7 @@ public State tick(QQOpMode opMode) {
             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/Actions/UpdateIntake.java b/...main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/UpdateIntake.java
@@ -8,7 +8,7 @@ public class UpdateIntake extends Node {
     @Override
     public State tick(QQOpMode opmode) {
         if (opmode.gamepad1.right_trigger > TRIGGER_THRESHOLD){
-            opmode.robot.intake.intake();
+            opmode.robot.intake.intake(opmode.gamepad1.right_trigger);
         }
         else if (opmode.gamepad1.x){
             opmode.robot.intake.eject();

diff --git a/.../org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/EnoughTimeForCycle.java b/.../org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/EnoughTimeForCycle.java
@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class EnoughTimeForCycle extends Node{
+        public static final int CYCLE_TIME = 0; //TODO: THIS IS WRONG!!!!
+
+        @Override
+        public State tick(QQOpMode opmode) {
+            if(opmode.time > CYCLE_TIME){
+                return State.SUCCESS;
+            }
+            return State.FAILURE;
+        }
+    }
+
+// need to change
diff --git a/...rg/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfEjectButtonPressed.java b/...rg/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfEjectButtonPressed.java
diff --git a/...g/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfIntakeButtonPressed.java b/...g/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfIntakeButtonPressed.java
diff --git a/.../org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IsBackboardInRange.java b/.../org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IsBackboardInRange.java
@@ -4,7 +4,7 @@
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 
 public class IsBackboardInRange extends Node {
-    public double BACKBOARD_RANGE_CM = 10;
+    public double BACKBOARD_RANGE_CM = 15;
     @Override
     public State tick(QQOpMode opmode) {
         if (opmode.robot.backDistanceSensor.distanceToBackboard()<BACKBOARD_RANGE_CM){

diff --git a/...src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/isRed.java b/...src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/isRed.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions;
+
+public class isRed {
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Not.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Not.java
@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees;
+
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.List;
+
+//Not reverses a value
+public class Not extends Node {
+    Node child;
+
+    public Not(Node a) {
+        this.child = a;
+    }
+
+    @Override
+    public State tick(QQOpMode opmode) {
+        State state = child.tick(opmode);
+
+        if (state == State.FAILURE) {
+            return State.SUCCESS;
+        } else if (state == State.SUCCESS) {
+            return State.FAILURE;
+        }
+
+        return state;
+    }
+}