From a3201ae2fcbc4ab26dbf1e89022df0738bad1dee Mon Sep 17 00:00:00 2001 From: joshua Smith Date: Fri, 10 Jan 2025 16:27:25 -0500 Subject: [PATCH 1/3] Two Specimen Cycle works --- .../BehaviorTrees/Actions/SlidesOut.java | 31 ++++++++++++ .../Trees/SpecimenCycleAutoTree.java | 47 +++++++++++++++++++ .../ftc16072/Mechanisms/IntakeSlides.java | 9 ++++ .../ftc16072/Mechanisms/ScoreArm.java | 4 +- .../ftc16072/OpModes/TwoSpecimenAuto.java | 4 -- 5 files changed, 89 insertions(+), 6 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOut.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOut.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOut.java new file mode 100644 index 0000000..abb856f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOut.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 SlidesOut extends QQTimeoutNode { + public SlidesOut(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.intakeSlides.halfExtension(); + opMode.robot.intakeArm.goToIntake(); + if (hasTimedOut()) { + lastStatus = State.FAILURE; + return State.FAILURE; + } else if (opMode.robot.intakeSlides.getIsWithinTolerence()) { + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java new file mode 100644 index 0000000..8991a3c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +/* Tree PreloadAuto for 16072 generated by http://behaviortrees.ftcteams.com */ + +import com.ftcteams.behaviortrees.Failover; +import com.ftcteams.behaviortrees.Node; +import com.ftcteams.behaviortrees.Parallel; +import com.ftcteams.behaviortrees.Sequence; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen; + + +public class SpecimenCycleAutoTree { + + public static final int TIMEOUT_SECONDS = 10; + + public static Node root(){ + return new Failover( + new Sequence( + new DriveToChamber(TIMEOUT_SECONDS), + new WaitForClawOpen(TIMEOUT_SECONDS), + new BehindChamber(TIMEOUT_SECONDS), + new Parallel(2, + new ArmToIntake(TIMEOUT_SECONDS), + new ReadyToIntake(TIMEOUT_SECONDS)), + Intake.root(), + new BehindChamber(TIMEOUT_SECONDS), + new DriveToChamber(TIMEOUT_SECONDS), + new WaitForClawOpen(TIMEOUT_SECONDS), + new Parallel(TIMEOUT_SECONDS)), + new Park(TIMEOUT_SECONDS)); + } +} + +/* TREE +? +| -> +| | [DriveToChamber] +| | [Score] +| | [Park] +| [Park] + */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java index 992c8d1..efc23b7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java @@ -34,6 +34,8 @@ public class IntakeSlides extends QQMechanism { private DcMotor leftIntakeSlideMotor; private TouchSensor limitSwitch; + boolean isWithinTolerence; + public int currentPos; public int desiredPos; @@ -89,6 +91,9 @@ public void update(Telemetry telemetry){ desiredPos = 0; } } + if (Math.abs(desiredPos - currentPos) <= 30){ + isWithinTolerence = true; + } currentPos = (leftIntakeSlideMotor.getCurrentPosition() + rightIntakeSlideMotor.getCurrentPosition())/2;//average left and right speeds double motorPower = pidfController.calculate(desiredPos,currentPos); if (desiredPos == 0 && !limitSwitch.isPressed()){ @@ -115,6 +120,10 @@ public boolean isSwitchPressed(){ } public boolean isSafeToRotate() { return currentPos > HALF_EXTENSION_POSITION;} + public boolean getIsWithinTolerence(){ + return isWithinTolerence; + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java index 904bb74..fbcf889 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java @@ -25,7 +25,7 @@ public class ScoreArm extends QQMechanism{ TouchSensor limitSwitch; TouchSensor rightChamberContact; TouchSensor leftChamberContact; - public static double kP = 0.00159; + public static double kP = 0.0017; public static double kI = 0.0; public static double kD = 0; public static double kF = 0; @@ -39,7 +39,7 @@ public class ScoreArm extends QQMechanism{ public static int INTAKE_POSITION = 0; public static int SCORING_POSITION = 350; - public static int PLACING_POSITION = 750; + public static int PLACING_POSITION = 880; public static int MOVING_POSITION = 600; public static int INIT_POSITION = 260; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TwoSpecimenAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TwoSpecimenAuto.java index 7d4522b..5e52854 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TwoSpecimenAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TwoSpecimenAuto.java @@ -43,9 +43,6 @@ public void start(){ public void loop() { super.loop(); - telemetry.addData("clawClosed",robot.scoringClaw.isClawClosed()); - telemetry.addData("clawWasClosed",clawWasClosed); - telemetry.addData("test",test); if (robot.scoringClaw.isClawClosed() && !clawWasClosed) { robot.intakeClaw.open(); robot.scoreArm.goToPlace(); @@ -59,7 +56,6 @@ public void loop() { } if(!done){ Node.State state = root.tick(debugTree, this); - //telemetry.addData("BT",debugTree); if(state == Node.State.SUCCESS){ done = true; } From 1789032194c2f99a1efb978ac1728fdf4ffb6937 Mon Sep 17 00:00:00 2001 From: joshua Smith Date: Sat, 11 Jan 2025 15:11:26 -0500 Subject: [PATCH 2/3] workedon three spec --- .../ftc16072/BehaviorTrees/Actions/Delay.java | 20 ++++++ .../BehaviorTrees/Actions/IntakeArmIn.java | 28 ++++++++ .../BehaviorTrees/Actions/IntakeArmOut.java | 28 ++++++++ .../Actions/IntakeClawClose.java | 28 ++++++++ .../BehaviorTrees/Actions/IntakeClawOpen.java | 28 ++++++++ ...adyToIntake.java => ReadyToIntakeOne.java} | 6 +- .../Actions/{SlidesOut.java => SlidesIn.java} | 7 +- .../Actions/SlidesOutToMiddle.java | 30 ++++++++ .../BehaviorTrees/Actions/WristToIntake.java | 28 ++++++++ .../Actions/WristToTransfer.java | 28 ++++++++ .../BehaviorTrees/Trees/DoubleIntake.java | 39 +++++++++++ .../ftc16072/BehaviorTrees/Trees/Intake.java | 4 +- .../BehaviorTrees/Trees/PreloadAuto.java | 5 +- .../Trees/SpecimenCycleAutoTree.java | 24 +++++-- .../Trees/TwoSpecimenAutoTree.java | 4 +- .../ftc16072/Mechanisms/IntakeSlides.java | 2 + .../ftc16072/OpModes/SpecimenCycleAuto.java | 68 +++++++++++++++++++ 17 files changed, 356 insertions(+), 21 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Delay.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeArmIn.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeArmOut.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeClawClose.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeClawOpen.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/{ReadyToIntake.java => ReadyToIntakeOne.java} (88%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/{SlidesOut.java => SlidesIn.java} (81%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOutToMiddle.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WristToIntake.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WristToTransfer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/DoubleIntake.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpecimenCycleAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Delay.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Delay.java new file mode 100644 index 0000000..655f723 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/Delay.java @@ -0,0 +1,20 @@ +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 Delay extends QQTimeoutNode { + public Delay(double seconds) { + super(seconds); + } + + @Override + public State tick(DebugTree debug, QQOpMode opMode) { + if (hasTimedOut()){ + return State.SUCCESS; + } + return State.RUNNING; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeArmIn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeArmIn.java new file mode 100644 index 0000000..a91a4de --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeArmIn.java @@ -0,0 +1,28 @@ +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 IntakeArmIn extends QQTimeoutNode { + public IntakeArmIn(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.intakeArm.goToDropPos(); + if (hasTimedOut()) { + opMode.robot.scoreArm.setNotScoring(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeArmOut.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeArmOut.java new file mode 100644 index 0000000..5da68d9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeArmOut.java @@ -0,0 +1,28 @@ +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 IntakeArmOut extends QQTimeoutNode { + public IntakeArmOut(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.intakeArm.goToIntake(); + if (hasTimedOut()) { + opMode.robot.scoreArm.setNotScoring(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeClawClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeClawClose.java new file mode 100644 index 0000000..759d25d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeClawClose.java @@ -0,0 +1,28 @@ +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 IntakeClawClose extends QQTimeoutNode { + public IntakeClawClose(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.intakeClaw.close(); + if (hasTimedOut()) { + opMode.robot.scoreArm.setNotScoring(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeClawOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeClawOpen.java new file mode 100644 index 0000000..8ef5e0e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/IntakeClawOpen.java @@ -0,0 +1,28 @@ +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 IntakeClawOpen extends QQTimeoutNode { + public IntakeClawOpen(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.intakeClaw.open(); + if (hasTimedOut()) { + opMode.robot.scoreArm.setNotScoring(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReadyToIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReadyToIntakeOne.java similarity index 88% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReadyToIntake.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReadyToIntakeOne.java index 5d6bdbe..0664259 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReadyToIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/ReadyToIntakeOne.java @@ -5,8 +5,8 @@ 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) { +public class ReadyToIntakeOne extends QQTimeoutNode { + public ReadyToIntakeOne(double seconds) { super(seconds); } State lastStatus = State.RUNNING; @@ -18,7 +18,7 @@ public State tick(DebugTree debug, QQOpMode opMode) { if (lastStatus != State.RUNNING){ return lastStatus; }else{ - boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-84,0); + boolean isDoneDriving = opMode.nav.driveToPositionIN(14,-82.5,0); if (isDoneDriving) { lastStatus = State.SUCCESS; return State.SUCCESS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOut.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesIn.java similarity index 81% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOut.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesIn.java index abb856f..7a1df9d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOut.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesIn.java @@ -5,8 +5,8 @@ import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.QQTimeoutNode; import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; -public class SlidesOut extends QQTimeoutNode { - public SlidesOut(double seconds) { +public class SlidesIn extends QQTimeoutNode { + public SlidesIn(double seconds) { super(seconds); } State lastStatus = State.RUNNING; @@ -16,8 +16,7 @@ public State tick(DebugTree debug, QQOpMode opMode) { if (lastStatus != State.RUNNING) { return lastStatus; }else { - opMode.robot.intakeSlides.halfExtension(); - opMode.robot.intakeArm.goToIntake(); + opMode.robot.intakeSlides.startPosition(); if (hasTimedOut()) { lastStatus = State.FAILURE; return State.FAILURE; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOutToMiddle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOutToMiddle.java new file mode 100644 index 0000000..c8d969c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SlidesOutToMiddle.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 SlidesOutToMiddle extends QQTimeoutNode { + public SlidesOutToMiddle(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.intakeSlides.setAutoExtensionPosition(); + if (hasTimedOut()) { + lastStatus = State.FAILURE; + return State.FAILURE; + } else if (opMode.robot.intakeSlides.getIsWithinTolerence()) { + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WristToIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WristToIntake.java new file mode 100644 index 0000000..940af49 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WristToIntake.java @@ -0,0 +1,28 @@ +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 WristToIntake extends QQTimeoutNode { + public WristToIntake(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.intakeClaw.wristIntake(); + if (hasTimedOut()) { + opMode.robot.scoreArm.setNotScoring(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WristToTransfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WristToTransfer.java new file mode 100644 index 0000000..935f992 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/WristToTransfer.java @@ -0,0 +1,28 @@ +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 WristToTransfer extends QQTimeoutNode { + public WristToTransfer(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.intakeClaw.wristTransfer(); + if (hasTimedOut()) { + opMode.robot.scoreArm.setNotScoring(); + lastStatus = State.SUCCESS; + return State.SUCCESS; + } + return State.RUNNING; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/DoubleIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/DoubleIntake.java new file mode 100644 index 0000000..cc8b660 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/DoubleIntake.java @@ -0,0 +1,39 @@ + +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +import com.ftcteams.behaviortrees.Failover; +import com.ftcteams.behaviortrees.Node; +import com.ftcteams.behaviortrees.Sequence; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeAttempt; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeClawClose; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SlidesIn; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WristToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WristToTransfer; + + +public class DoubleIntake { + public static Node root(){ + final double INTAKE_TIMEOUT_SECONDS = 1.5; + final double MOVEMENT_TIMEOUT_SECONDS = 5; + return new Sequence( + new WristToIntake(0.5), + new IntakeClawClose(0.5), + new WristToTransfer(0.5), + new SlidesIn(MOVEMENT_TIMEOUT_SECONDS), + new Failover( + new IntakeAttempt(INTAKE_TIMEOUT_SECONDS), + new Sequence( + new ReadyToIntakeOne(MOVEMENT_TIMEOUT_SECONDS), + new IntakeAttempt(INTAKE_TIMEOUT_SECONDS)))); + } +} + +/* TREE +? +| [IntakeAttempt] +| -> +| | [ReadyForIntake] +| | [IntakeAttempt] + */ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Intake.java index 8054c55..f24bf36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Intake.java @@ -5,7 +5,7 @@ import com.ftcteams.behaviortrees.Node; import com.ftcteams.behaviortrees.Sequence; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeAttempt; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; public class Intake { @@ -15,7 +15,7 @@ public static Node root(){ return new Failover( new IntakeAttempt(INTAKE_TIMEOUT_SECONDS), new Sequence( - new ReadyToIntake(MOVEMENT_TIMEOUT_SECONDS), + new ReadyToIntakeOne(MOVEMENT_TIMEOUT_SECONDS), new IntakeAttempt(INTAKE_TIMEOUT_SECONDS))); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PreloadAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PreloadAuto.java index 7e1ad57..60fc655 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PreloadAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PreloadAuto.java @@ -11,9 +11,8 @@ import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.GetReadyToPushSamples; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForScore; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; @@ -31,7 +30,7 @@ public static Node root(){ PushSamples.root(), new Parallel(2, new ArmToIntake(TIMEOUT_SECONDS), - new ReadyToIntake(TIMEOUT_SECONDS))), + new ReadyToIntakeOne(TIMEOUT_SECONDS))), new Park(TIMEOUT_SECONDS)); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java index 8991a3c..1e99f8e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenCycleAutoTree.java @@ -10,9 +10,12 @@ import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ArmToIntake; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.IntakeArmOut; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SlidesOutToMiddle; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Delay; public class SpecimenCycleAutoTree { @@ -24,15 +27,22 @@ public static Node root(){ new Sequence( new DriveToChamber(TIMEOUT_SECONDS), new WaitForClawOpen(TIMEOUT_SECONDS), - new BehindChamber(TIMEOUT_SECONDS), - new Parallel(2, + new Parallel(1, + new IntakeArmOut(4), + new BehindChamber(TIMEOUT_SECONDS) + ), + new Parallel(3, + new Sequence( + new Delay(1), + new SlidesOutToMiddle(TIMEOUT_SECONDS) + ), new ArmToIntake(TIMEOUT_SECONDS), - new ReadyToIntake(TIMEOUT_SECONDS)), - Intake.root(), - new BehindChamber(TIMEOUT_SECONDS), + new ReadyToIntakeOne(TIMEOUT_SECONDS)), + DoubleIntake.root()) + /* new BehindChamber(TIMEOUT_SECONDS), new DriveToChamber(TIMEOUT_SECONDS), new WaitForClawOpen(TIMEOUT_SECONDS), - new Parallel(TIMEOUT_SECONDS)), + new Parallel(TIMEOUT_SECONDS))*/, new Park(TIMEOUT_SECONDS)); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TwoSpecimenAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TwoSpecimenAutoTree.java index ce20708..23172c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TwoSpecimenAutoTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TwoSpecimenAutoTree.java @@ -11,7 +11,7 @@ import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.BehindChamber; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveToChamber; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.Park; -import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntake; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ReadyToIntakeOne; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.WaitForClawOpen; @@ -27,7 +27,7 @@ public static Node root(){ new BehindChamber(TIMEOUT_SECONDS), new Parallel(2, new ArmToIntake(TIMEOUT_SECONDS), - new ReadyToIntake(TIMEOUT_SECONDS)), + new ReadyToIntakeOne(TIMEOUT_SECONDS)), Intake.root(), new BehindChamber(TIMEOUT_SECONDS), new DriveToChamber(TIMEOUT_SECONDS), diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java index efc23b7..50e58fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java @@ -27,6 +27,7 @@ public class IntakeSlides extends QQMechanism { private static final int FULL_EXTENSION_POSITION = 1480; public static final int SLIDES_EXTENSION_BOUNDARY = FULL_EXTENSION_POSITION+10; private static final int HALF_EXTENSION_POSITION = 740; + private static final int AUTO_EXTENSION_POSITION = 940; private static final int START_POSITION = 0; public static double MANUAL_CHANGE_AMOUNT = 30; @@ -141,6 +142,7 @@ public void fullExtension(){ public void halfExtension(){ setPosition(HALF_EXTENSION_POSITION); } + public void setAutoExtensionPosition(){setPosition(AUTO_EXTENSION_POSITION);} public void startPosition(){ setPosition(START_POSITION); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpecimenCycleAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpecimenCycleAuto.java new file mode 100644 index 0000000..930fdc2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/SpecimenCycleAuto.java @@ -0,0 +1,68 @@ +package org.firstinspires.ftc.teamcode.ftc16072.OpModes; + +import com.ftcteams.behaviortrees.DebugTree; +import com.ftcteams.behaviortrees.Node; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.SpecimenCycleAutoTree; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.TwoSpecimenAutoTree; + +@Autonomous +public class SpecimenCycleAuto extends QQOpMode{ + boolean clawWasClosed; + boolean test; + boolean done; + Node root = SpecimenCycleAutoTree.root(); + DebugTree debugTree = new DebugTree(); + ElapsedTime moveTimer = new ElapsedTime(); + double INIT_MOVE_TIME_SECONDS = 1; + double INIT_MOVE_SPEED = -0.2; + @Override + public void init(){ + super.init(); + moveTimer.reset(); + robot.scoreArm.goToInit(); + robot.scoringClaw.close(); + robot.intakeArm.goToDropPos(); + } + + @Override + public void init_loop() { + if (moveTimer.seconds() < INIT_MOVE_TIME_SECONDS){ + robot.mecanumDrive.move(INIT_MOVE_SPEED,0,0); + }else { + robot.mecanumDrive.stop(); + robot.controlHub.resetGyro(); + } + robot.scoreArm.update(telemetry); + + } + public void start(){ + robot.otos.setOtosPosition(7,-61.5,0); + } + + public void loop() { + super.loop(); + if (robot.scoringClaw.isClawClosed() && !clawWasClosed) { + robot.intakeClaw.open(); + robot.scoreArm.goToPlace(); + test = true; + } + if (robot.scoringClaw.isClawOpen()){ + test = false; + } + if (robot.scoringClaw.isScoreSwitchPressed()){ + robot.scoreArm.setNotScoring(); + } + if(!done){ + Node.State state = root.tick(debugTree, this); + if(state == Node.State.SUCCESS){ + done = true; + } + } + clawWasClosed = robot.scoringClaw.isClawClosed(); + } +} + + From 4272ff01773155e03cde3aeec1c2bef85d86be47 Mon Sep 17 00:00:00 2001 From: joshua Date: Sun, 12 Jan 2025 16:32:13 -0500 Subject: [PATCH 3/3] Changed Positions in auto and added manual intake arm to gamepad 2 --- .../OpModes/PushintoNetandAscendAuto.java | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PushintoNetandAscendAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PushintoNetandAscendAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PushintoNetandAscendAuto.java new file mode 100644 index 0000000..23d421c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PushintoNetandAscendAuto.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.ftc16072.OpModes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +@Autonomous +public class PushintoNetandAscendAuto extends QQOpMode{ + private double step = 0; + + public void init(){ + super.init(); + robot.otos.setOtosPosition(-61.5,0,-90); + } + public void loop(){ + super.loop(); + if(step == 0){ + boolean donedriving = nav.driveToPositionIN(-58,24,0); + if(donedriving){ + step = 1; + } + }else if(step == 1){ + boolean doneDriving = nav.driveToPositionIN(-28,24,0); + if(doneDriving){ + step = 2; + } + }else if(step == 2){ + boolean doneDriving = nav.driveToPositionIN(-24,10,0); + if(doneDriving){ + step = 3; + } + } else if (step == 3) { + boolean doneDriving = nav.driveToPositionIN(-24,10,-90); + if (doneDriving){ + step = 4; + robot.scoreArm.goToPlace(); + } + } + + } +}