1/10 raleigh qual

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

@@ -0,0 +1,13 @@
+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 DroneRelease extends Node {
+
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.robot.drone.releaseDrone();
+        return State.SUCCESS;
+    }
+}

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

@@ -0,0 +1,20 @@
+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 PlacePixels2 extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        //TODO: change later so we can release pixels individually
+        if(opmode.gamepad1.x){
+            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/UpdateArmAndLift.java

@@ -8,6 +8,7 @@ public class UpdateArmAndLift extends Node {
     @Override
     public State tick(QQOpMode opmode) {
         armControl.updateArm(opmode.robot);
+
         opmode.robot.lift.update(opmode.telemetry);
 
         return State.RUNNING;

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

@@ -4,7 +4,7 @@
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 
 public class IsBackboardInRange extends Node {
-    public double BACKBOARD_RANGE_CM = 8;
+    public double BACKBOARD_RANGE_CM = 10;
     @Override
     public State tick(QQOpMode opmode) {
         if (opmode.robot.backDistanceSensor.distanceToBackboard()<BACKBOARD_RANGE_CM){

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

@@ -0,0 +1,15 @@
+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 ifDroneReleaseButtonPressed extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        if (opmode.gamepad2.right_trigger>0.25){
+            return State.SUCCESS;
+        }
+
+        return State.FAILURE;
+    }
+}

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

@@ -3,20 +3,23 @@
 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.DroneRelease;
 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.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.PlacePixels2;
 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.Conditions.AreNotSlidesExtended;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreSlidesExtended;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1Pixel;
@@ -28,7 +31,9 @@
 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.IsBackboardInRange;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsControllerDriving;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.ifDroneReleaseButtonPressed;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Parallel;
@@ -120,20 +125,34 @@ public static Node root(){
                 ),
 
                  */
+                new Sequence(
+                        new ifDroneReleaseButtonPressed(),
+                        new DroneRelease()
+
+
+                ),
+
+                new UpdateClimber(),
                 new DriveFieldRelative(),
                 new UpdateArmAndLift(),
+                new Sequence( //distance sensor nodes
+                        new AreSlidesExtended(),
+                        new IsBackboardInRange(),
+                        new RumbleGamepad()
+                ),
+
                 new Failover(
                         new Sequence(
-                                new HasLessThan2Pixels(),
+                            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
 
@@ -162,7 +181,8 @@ public static Node root(){
                                         new IfLiftAtBottom(),
                                         new Has1or2Pixels(),
                                         new AddTelemetry(),
-                                        new Sequence(
+                                        new Parallel(2,
+
                                                 new MoveArmAndLift(),
                                                 new PlacePixels()
                                         )
@@ -173,7 +193,7 @@ public static Node root(){
                                 ),
                                 new Failover(
                                         new IfLiftAtBottom(),
-                                        new Sequence(
+                                        new Parallel(2,
                                                 new MoveArmAndLift(),
                                                 new PlacePixels()
                                         )

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

@@ -10,12 +10,16 @@
 import java.util.List;
 
 public class Drone implements Mechanism{
+    public static final double DRONE_RELEASE = 0.4;
     private Servo droneServo;
     @Override
     public void init(HardwareMap hwMap) {
         droneServo = hwMap.get(Servo.class,"drone_servo");
 
     }
+    public void releaseDrone(){
+        droneServo.setPosition(DRONE_RELEASE);
+    }
 
     @Override
     public List<QQtest> getTests() {

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

@@ -34,7 +34,7 @@ public enum LiftPositions{
         TOP_POSITION
 
     }
-    public static final int ARE_SLIDES_EXTENDED_BOUNDARY = 600;
+    public static final int ARE_SLIDES_EXTENDED_BOUNDARY = 800;
 
     private LiftPositions manipulatorPosition;
     private static final int LIFT_POSITION_SAFETY_BOTTOM = -50;
@@ -45,7 +45,7 @@ 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 INTAKE_POSITION = 250;
+    private static final int INTAKE_POSITION = 100;
     private static final int FLOOR_POSITION =0  ;
     private static final int PIXEL_HEIGHT = 271;
     private final int MANUAL_CHANGE = 50;

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

@@ -10,7 +10,7 @@
 import java.util.List;
 
 public class Placement implements Mechanism{
-    public static final double LEFT_SERVO_GRAB_POSITION = 0.55;
+    public static final double LEFT_SERVO_GRAB_POSITION = 0.5;
     public static final double LEFT_SERVO_EJECT_POSITION = 0.4;
     public static final double RIGHT_SERVO_GRAB_POSITION = .4;
     public static final double RIGHT_SERVO_EJECT_POSITION = .7;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/QQTest/TestTwoMotor.java

@@ -32,6 +32,9 @@ public void run(boolean on, Telemetry telemetry) {
         }
         telemetry.addData("Current Position Motor Right ", motorRight.getCurrentPosition());
         telemetry.addData("Current Position Motor Left ", motorLeft.getCurrentPosition());
+        telemetry.addData("Current Position Motor Left ", motorRight.getVelocity());
+        telemetry.addData("Current Position Motor Left ", motorLeft.getVelocity());
+
 
     }
 }

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

@@ -7,6 +7,7 @@
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Camera;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Climber;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub;
+import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Drone;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.HoldingCell;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Lift;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Intake;
@@ -35,6 +36,7 @@ public class Robot {
     public Arm arm;
     public Placement placement;
     public Climber climber;
+    public Drone drone;
 
 
 
@@ -51,6 +53,8 @@ public Robot() {
         arm = new Arm();
         placement = new Placement();
         climber = new Climber();
+        drone = new Drone();
+
 
 
         mechanisms = Arrays.asList(
@@ -63,7 +67,9 @@ public Robot() {
                 arm,
                 placement,
                 climber,
-                backDistanceSensor
+                backDistanceSensor,
+                drone
+
         );
     }
 

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

@@ -3,7 +3,7 @@
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
 public class ArmControl {
-   public final int SAFE_POSITION = 600;
+   public final int SAFE_POSITION = 800;
 
     public void updateArm (Robot robot){
         if (robot.lift.currentPosition() > SAFE_POSITION){

TeamCode/src/main/res/xml/comp_config.xml

@@ -2,6 +2,9 @@
 <Robot type="FirstInspires-FTC">
     <LynxUsbDevice name="Control Hub Portal" serialNumber="(embedded)" parentModuleAddress="173">
         <LynxModule name="Expansion Hub 2" port="2">
+            <DigitalDevice name="right_limit_switch" port="1" />
+            <DigitalDevice name="left_limit_switch" port="3" />
+            <Servo name="drone_servo" port="0" />
             <goBILDA5202SeriesMotor name="left_lift_motor" port="0" />
             <goBILDA5202SeriesMotor name="right_lift_motor" port="1" />
             <goBILDA5202SeriesMotor name="Intake_Motor" port="2" />
@@ -19,8 +22,7 @@
             <Servo name="right_arm_servo" port="2" />
             <Servo name="left_placement" port="4" />
             <Servo name="right_placement" port="5" />
-            <DigitalDevice name="right_limit_switch" port="1" />
-            <DigitalDevice name="left_limit_switch" port="3" />
+
             <ControlHubImuBHI260AP name="imu" port="0" bus="0" />
         </LynxModule>
     </LynxUsbDevice>