Navigation with limelight

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToFirstSample.java

@@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) {
         if (lastStatus != State.RUNNING){
             return lastStatus;
         }else{
-            boolean isDoneDriving = opMode.nav.driveToPositionIN(55,-102,0);
+            boolean isDoneDriving = opMode.nav.driveToPositionIN(37,80,0);
             if (isDoneDriving) {
                 lastStatus = State.SUCCESS;
                 return State.SUCCESS;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToIntakePosition.java

@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import com.ftcteams.behaviortrees.DebugTree;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class DriveToIntakePosition extends QQTimeoutNode {
+    public DriveToIntakePosition(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+
+        opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition());
+        if (lastStatus != State.RUNNING){
+            return lastStatus;
+        }else{
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(52,47,0,6);
+        if (isDoneDriving) {
+            lastStatus = State.SUCCESS;
+            return State.SUCCESS;
+        }
+        if (hasTimedOut()) {
+            lastStatus = State.FAILURE;
+            return State.FAILURE;
+        }
+        return State.RUNNING;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import com.ftcteams.behaviortrees.DebugTree;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class DriveToScorePosition extends QQTimeoutNode {
+    double xPosition;
+    public DriveToScorePosition(double seconds, double xPosition) {
+        super(seconds);
+        this.xPosition = xPosition;
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+
+        opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition());
+        if (lastStatus != State.RUNNING){
+            return lastStatus;
+        }else{
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(xPosition,50,0,4);
+        if (isDoneDriving) {
+            lastStatus = State.SUCCESS;
+            return State.SUCCESS;
+        }
+        if (hasTimedOut()) {
+            lastStatus = State.FAILURE;
+            return State.FAILURE;
+        }
+        return State.RUNNING;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToChamber.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/FirstScore.java

@@ -5,11 +5,11 @@
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 
-public class DriveToChamber extends QQTimeoutNode {
-    public static final double FORWARD_SPEED = 0.3;
+public class FirstScore extends QQTimeoutNode {
+    public static final double FORWARD_SPEED = 0.4;
     State lastStatus = State.RUNNING;
 
-    public DriveToChamber(double seconds) {
+    public FirstScore(double seconds) {
         super(seconds);
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/GetReadyToPushSamples.java

@@ -17,7 +17,7 @@ public State tick(DebugTree debug, QQOpMode opMode) {
         if (lastStatus != State.RUNNING){
             return lastStatus;
         }else{
-            boolean isDoneDriving = opMode.nav.driveToPositionIN(17,-91,0);
+            boolean isDoneDriving = opMode.nav.driveToPositionIN(37,50,0);
         if (isDoneDriving) {
             lastStatus = State.SUCCESS;
             return State.SUCCESS;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeAttempt.java

@@ -26,8 +26,7 @@ public State tick(DebugTree debug, QQOpMode opMode) {
             opMode.robot.mecanumDrive.move(INTAKE_SPEED,0,0);
             if (opMode.robot.scoringClaw.isClawClosed()){
                 opMode.robot.mecanumDrive.stop();
-                opMode.robot.intakeClaw.open();
-                opMode.robot.intakeArm.goToAutoDropPos();
+                opMode.robot.intakeClaw.open();;
                 lastStatus = State.SUCCESS;
                 return State.SUCCESS;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveForwardForTime.java

@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import com.ftcteams.behaviortrees.DebugTree;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class MoveForwardForTime extends QQTimeoutNode {
+    double speed;
+    State lastStatus = State.RUNNING;
+    public MoveForwardForTime(double seconds, double speed) {
+        super(seconds);
+        this.speed = speed;
+    }
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING){
+            return lastStatus;}
+        else {
+            opMode.robot.mecanumDrive.move(-speed,0,0);
+            if (hasTimedOut()){
+                opMode.robot.mecanumDrive.stop();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+            }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveRightForTime.java

@@ -0,0 +1,31 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import com.ftcteams.behaviortrees.DebugTree;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class MoveRightForTime extends QQTimeoutNode {
+    double speed;
+    State lastStatus = State.RUNNING;
+    public MoveRightForTime(double seconds, double speed) {
+        super(seconds);
+        this.speed = speed;
+    }
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING){
+            return lastStatus;}
+        else {
+            opMode.robot.mecanumDrive.move(0,speed,0);
+            if (hasTimedOut()){
+                opMode.robot.mecanumDrive.stop();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+
+            }
+            return State.RUNNING;
+            }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Park.java

@@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) {
         if (lastStatus != State.RUNNING){
             return lastStatus;
         }else{
-           boolean isDoneDriving = opMode.nav.driveToPositionIN(12,-90.5,0);
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(45,37,0);
         if (isDoneDriving) {
             lastStatus = State.SUCCESS;
             return State.SUCCESS;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushFirstSample.java

@@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) {
         if (lastStatus != State.RUNNING){
             return lastStatus;
         }else{
-           boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-88,90);
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(50,80,0);
         if (isDoneDriving) {
             lastStatus = State.SUCCESS;
             return State.SUCCESS;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSamplesIn.java

@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import com.ftcteams.behaviortrees.DebugTree;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class PushSamplesIn extends QQTimeoutNode {
+    public PushSamplesIn(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition());
+        if (lastStatus != State.RUNNING){
+            return lastStatus;
+        }else{
+            boolean isDoneDriving = opMode.nav.driveToPositionIN(opMode.robot.otos.getOtosPosition().x, 47,0);
+        if (isDoneDriving) {
+            lastStatus = State.SUCCESS;
+            return State.SUCCESS;
+        }if (hasTimedOut()) {
+            lastStatus = State.FAILURE;
+            return State.FAILURE;
+        }
+        return State.RUNNING;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushSecondSample.java

@@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) {
         if (lastStatus != State.RUNNING){
             return lastStatus;
         }else{
-           boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-88,90);
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(61.5,85,0);
         if (isDoneDriving) {
             lastStatus = State.SUCCESS;
             return State.SUCCESS;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PushThirdSample.java

@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import com.ftcteams.behaviortrees.DebugTree;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class PushThirdSample extends QQTimeoutNode {
+    public PushThirdSample(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+
+        opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition());
+        if (lastStatus != State.RUNNING){
+            return lastStatus;
+        }else{
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(59,82,0);
+        if (isDoneDriving) {
+            lastStatus = State.SUCCESS;
+            return State.SUCCESS;
+        }
+        if (hasTimedOut()) {
+            lastStatus = State.FAILURE;
+            return State.FAILURE;
+        }
+        return State.RUNNING;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/StandardScore.java

@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import com.ftcteams.behaviortrees.DebugTree;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class StandardScore extends QQTimeoutNode {
+    public static final double FORWARD_SPEED = 0.3;
+    public static final double RIGHT_SPEED = 0.1;
+    State lastStatus = State.RUNNING;
+
+    public StandardScore(double seconds) {
+        super(seconds);
+    }
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING){
+            return lastStatus;
+        }else{
+            if(hasTimedOut()){
+                opMode.robot.mecanumDrive.stop();
+                lastStatus = State.FAILURE;
+                return State.FAILURE;
+            }
+            opMode.robot.mecanumDrive.move(FORWARD_SPEED,RIGHT_SPEED,0);
+            if (opMode.robot.scoringClaw.isClawOpen()){
+                opMode.robot.mecanumDrive.stop();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+
+            }
+            return State.RUNNING;
+        }
+    }
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java

@@ -0,0 +1,47 @@
+/* Tree TwoSpecimenTree for 16072 generated by http://behaviortrees.ftcteams.com */
+        package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;
+
+import com.ftcteams.behaviortrees.Node;
+import com.ftcteams.behaviortrees.Parallel;
+import com.ftcteams.behaviortrees.Sequence;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToScore;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToIntakePosition;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToScorePosition;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveForwardForTime;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.StandardScore;
+
+
+public class Cycle {
+    public static final int TIMEOUT_SECONDS = 10;
+
+
+
+    public static Node root(double xScorePosition){
+        return new Sequence(
+                Intake.root(),
+                new MoveForwardForTime(.2,-.25),
+                new Parallel(2,
+                        new DriveToScorePosition(TIMEOUT_SECONDS, xScorePosition),
+                        new ArmToScore(TIMEOUT_SECONDS)),
+                new StandardScore(TIMEOUT_SECONDS),
+                new MoveForwardForTime(0.25,1),
+                new Parallel(2,
+                        new DriveToIntakePosition(TIMEOUT_SECONDS),
+                        new ArmToIntake(TIMEOUT_SECONDS)));
+
+    }
+}
+
+/* TREE
+?
+|  ->
+|  |  [FisrtScore]
+|  |  [DriveToIntakePosition]
+|  |  [Intake]
+|  |  [DrivetoScorePosition]
+|  |  [StandardScore]
+|  |  [Park]
+|  [Park]
+ */