Spike auto now working

ftc16072 · Feb 19, 2024 · 234b2b3 · 234b2b3
1 parent 8a2f1e0
commit 234b2b3
Show file tree

Hide file tree

Showing 15 changed files with 106 additions and 138 deletions.
diff --git a/.../java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReleaseAutoPixel.java b/.../java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReleaseAutoPixel.java
@@ -1,10 +1,6 @@
 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 {

diff --git a/...stinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/FollowTrajectory.java b/...stinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/FollowTrajectory.java
@@ -6,23 +6,30 @@
 
 public class FollowTrajectory extends Node{
     boolean started;
+    TrajectorySequence trajectorySequence;
+    String trajectorySequenceString;
 
     @Override
     public State tick(QQOpMode opmode) {
         if(!started){
-            TrajectorySequence trajectorySequence = opmode.board.get(TrajectorySequence.class, "TrajectorySequence");
+            trajectorySequence = opmode.board.get(TrajectorySequence.class, "TrajectorySequence");
+            trajectorySequenceString = opmode.board.get(String.class, "TrajectorySequenceString");
             if(trajectorySequence == null){
                 return State.FAILURE;
             }
             opmode.robot.nav.startNewTrajectorySequence(trajectorySequence);
             started = true;
         }
         else{
+
             opmode.robot.nav.update();
             if (!opmode.robot.nav.isBusy()){
                 return State.SUCCESS;
             }
         }
+        opmode.telemetry.addData("Current trajectory:",  trajectorySequence);
+
+
         return State.RUNNING;
     }
 }
diff --git a/.../Trajectories/SetLeftSpikeTrajectory.java → ...ikeTrajectory/SetLeftSpikeTrajectory.java b/.../Trajectories/SetLeftSpikeTrajectory.java → ...ikeTrajectory/SetLeftSpikeTrajectory.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories;
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory;
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
@@ -7,21 +7,19 @@
 import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequenceBuilder;
 
 public class SetLeftSpikeTrajectory extends Node {
-    public static final int FORWARD_DISTANCE_INCHES = 20;
-    public static final int LEFT_DISTANCE_INCHES = 5;
+    public static final int FORWARD_DISTANCE_INCHES = 23;
+
 
     @Override
     public State tick(QQOpMode opMode) {
         TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
         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("TrajectorySequenceString", "Left Spike Trajectory 1");
         opMode.board.add("SpikePosition", SpikePosition.LEFT);
         return State.SUCCESS;
     }

diff --git a/...2/BehaviorTrees/Actions/Trajectories/LeftSpikeTrajectory/SetLeftSpikeTrajectoryPart2.java b/...2/BehaviorTrees/Actions/Trajectories/LeftSpikeTrajectory/SetLeftSpikeTrajectoryPart2.java
@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.LeftSpikeTrajectory;
+
+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 SetLeftSpikeTrajectoryPart2 extends Node {
+
+    public static final int LEFT_DISTANCE_INCHES = 8;
+
+    @Override
+    public State tick(QQOpMode opMode) {
+        TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = builder
+                //right because robot is facing backward
+                .strafeRight(LEFT_DISTANCE_INCHES)
+                .build();
+        if (sequence == null){
+            return State.FAILURE;
+        }
+        opMode.board.add("TrajectorySequence", sequence);
+        opMode.board.add("TrajectorySequenceString", "Left Spike Trajectory 2");
+
+        opMode.board.add("SpikePosition", SpikePosition.LEFT);
+        return State.SUCCESS;
+    }
+}
diff --git a/...Trajectories/SetRightSpikeTrajectory.java → ...keTrajectory/SetRightSpikeTrajectory.java b/...Trajectories/SetRightSpikeTrajectory.java → ...keTrajectory/SetRightSpikeTrajectory.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories;
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory;
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
@@ -8,15 +8,12 @@
 
 public class SetRightSpikeTrajectory extends Node {
     public static final int FORWARD_DISTANCE_INCHES = 20;
-    public static final int RIGHT_DISTANCE_INCHES = 5;
+
 
     @Override
     public State tick(QQOpMode opMode) {
         TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
-        TrajectorySequence sequence = builder.forward(FORWARD_DISTANCE_INCHES).
-                                            strafeRight(RIGHT_DISTANCE_INCHES).
-                                            strafeLeft(RIGHT_DISTANCE_INCHES).
-                                            back(FORWARD_DISTANCE_INCHES).
+        TrajectorySequence sequence = builder.back(FORWARD_DISTANCE_INCHES).
                                             build();
         if (sequence == null){
             return State.FAILURE;

diff --git a/...BehaviorTrees/Actions/Trajectories/RightSpikeTrajectory/SetRightSpikeTrajectoryPart2.java b/...BehaviorTrees/Actions/Trajectories/RightSpikeTrajectory/SetRightSpikeTrajectoryPart2.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory;
+
+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 SetRightSpikeTrajectoryPart2 extends Node {
+
+    public static final int RIGHT_DISTANCE_INCHES = 20;
+
+    @Override
+    public State tick(QQOpMode opMode) {
+        TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate());
+        TrajectorySequence sequence = builder.strafeLeft(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/...es/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetMiddleSpikeTrajectory.java b/...es/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Trajectories/SetMiddleSpikeTrajectory.java
@@ -7,14 +7,13 @@
 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 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)
                 .back(FORWARD_DISTANCE_INCHES).
                 build();
         if (sequence == null){

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
@@ -1,15 +1,14 @@
 package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;
 
-import org.checkerframework.checker.units.qual.A;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpikeLocationTelemetry;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePurplePixel;
 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.SpikeLocationTelemetry;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.FollowTrajectory;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.SetLeftSpikeTrajectory;
+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.SetRightSpikeTrajectory;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Trajectories.RightSpikeTrajectory.SetRightSpikeTrajectory;
 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;
@@ -37,17 +36,25 @@ public static Node root(){
                 new Failover(
                         new Sequence(
                                 new IsLeftSpike(),
-                                new SetLeftSpikeTrajectory()
+                                new SetLeftSpikeTrajectory(),
+                                new FollowTrajectory(),
+                                new SetLeftSpikeTrajectoryPart2(),
+                                new FollowTrajectory()
+
                         ),
                         new Sequence(
                                 new IsRightSpike(),
-                                new SetRightSpikeTrajectory()
+                                new SetRightSpikeTrajectory(),
+                                new FollowTrajectory(),
+                                new SetRightSpikeTrajectoryPart2(),
+                                new FollowTrajectory()
                         ),
-                        new SetMiddleSpikeTrajectory()
+                        new Sequence(
+                            new SetMiddleSpikeTrajectory(),
+                            new FollowTrajectory()
+                        )
                 ),
 
-                //follows trajectory set based on spike location
-                new FollowTrajectory(),
                 //Move servo to release purple pixel
                 new ReleaseAutoPixel(),
                 //set trajectory to backboard from tape location

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/PixelServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/PixelServo.java
@@ -12,7 +12,8 @@
 @Config
 
 public class PixelServo implements Mechanism{
-    public static final double RELEASE_POSITION= .7; //Tuned
+    public static final double RELEASE_POSITION= .4; //Tuned
+
     Servo pixelServo;
     @Override
     public void init(HardwareMap hwMap) {
@@ -26,7 +27,7 @@ public void releasePixel(){
 
     @Override
     public List<QQtest> getTests() {
-        return Collections.singletonList(new TestServo("Pixel Servo", .8, 0.6
+        return Collections.singletonList(new TestServo("Pixel Servo", RELEASE_POSITION, 0.67
                 , pixelServo));
     }
 }   
diff --git a/...Code/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PlacingOnTapeAuto.java b/...Code/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PlacingOnTapeAuto.java
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpikeAutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpikeAutoBase.java
@@ -13,10 +13,15 @@ abstract public class SpikeAutoBase extends VisionBase{
     @Override
     public void init(){
         super.init();
-        board.add("proploocation", robot.cameraBack.getTeamPropPosition());
         robot.makeDriveOnly();
-
-        telemetry.addData("prop Location", robot.cameraBack.getTeamPropPosition());
+    }
+    @Override
+    public void init_loop(){
+        telemetry.addData("PropLocation: ", robot.cameraBack.getTeamPropPosition());
+    }
+    @Override
+    public void start(){
+        board.add("proplocation", robot.cameraBack.getTeamPropPosition());
     }
     @Override
     public void loop() {

diff --git a/...ftc16072/OpModes/BlueClose_SpikeAuto.java → ...pikeTrajectories/BlueClose_SpikeAuto.java b/...ftc16072/OpModes/BlueClose_SpikeAuto.java → ...pikeTrajectories/BlueClose_SpikeAuto.java
@@ -1,29 +1,20 @@
-package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
+package org.firstinspires.ftc.teamcode.ftc16072.OpModes.SpikeTrajectories;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.SpikeAutoTree;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.TeleopTree;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.SpikeAutoBase;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.Alliance;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.StartPosition;
 @Autonomous
 public class BlueClose_SpikeAuto extends SpikeAutoBase {
 
-
-
     @Override
     public void init(){
         super.init();
         board.add("Alliance", Alliance.BLUE);
         board.add("StartPosition", StartPosition.CLOSE);
-
-
-    }
-
-    @Override
-    public void init_loop() {
-        super.init_loop();
-        telemetry.addData("Prop Location: ", robot.cameraBack.getTeamPropPosition());
     }
 }
diff --git a/...e/ftc16072/OpModes/BlueFar_SpikeAuto.java → .../SpikeTrajectories/BlueFar_SpikeAuto.java b/...e/ftc16072/OpModes/BlueFar_SpikeAuto.java → .../SpikeTrajectories/BlueFar_SpikeAuto.java
@@ -1,7 +1,8 @@
-package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
+package org.firstinspires.ftc.teamcode.ftc16072.OpModes.SpikeTrajectories;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.SpikeAutoBase;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.Alliance;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.StartPosition;