Merge pull request #57 from ftc16072/TestingTeleOpTree

Testing tele op tree

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

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class AddTelemetry extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.telemetry.addData("behavior tree is here: ", 11);
+        return State.SUCCESS;
+    }
+}

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

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class MoveLiftToIntakePosition extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.robot.lift.goToIntake();
+        return  State.SUCCESS;
+    }
+}

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

@@ -11,6 +11,9 @@ public State tick(QQOpMode opmode) {
             opmode.robot.placement.leftServoEject();
             opmode.robot.placement.rightServoEject();
 
+        } else {
+            opmode.robot.placement.leftServoGrab();
+            opmode.robot.placement.rightServoGrab();
         }
         return State.RUNNING;
     }

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

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class RumbleGamepad extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.gamepad1.rumbleBlips(2);
+        return State.SUCCESS;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/Has1or2Pixels.java

@@ -7,11 +7,13 @@ public class Has1or2Pixels extends Node {
     @Override
     public State tick(QQOpMode opmode) {
 
-        if (opmode.robot.holdingCell.getNumPixels()>0){
+        opmode.telemetry.addData("num pixels", opmode.robot.holdingCell.getNumPixels());
+        if (opmode.robot.holdingCell.getNumPixels()>=1){
             return State.SUCCESS;
         }
         return  State.FAILURE;
 
 
+
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfLiftAtBottom.java

@@ -0,0 +1,17 @@
+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 IfLiftAtBottom extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.telemetry.addData("limit switch", opmode.robot.holdingCell.isLimitSwitchDetected());
+        if(opmode.robot.holdingCell.isLimitSwitchDetected()){
+            return State.SUCCESS;
+
+
+        }
+        return State.FAILURE;
+    }
+}

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

@@ -1,17 +1,34 @@
 package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;
 
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.AddTelemetry;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ClampOnPixel;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveFieldRelative;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeFastDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeNormalDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeSlowDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveArmAndLift;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ResetGyro;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToIntakePosition;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToPixelGrabPosition;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.RumbleGamepad;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLiftPosition;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinInIntakeMotor;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinOutIntakeMotor;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.StopIntakeMotor;
+
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateArmAndLift;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateClimber;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateGrabber;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateIntake;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreNotSlidesExtended;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreSlidesExtended;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1Pixel;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1or2Pixels;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has2Pixels;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.HasLessThan2Pixels;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfEjectButtonPressed;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfIntakeButtonPressed;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLeftTriggerPressed;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftAtBottom;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftToPixelGrabPosButtonPressed;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsControllerDriving;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Parallel;
@@ -75,28 +92,125 @@
  */
 public class TeleopTree {
     public static Node root(){
-        return new Parallel(6,
-                new DriveFieldRelative(),
-                new MoveArmAndLift(),
-                new UpdateArmAndLift(),
-                new UpdateClimber(),
-                new ResetGyro(),
-                new UpdateIntake(),
-                new UpdateGrabber(),
+        return new Parallel(5,
+                /*
                 new Sequence(
-                    new Failover(
-                            new AreNotSlidesExtended(),
-                            new MakeSlowDrive()
-                    ),
-                    new Failover(
-                            new AreSlidesExtended(),
+
+                    new IsEndgame(),
+
+                    new Parallel(2,
                             new Sequence(
-                                    new IfLeftTriggerPressed(),
-                                    new MakeSlowDrive()
+                                    new Failover(
+                                            new IsLastThreeSeconds(),
+                                            new Sequence(
+                                                new IsDroneLocation(),
+                                                new IsDroneButtonPressed()
+                                            )
+                                    ),
+                                    new HaveNotLaunchedDrone(),
+                                    new LaunchDrone()
                             ),
-                            new MakeNormalDrive()
+
+                            new Sequence(
+                                    new IsClimbLocation(),
+                                    new IsClimbButtonPressed(),
+                                    new Climb()
+                            )
                     )
+                ),
+
+                 */
+                new DriveFieldRelative(),
+                new UpdateArmAndLift(),
+                new Failover(
+                        new Sequence(
+                                new HasLessThan2Pixels(),
+                                new IfIntakeButtonPressed(),
+                                new SpinInIntakeMotor(), // currently the lift moves to intake position when the intake is turned on
+                                new MoveLiftToIntakePosition()  // might want to consider adding an additional conditional if this is consistently being mispressed
+                        ),
+                        new Sequence( // moving lift down to grab pixels when there is 2 pixels
+                                new Has2Pixels(), // having more than 2 pixels is impossible
+
+                                new MoveLiftToPixelGrabPosition(),
+                                new ClampOnPixel(),
+                                new IfEjectButtonPressed(), // might want to consider removing this and automatically ejecting
+                                new SpinOutIntakeMotor() // X not using eject
+
+
+                        ),
+                        new Sequence( // moving lift down to grab pixels when there is only 1 pixel
+                                new Has1Pixel(),
+
+                                new IfLiftToPixelGrabPosButtonPressed(),
+                                new MoveLiftToPixelGrabPosition(),
+                                new ClampOnPixel(),
+                                new IfEjectButtonPressed(),
+                                new SpinOutIntakeMotor() // X not using eject
+
+                        ),
+                        new StopIntakeMotor()
+                ),
+                new Failover(
+                        new Sequence(
+                                new SetLiftPosition()
+                        )
+                ),
+                new Parallel(2,
+                        new Failover(
+                                new Sequence(
+                                        new IfLiftAtBottom(),
+                                        new Has1or2Pixels(),
+                                        new AddTelemetry(),
+                                        new Sequence(
+                                                new MoveArmAndLift(),
+                                                new PlacePixels()
+                                        )
+
+
+
+
+                                ),
+                                new Failover(
+                                        new IfLiftAtBottom(),
+                                        new Sequence(
+                                                new MoveArmAndLift(),
+                                                new PlacePixels()
+                                        )
+
+
+                                )
+
+
+
+
+                        ),
+                        new Failover(
+                                new AreSlidesExtended(),
+                                new IsControllerDriving(),
+                                new MakeSlowDrive()
+                        ),
+                        new Sequence(
+                                new AreNotSlidesExtended(),
+                                new Sequence(
+                                        new IsControllerDriving(),
+                                        new Failover(
+                                                new MakeNormalDrive(),
+                                                new Sequence(
+                                                        new IfLeftTriggerPressed(),
+                                                        new MakeFastDrive()
+
+                                                )
+
+
+                                        )
+
+                                )
+                        )
+
                 )
         );
     }
+
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/HoldingCell.java

@@ -18,28 +18,28 @@ public class  HoldingCell implements Mechanism{
     private ColorRangeSensor leftPixelCounter;
     private ColorRangeSensor rightPixelCounter;
 
-    private DigitalChannel leftConeDetector;
+    private DigitalChannel leftLimitSwitch;
 
-    private DigitalChannel rightConeDetector;
+    private DigitalChannel rightLimitSwitch;
 
     @Override
     public void init(HardwareMap hwMap) {
         leftPixelCounter = hwMap.get(ColorRangeSensor.class, "left_pixel_counter");
         rightPixelCounter = hwMap.get(ColorRangeSensor.class, "right_pixel_counter");
-        leftConeDetector = hwMap.get(DigitalChannel.class,"left_cone_detector");
-        rightConeDetector = hwMap.get(DigitalChannel.class,"right_cone_detector");
+        leftLimitSwitch = hwMap.get(DigitalChannel.class,"left_limit_switch");
+        rightLimitSwitch = hwMap.get(DigitalChannel.class,"right_limit_switch");
     }
 
     public List<QQtest> getTests() {
         return Arrays.asList(
                 new TestColorRangeSensor("left pixel detector", leftPixelCounter),
                 new TestColorRangeSensor("right pixel detector", rightPixelCounter),
-                new TestSwitch("left_cone_detector",leftConeDetector),
-                new TestSwitch("right_cone_detector", rightConeDetector));
+                new TestSwitch("left_limit_switch",leftLimitSwitch),
+                new TestSwitch("right_limit_switch", rightLimitSwitch));
     }
 
-    public boolean isConeDetected(){
-        return leftConeDetector.getState() || rightConeDetector.getState();
+    public boolean isLimitSwitchDetected(){
+        return !leftLimitSwitch.getState() || !rightLimitSwitch.getState();
     }
     /**
      * Checks how many pixels are in the holding cell.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Lift.java

@@ -45,7 +45,8 @@ public enum LiftPositions{
     private static final int LOW_POSITION = 700;
     private static final int MIDDLE_POSITION = 1600;
     private static final int TOP_POSITION = 2400;
-    private static final int FLOOR_POSITION =100  ;
+    private static final int INTAKE_POSITION = 250;
+    private static final int FLOOR_POSITION =0  ;
     private static final int PIXEL_HEIGHT = 271;
     private final int MANUAL_CHANGE = 50;
     private DcMotorEx rightLiftMotor;
@@ -136,6 +137,9 @@ public double getDesiredPosition(){
     public void goToPixelGrab(){
         setDesiredPosition(PIXEL_GRAB_POSITION);
     }
+    public void goToIntake(){
+        setDesiredPosition(INTAKE_POSITION);
+    }
 
     public void goToLow(){
         setDesiredPosition(LOW_POSITION);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/AprilTag.java

@@ -16,8 +16,7 @@ public void init() {
         super.init();
     }
     public void init_loop(){
-        telemetry.addLine("Front Camera");
-        printAprilTags(robot.cameraFront.getAprilTagDetections());
+
         telemetry.addLine("Back Camera");
         printAprilTags(robot.cameraBack.getAprilTagDetections());
         telemetry.addLine("------------");

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java

@@ -27,7 +27,7 @@ public class Robot {
     public Intake intake;
 
     public HoldingCell holdingCell;
-    public Camera cameraFront;
+    //public Camera cameraFront;
     public Camera cameraBack;
     public Lift lift;
     public Arm arm;
@@ -51,15 +51,15 @@ public Robot() {
 
 
         mechanisms = Arrays.asList(
-                //mecanumDrive,
+                mecanumDrive,
                 controlHub,
-                //intake,
-                cameraBack
-                //holdingCell,
-                //lift,
-                //arm,
-                //placement,
-                //climber
+                intake,
+                cameraBack,
+                holdingCell,
+                lift,
+                arm,
+                placement,
+                climber
         );
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java

@@ -18,8 +18,8 @@ public class TeamPropDetector implements VisionProcessor {
     //makes "detection zones" for each tape zone
     TeamPropLocation location = TeamPropLocation.NOT_DETECTED;
     public Rect leftTapeDetectionZone = new Rect(100,100,75,75);
-    public Rect middleTapeDetectionZone = new Rect(400,130,75,75);
-    public Rect rightTapeDetectionZone = new Rect(800,100,75,75);
+    public Rect middleTapeDetectionZone = new Rect(250,130,75,75);
+    public Rect rightTapeDetectionZone = new Rect(400,100,75,75);
 
     //submats are smaller portions of the frame that you can get values from
     Mat submat = new Mat();