Driver enhancements

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ArmToIntake.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 ArmToIntake extends QQTimeoutNode {
+    public ArmToIntake(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.scoreArm.goToIntake();
+            if (hasTimedOut()) {
+                lastStatus = State.FAILURE;
+                return State.FAILURE;
+            } else if (opMode.robot.scoreArm.isLimitSwitchPressed()) {
+                lastStatus = State.SUCCESS;
+                return State.SUCCESS;
+            }
+            return State.RUNNING;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/BehindChamber.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 BehindChamber extends QQTimeoutNode {
+    public BehindChamber(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(17,-58,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/DriveToChamber.java

@@ -34,3 +34,4 @@ public State tick(DebugTree debug, QQOpMode opMode) {
         }
     }
 }
+

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

@@ -0,0 +1,34 @@
+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 DriveToFirstSample extends QQTimeoutNode {
+    public DriveToFirstSample(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(42,-88,90);
+            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/DriveToSamples.java

@@ -0,0 +1,34 @@
+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 DriveToSamples extends QQTimeoutNode {
+    public DriveToSamples(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(42,-84,90);
+            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/DriveToSecondSample.java

@@ -0,0 +1,34 @@
+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 DriveToSecondSample extends QQTimeoutNode {
+    public DriveToSecondSample(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(42,-94,90);
+            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/GetReadyToPushSamples.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 GetReadyToPushSamples extends QQTimeoutNode {
+    public GetReadyToPushSamples(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(17,-83,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/IntakeAttempt.java

@@ -0,0 +1,37 @@
+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 IntakeAttempt extends QQTimeoutNode {
+    public static final double INTAKE_SPEED = -0.3;
+    State lastStatus = State.RUNNING;
+
+    public IntakeAttempt(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(INTAKE_SPEED,0,0);
+            if (opMode.robot.scoringClaw.isClawClosed()){
+                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

@@ -9,17 +9,25 @@ public class Park extends QQTimeoutNode {
     public Park(double seconds) {
         super(seconds);
     }
+    State lastStatus = State.RUNNING;
 
     @Override
     public State tick(DebugTree debug, QQOpMode opMode) {
-        boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-109.5,0);
+
         opMode.telemetry.addData("location",opMode.robot.otos.getOtosPosition());
+        if (lastStatus != State.RUNNING){
+            return lastStatus;
+        }else{
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(12,-90.5,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/PushFirstSample.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 PushFirstSample extends QQTimeoutNode {
+    public PushFirstSample(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(14,-88,90);
+        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

@@ -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 PushSecondSample extends QQTimeoutNode {
+    public PushSecondSample(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(14,-88,90);
+        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/ReadyToIntake.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 ReadyToIntake extends QQTimeoutNode {
+    public ReadyToIntake(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(14,-84,0);
+        if (isDoneDriving) {
+            lastStatus = State.SUCCESS;
+            return State.SUCCESS;
+        }
+        if (hasTimedOut()) {
+            lastStatus = State.FAILURE;
+            return State.FAILURE;
+        }
+        return State.RUNNING;
+        }
+    }
+}