Implementing sensors and auto paths

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ClampOnPixel.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 ClampOnPixel extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.robot.placement.leftServoGrab();
+        opmode.robot.placement.rightServoGrab();
+        return State.SUCCESS;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveLiftToPixelGrabPosition.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 MoveLiftToPixelGrabPosition extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        opmode.liftControl.goToPixelGrab();
+        return State.SUCCESS;
+    }
+}

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

@@ -10,9 +10,7 @@ public State tick(QQOpMode opmode) {
         if(opmode.gamepad1.b){
             opmode.robot.placement.leftServoEject();
             opmode.robot.placement.rightServoEject();
-        }else if(opmode.gamepad1.a){
-            opmode.robot.placement.leftServoGrab();
-            opmode.robot.placement.rightServoGrab();
+
         }
         return State.RUNNING;
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/Has1Pixel.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 Has1Pixel extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+        if (opmode.robot.holdingCell.getNumPixels()==1){
+            return State.SUCCESS;
+        }
+        return  State.FAILURE;
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/HasMoreThan2Pixels.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/Has2Pixels.java

@@ -3,11 +3,11 @@
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 
-public class HasMoreThan2Pixels extends Node {
+public class Has2Pixels extends Node {
 
     @Override
     public State tick(QQOpMode opmode) {
-        if (opmode.robot.holdingCell.getNumPixels()>2){
+        if (opmode.robot.holdingCell.getNumPixels()==2){
             return State.SUCCESS;
         }
         return  State.FAILURE;

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

@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class IfLiftToPixelGrabPosButtonPressed  extends Node {
+
+    @Override
+    public State tick(QQOpMode opmode) {
+        if (opmode.gamepad1.x){
+            return State.SUCCESS;
+        } else{
+            return State.FAILURE;
+        }
+    }
+}

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

@@ -1,10 +1,12 @@
 package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;
 
+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.MoveLiftToPixelGrabPosition;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLiftPosition;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinInIntakeMotor;
@@ -14,9 +16,14 @@
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateArmAndLift;
 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.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;
@@ -111,17 +118,28 @@ public static Node root(){
                  */
                 new DriveFieldRelative(),
                 new UpdateArmAndLift(),
-                new PlacePixels(),
                 new Failover(
                         new Sequence(
-                                //new HasLessThan2Pixels(),
+                                new HasLessThan2Pixels(),
                                 new IfIntakeButtonPressed(),
                                 new SpinInIntakeMotor()
                         ),
-                        new Sequence(
-                                //new HasMoreThan2Pixels(), // having more than 2 pixels is impossible
+                        new Sequence( // moving lift down to grab pixels when there is 2 pixels
+                                new Has2Pixels(), // having more than 2 pixels is impossible
                                 new IfEjectButtonPressed(),
-                                new SpinOutIntakeMotor() // X not using eject
+                                new SpinOutIntakeMotor(), // X not using eject
+                                new MoveLiftToPixelGrabPosition(),
+                                new ClampOnPixel()
+
+                        ),
+                        new Sequence( // moving lift down to grab pixels when there is only 1 pixel
+                                new Has1Pixel(),
+                                new IfEjectButtonPressed(),
+                                new SpinOutIntakeMotor(), // X not using eject
+                                new IfLiftToPixelGrabPosButtonPressed(),
+                                new MoveLiftToPixelGrabPosition(),
+                                new ClampOnPixel()
+
                         ),
                         new StopIntakeMotor()
                 ),
@@ -132,10 +150,11 @@ public static Node root(){
                 ),
                 new Parallel(2,
                         new Sequence(
-                                //new Has1or2Pixels(),
+                                new Has1or2Pixels(),
                                 new Failover(
                                         new Sequence(
-                                                new MoveArmAndLift()
+                                                new MoveArmAndLift(),
+                                                new PlacePixels()
                                         )
                                 )
                         ),

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

@@ -29,15 +29,15 @@ public void init(HardwareMap hwMap) {
         leftArmServo = hwMap.get(Servo.class,"left_arm_servo");
         rightArmServo = hwMap.get(Servo.class,"right_arm_servo");
         rightArmServo.setDirection(Servo.Direction.REVERSE);
-        armPosition= ArmPositions.INTAKE_POSITION;
+        //armPosition= ArmPositions.INTAKE_POSITION; // could be
 
     }
 
     @Override
     public List<QQtest> getTests() {
         return Arrays.asList(
-                new TestServo("left_arm_servo", 0.5, 0, leftArmServo),
-                new TestServo("right_arm_servo", 0.5,0, rightArmServo)
+                new TestServo("left_arm_servo", 1, 0, leftArmServo),
+                new TestServo("right_arm_servo", 1,0, rightArmServo)
         );
     }
     public void goToIntakePosition(){

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

@@ -12,7 +12,7 @@
 import java.util.Arrays;
 import java.util.List;
 
-public class HoldingCell implements Mechanism{
+public class  HoldingCell implements Mechanism{
     public static final int PIXEL_THRESHOLD_MM = 20;
 
     private ColorRangeSensor leftPixelCounter;

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

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -12,7 +13,7 @@
 
 import java.util.Arrays;
 import java.util.List;
-
+@Config
 public class Lift implements  Mechanism{
     public LiftPositions getManipulatorPosition() {
         return manipulatorPosition;
@@ -40,10 +41,12 @@ public enum LiftPositions{
     private static final int LIFT_POSITION_SAFETY_BOTTOM = -50;
     private static final int LIFT_POSITION_SAFETY_TOP = 343455; //TODO need to fix
 
+    private static final int PIXEL_GRAB_POSITION=0;
+
     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 =0  ;
+    private static final int FLOOR_POSITION =100  ;
     private static final int PIXEL_HEIGHT = 271;
     private final int MANUAL_CHANGE = 50;
     private DcMotorEx rightLiftMotor;
@@ -55,9 +58,9 @@ public enum LiftPositions{
     private double desiredPosition;
     private double sumOfErrors;
     private double lastError;
-    static double K_P = 0.007;
-    static double K_I = 0.0001;
-    static double K_D = 0.2;
+    public static double K_P = 0.007;
+    public static double K_I = 0.0001;
+    public static double K_D = 0.2;
     public double motorPower;
 
 
@@ -135,7 +138,9 @@ public double getPower(){
     public double getDesiredPosition(){
         return desiredPosition;
     }
-
+    public void goToPixelGrab(){
+        setDesiredPosition(PIXEL_GRAB_POSITION);
+    }
 
     public void goToLow(){
         setDesiredPosition(LOW_POSITION);

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

@@ -12,10 +12,10 @@
 public class Placement implements Mechanism{
     public static final double PLACEMENT_GRAB = 0;
     public static final double PLACEMENT_EJECT = 0.3;
-    public static final double LEFT_SERVO_GRAB_POSITION = 0.7;
-    public static final double LEFT_SERVO_EJECT_POSITION = 0.4;
-    public static final double RIGHT_SERVO_GRAB_POSITION = 0.1;
-    public static final double RIGHT_SERVO_EJECT_POSITION = 1;
+    public static final double LEFT_SERVO_GRAB_POSITION = 1;
+    public static final double LEFT_SERVO_EJECT_POSITION = 0;
+    public static final double RIGHT_SERVO_GRAB_POSITION = 0.25;
+    public static final double RIGHT_SERVO_EJECT_POSITION = 0.65 ;
     Servo rightPlacement;
     Servo leftPlacement;
     @Override
@@ -25,6 +25,7 @@ public void init(HardwareMap hwMap) {
 
 
 
+
     }
     public void leftServoGrab(){
         leftPlacement.setPosition(LEFT_SERVO_GRAB_POSITION);

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

@@ -1,13 +1,22 @@
 package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
 
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.Gamepad;
 
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 @TeleOp
+@Config
 public class TestingRobotTeleOp extends QQOpMode {
+    FtcDashboard dashboard;
+
     public void init(){
         robot.init(hardwareMap);
+        dashboard = FtcDashboard.getInstance();
+        telemetry = dashboard.getTelemetry();
+
+
     }
     @Override
     public void loop() {
@@ -31,6 +40,10 @@ public void loop() {
             robot.lift.manualLiftDown();
         } else if (gamepad1.dpad_up){
             robot.lift.manualLiftUp();
+        } if (gamepad1.x){
+            robot.lift.goToMiddle();
+        } else if (gamepad1.b){
+            robot.lift.goToLow();
         }
 
 

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

@@ -22,6 +22,9 @@ public void manualLiftUp(){
     public void goToLow(){
         lift.goToLow();
     }
+    public void goToPixelGrab(){
+        lift.goToPixelGrab();
+    }
     public void goToMiddle(){
         lift.goToMiddle();
     }