Merge pull request #40 from ftc16072/autoWork

Auto work

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

@@ -0,0 +1,20 @@
+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 Delay extends QQTimeoutNode {
+    public Delay(double seconds) {
+        super(seconds);
+    }
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (hasTimedOut()){
+            return State.SUCCESS;
+        }
+        return State.RUNNING;
+    }
+}

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

@@ -0,0 +1,28 @@
+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 IntakeArmIn extends QQTimeoutNode {
+    public IntakeArmIn(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING) {
+            return lastStatus;
+        }else {
+            opMode.robot.intakeArm.goToDropPos();
+            if (hasTimedOut()) {
+                opMode.robot.scoreArm.setNotScoring();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

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

@@ -0,0 +1,28 @@
+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 IntakeArmOut extends QQTimeoutNode {
+    public IntakeArmOut(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING) {
+            return lastStatus;
+        }else {
+            opMode.robot.intakeArm.goToIntake();
+            if (hasTimedOut()) {
+                opMode.robot.scoreArm.setNotScoring();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

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

@@ -0,0 +1,28 @@
+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 IntakeClawClose extends QQTimeoutNode {
+    public IntakeClawClose(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING) {
+            return lastStatus;
+        }else {
+            opMode.robot.intakeClaw.close();
+            if (hasTimedOut()) {
+                opMode.robot.scoreArm.setNotScoring();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

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

@@ -0,0 +1,28 @@
+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 IntakeClawOpen extends QQTimeoutNode {
+    public IntakeClawOpen(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING) {
+            return lastStatus;
+        }else {
+            opMode.robot.intakeClaw.open();
+            if (hasTimedOut()) {
+                opMode.robot.scoreArm.setNotScoring();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReadyToIntake.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReadyToIntakeOne.java

@@ -5,8 +5,8 @@
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 
-public class ReadyToIntake extends QQTimeoutNode {
-    public ReadyToIntake(double seconds) {
+public class ReadyToIntakeOne extends QQTimeoutNode {
+    public ReadyToIntakeOne(double seconds) {
         super(seconds);
     }
     State lastStatus = State.RUNNING;
@@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) {
         if (lastStatus != State.RUNNING){
             return lastStatus;
         }else{
-           boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-84,0);
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-82.5,0);
         if (isDoneDriving) {
             lastStatus = State.SUCCESS;
             return State.SUCCESS;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesIn.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 SlidesIn extends QQTimeoutNode {
+    public SlidesIn(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING) {
+            return lastStatus;
+        }else {
+            opMode.robot.intakeSlides.startPosition();
+            if (hasTimedOut()) {
+                lastStatus = State.FAILURE;
+                return State.FAILURE;
+            } else if (opMode.robot.intakeSlides.getIsWithinTolerence()) {
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOutToMiddle.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 SlidesOutToMiddle extends QQTimeoutNode {
+    public SlidesOutToMiddle(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING) {
+            return lastStatus;
+        }else {
+            opMode.robot.intakeSlides.setAutoExtensionPosition();
+            if (hasTimedOut()) {
+                lastStatus = State.FAILURE;
+                return State.FAILURE;
+            } else if (opMode.robot.intakeSlides.getIsWithinTolerence()) {
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

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

@@ -0,0 +1,28 @@
+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 WristToIntake extends QQTimeoutNode {
+    public WristToIntake(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING) {
+            return lastStatus;
+        }else {
+            opMode.robot.intakeClaw.wristIntake();
+            if (hasTimedOut()) {
+                opMode.robot.scoreArm.setNotScoring();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

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

@@ -0,0 +1,28 @@
+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 WristToTransfer extends QQTimeoutNode {
+    public WristToTransfer(double seconds) {
+        super(seconds);
+    }
+    State lastStatus = State.RUNNING;
+
+    @Override
+    public State tick(DebugTree debug, QQOpMode opMode) {
+        if (lastStatus != State.RUNNING) {
+            return lastStatus;
+        }else {
+            opMode.robot.intakeClaw.wristTransfer();
+            if (hasTimedOut()) {
+                opMode.robot.scoreArm.setNotScoring();
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

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

@@ -0,0 +1,39 @@
+
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;
+
+import com.ftcteams.behaviortrees.Failover;
+import com.ftcteams.behaviortrees.Node;
+import com.ftcteams.behaviortrees.Sequence;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeAttempt;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeClawClose;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SlidesIn;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WristToIntake;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WristToTransfer;
+
+
+public class DoubleIntake {
+    public static Node root(){
+        final double INTAKE_TIMEOUT_SECONDS = 1.5;
+        final double MOVEMENT_TIMEOUT_SECONDS = 5;
+        return new Sequence(
+                new WristToIntake(0.5),
+                new IntakeClawClose(0.5),
+                new WristToTransfer(0.5),
+                new SlidesIn(MOVEMENT_TIMEOUT_SECONDS),
+                new Failover(
+                    new IntakeAttempt(INTAKE_TIMEOUT_SECONDS),
+                    new Sequence(
+                            new ReadyToIntakeOne(MOVEMENT_TIMEOUT_SECONDS),
+                            new IntakeAttempt(INTAKE_TIMEOUT_SECONDS))));
+    }
+}
+
+/* TREE
+?
+|  [IntakeAttempt]
+|  ->
+|  |  [ReadyForIntake]
+|  |  [IntakeAttempt]
+ */

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

@@ -5,7 +5,7 @@
 import com.ftcteams.behaviortrees.Node;
 import com.ftcteams.behaviortrees.Sequence;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeAttempt;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntake;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne;
 
 
 public class Intake {
@@ -15,7 +15,7 @@ public static Node root(){
         return new Failover(
                 new IntakeAttempt(INTAKE_TIMEOUT_SECONDS),
                 new Sequence(
-                        new ReadyToIntake(MOVEMENT_TIMEOUT_SECONDS),
+                        new ReadyToIntakeOne(MOVEMENT_TIMEOUT_SECONDS),
                         new IntakeAttempt(INTAKE_TIMEOUT_SECONDS)));
     }
 }

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

@@ -11,9 +11,8 @@
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.GetReadyToPushSamples;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntake;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForScore;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park;
 
 
@@ -31,7 +30,7 @@ public static Node root(){
                         PushSamples.root(),
                         new Parallel(2,
                                 new ArmToIntake(TIMEOUT_SECONDS),
-                                new ReadyToIntake(TIMEOUT_SECONDS))),
+                                new ReadyToIntakeOne(TIMEOUT_SECONDS))),
                 new Park(TIMEOUT_SECONDS));
     }
 }