diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/AddTelemetry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/AddTelemetry.java new file mode 100644 index 0000000..fbedf14 --- /dev/null +++ b/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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveLiftToIntakePosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/MoveLiftToIntakePosition.java new file mode 100644 index 0000000..70962b9 --- /dev/null +++ b/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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PlacePixels.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PlacePixels.java index 21b713f..f920d0d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/PlacePixels.java +++ b/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; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/RumbleGamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/RumbleGamepad.java new file mode 100644 index 0000000..31bbe46 --- /dev/null +++ b/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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/Has1or2Pixels.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/Has1or2Pixels.java index 9eccd2f..25e5e36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/Has1or2Pixels.java +++ b/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; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfLiftAtBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfLiftAtBottom.java new file mode 100644 index 0000000..d695098 --- /dev/null +++ b/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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TeleopTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TeleopTree.java index f1c521d..e9f1597 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TeleopTree.java +++ b/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() + + ) + + + ) + + ) + ) + ) ); } + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/HoldingCell.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/HoldingCell.java index 5b43e57..6db2808 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/HoldingCell.java +++ b/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 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. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Lift.java index 843650d..e74e878 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Lift.java +++ b/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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/AprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/AprilTag.java index 7142f57..ed26c03 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/AprilTag.java +++ b/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("------------"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java index 1d4ed62..a4b45cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java +++ b/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 ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java index 6560487..63e35c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java +++ b/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();