From 3ab01c0a42bfa3d1d098da60f8be8d9f0765cc0d Mon Sep 17 00:00:00 2001 From: mahin Date: Mon, 30 Oct 2023 15:45:49 -0400 Subject: [PATCH 01/12] Added behavior tree for auto --- .../BehaviorTrees/Trees/AutoBehaviorTree.java | 47 +++++++++++++++++++ .../BehaviorTrees/Trees/CycleandPark.java | 44 +++++++++++++++++ .../ftc/teamcode/ftc16072/Mechanisms/Arm.java | 10 ++-- .../ftc16072/Mechanisms/ControlHub.java | 1 + .../teamcode/ftc16072/Mechanisms/Hang.java | 27 +++++++++++ .../teamcode/ftc16072/Mechanisms/Intake.java | 2 +- .../ftc/teamcode/ftc16072/Robot.java | 5 +- 7 files changed, 126 insertions(+), 10 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Hang.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java new file mode 100644 index 0000000..b982d94 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +public class AutoBehaviorTree { +}/* + +-> +| ? +| | -> +| | | (IsLeftSpike) +| | | [SetLeftSpikeTrajectory] +| | -> +| | | (IsRightSpike) +| | | [SetRightSpikeTrajectory] +| | [SetMiddleSpikeTrajectory] +| [FollowTrajectory] +| ? +| | -> (isClose) +| | | +| | | ->(isRed) +| | | |[SetRedCloseTrajectory] +| | | [SetBlueCloseTrajectory] +| | -> (isRed) +| | | [SetRedFarTrajectory] +| | [SetBlueFarTrajectory] +| [FollowTrajectory] +| ? +| | -> (IsLeftSpike) +| | | [SetLeftAprilTagTrajectory] +| | -> (IsRightSpike) +| | | [SetRightAprilTagTrajectory] +| | [SetMiddleAprilTagTrajectory] +| [FollowTrajectory] +| [PlacePixel] +| ? +| | -> (isRed) +| | | -> (IsLeftSpike) +| | | | [SetRedLeftAprilTagTrajectory] +| | | -> (IsRightSpike) +| | | | [SetRedRightAprilTagTrajectory] +| | | [SetRedMiddleAprilTagTrajectory] +| | -> (IsLeftSpike) +| | | [SetBlueLeftAprilTagTrajectory] +| | -> (IsRightSpike) +| | | [SeBlueRightAprilTagTrajectory] +| | [SetBlueMiddleAprilTagTrajectory] +| [CycleandPark] +*/ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java new file mode 100644 index 0000000..8effb70 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +public class CycleandPark { +} +/* +-> +| ? (enoughTimeForCycle) +| | -> (isRed) +| | | ? +| | | | -> (isLocationIntake) +| | | | | [setRedIntakeParkTrajectory] +| | | | -> (isLocationPlacing) +| | | | | [setRedPlacingParkTrajectory] +| | -> !(isRed) +| | | ? +| | | | -> (isLocationIntake) +| | | | | [setBlueIntakeParkTrajectory] +| | | | -> (isLocationPlacing) +| | | | | [setBluePlacingParkTrajectory] +| | -> [FollowTrajectory] +| ? +| | -> (isRed) +| | | ? +| | | | -> (isLeftSpike) +| | | | | [setRedLeftTrajectory] +| | | | -> (isRightSpike) +| | | | | [setRedRightTrajectory] +| | | | [setRedMiddleTrajectory] +| | -> +| | | (isLeftSpike) +| | | [setRedLeftTrajectory] +| | -> (isRightSpike) +| | | [setRedRightTrajectory] +| | [setRedMiddleTrajectory] +| [followTrajectory] +| [setLocationIntake] +| ? (has2Pixels) +| | [intakePixel] +| ? +| | -> (isRed) +| | | [setRedPlacingTrajectory] +| | [setBluePlacingTrajectory] +| [followTrajectory] +| [setLocationPlacing] */ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java index 3bc96ad..d00365a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java @@ -13,13 +13,13 @@ @Config public class Arm implements Mechanism{ - public static double WRIST_INTAKE_POS = 0.0; - public static double WRIST_PLACING_POS = 0.8; - public static double WRIST_DRIVING_POS = 0.2; + public static double WRIST_INTAKE_POS = 0.25; + public static double WRIST_PLACING_POS = 1; + public static double WRIST_DRIVING_POS = 0.4; public static double WRIST_TESTING_POS = 0.2; - public static double ARM_INTAKE_POS = 0.8; + public static double ARM_INTAKE_POS = 1; public static double ARM_PLACING_POS = 0.0; - public static double ARM_DRIVING_POS = 0.7; + public static double ARM_DRIVING_POS = 0.8; public static double ARM_TESTING_POS = 0.2; public static double MAX_ARM_CHANGE = 0.02; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ControlHub.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ControlHub.java index 43048fb..ea94432 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ControlHub.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ControlHub.java @@ -29,6 +29,7 @@ public void init(HardwareMap hwMap) { // Now initialize the IMU with this mounting orientation // Note: if you choose two conflicting directions, this initialization will cause a code exception. gyro.initialize(new IMU.Parameters(orientationOnRobot)); + gyro.resetYaw(); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Hang.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Hang.java new file mode 100644 index 0000000..c3e8f2a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Hang.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.ftc16072.QQTest.QQtest; + +import java.util.List; + +public class Hang implements Mechanism { + Servo hangServo; + @Override + public void init(HardwareMap hwMap) { + hangServo = hwMap.get(Servo.class, "hang"); + } + public void hang(){ + hangServo.setPosition(0); + } + public void reset(){ + hangServo.setPosition(1); + } + + @Override + public List getTests() { + return null; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Intake.java index ee529c2..4387dd9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Intake.java @@ -11,7 +11,7 @@ import java.util.List; public class Intake implements Mechanism{ - public static final double Intake_Speed = 1.0; + public static final double Intake_Speed = 0.3; public static final double Eject_Speed = -0.5; private DcMotor intakeLeft; private DcMotor intakeRight; 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 8dc3723..fc4b343 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 @@ -4,11 +4,11 @@ import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Arm; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.LineDetector; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Intake; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Mechanism; import org.firstinspires.ftc.teamcode.ftc16072.Util.Navigation; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Intake; import java.util.Arrays; import java.util.List; @@ -18,7 +18,6 @@ public class Robot { public MecanumDrive mecanumDrive; public Navigation nav; public ControlHub controlHub; - public LineDetector lineDetector; public Intake intake; public Arm arm; @@ -26,7 +25,6 @@ public class Robot { public Robot() { controlHub = new ControlHub(); mecanumDrive = new MecanumDrive(); - lineDetector = new LineDetector(); nav = new Navigation(controlHub, mecanumDrive); intake = new Intake(); arm = new Arm(); @@ -35,7 +33,6 @@ public Robot() { controlHub, intake, arm - // lineDetector ); } From 0da32bb2d719c18ff06c5bf9a0888887e13001fc Mon Sep 17 00:00:00 2001 From: mahin Date: Sun, 5 Nov 2023 11:41:05 -0500 Subject: [PATCH 02/12] Changed and simplified auto behavior trees --- .../BehaviorTrees/Trees/AutoBehaviorTree.java | 20 ++---- .../BehaviorTrees/Trees/CycleandPark.java | 61 ++++++++----------- 2 files changed, 31 insertions(+), 50 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java index b982d94..5489bb2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java @@ -2,7 +2,6 @@ public class AutoBehaviorTree { }/* - -> | ? | | -> @@ -16,9 +15,10 @@ public class AutoBehaviorTree { | ? | | -> (isClose) | | | -| | | ->(isRed) -| | | |[SetRedCloseTrajectory] -| | | [SetBlueCloseTrajectory] +| | | ? +| | | | ->(isRed) +| | | | |[SetRedCloseTrajectory] +| | | | [SetBlueCloseTrajectory] | | -> (isRed) | | | [SetRedFarTrajectory] | | [SetBlueFarTrajectory] @@ -31,17 +31,5 @@ public class AutoBehaviorTree { | | [SetMiddleAprilTagTrajectory] | [FollowTrajectory] | [PlacePixel] -| ? -| | -> (isRed) -| | | -> (IsLeftSpike) -| | | | [SetRedLeftAprilTagTrajectory] -| | | -> (IsRightSpike) -| | | | [SetRedRightAprilTagTrajectory] -| | | [SetRedMiddleAprilTagTrajectory] -| | -> (IsLeftSpike) -| | | [SetBlueLeftAprilTagTrajectory] -| | -> (IsRightSpike) -| | | [SeBlueRightAprilTagTrajectory] -| | [SetBlueMiddleAprilTagTrajectory] | [CycleandPark] */ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java index 8effb70..17a3616 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java @@ -5,40 +5,33 @@ public class CycleandPark { /* -> | ? (enoughTimeForCycle) +| | (isParked) | | -> (isRed) -| | | ? -| | | | -> (isLocationIntake) -| | | | | [setRedIntakeParkTrajectory] -| | | | -> (isLocationPlacing) -| | | | | [setRedPlacingParkTrajectory] +| | | [setRedParkTrajectory] | | -> !(isRed) -| | | ? -| | | | -> (isLocationIntake) -| | | | | [setBlueIntakeParkTrajectory] -| | | | -> (isLocationPlacing) -| | | | | [setBluePlacingParkTrajectory] +| | | [setBlueParkTrajectory] | | -> [FollowTrajectory] -| ? -| | -> (isRed) -| | | ? -| | | | -> (isLeftSpike) -| | | | | [setRedLeftTrajectory] -| | | | -> (isRightSpike) -| | | | | [setRedRightTrajectory] -| | | | [setRedMiddleTrajectory] -| | -> -| | | (isLeftSpike) -| | | [setRedLeftTrajectory] -| | -> (isRightSpike) -| | | [setRedRightTrajectory] -| | [setRedMiddleTrajectory] -| [followTrajectory] -| [setLocationIntake] -| ? (has2Pixels) -| | [intakePixel] -| ? -| | -> (isRed) -| | | [setRedPlacingTrajectory] -| | [setBluePlacingTrajectory] -| [followTrajectory] -| [setLocationPlacing] */ \ No newline at end of file +| -> !(isParked) +| | ? +| | | -> (isRed) +| | | | ? +| | | | | -> (isLeftSpike) +| | | | | | [setRedLeftTrajectory] +| | | | | -> (isRightSpike) +| | | | | | [setRedRightTrajectory] +| | | | | [setRedMiddleTrajectory] +| | | -> +| | | | (isLeftSpike) +| | | | [setBlueLeftTrajectory] +| | | -> (isRightSpike) +| | | | [setBlueRightTrajectory] +| | | [setBlueMiddleTrajectory] +| | [FollowTrajectory] +| | ? (has2Pixels) +| | | [intakePixel] +| | ? +| | | -> (isRed) +| | | | [setRedPlacingTrajectory] +| | | [setBluePlacingTrajectory] +| | [FollowTrajectory] + */ \ No newline at end of file From 05e7ad7d271d4d138aa3b2001301d7291e863534 Mon Sep 17 00:00:00 2001 From: alanwong9664 <57013773+alanwong9664@users.noreply.github.com> Date: Sat, 2 Dec 2023 18:45:47 -0500 Subject: [PATCH 03/12] started working on autobehavior tree --- .../BehaviorTrees/Trees/AutoBehaviorTree.java | 51 ++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java index 5489bb2..6b541e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java @@ -1,5 +1,13 @@ package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLeftSpikeTrajectory; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetMiddleSpikeTrajectory; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsLeftSpike; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsRightSpike; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Sequence; + public class AutoBehaviorTree { }/* -> @@ -32,4 +40,45 @@ public class AutoBehaviorTree { | [FollowTrajectory] | [PlacePixel] | [CycleandPark] -*/ \ No newline at end of file +*/ +public class AutoTree{ + public static Node root(){ + new Sequence( + new Failover( + new Sequence( + new IsLeftSpike(), + new SetLeftSpikeTrajectory() + ), + new Sequence( + new IsRightSpike(), + new setRightSpikeTrajectory() + ), + new SetMiddleSpikeTrajectory + + ), + new FollowTrajectory(), + new Failover( + new Sequence( + new SetBlueFarTrajectory(), + new IsRed(), + new SetRedFarTrajectory() + ) + new Sequence( + new isClose(), + new Failover( + new SetBlueCloseTrajectory(), + new Sequence( + new IsRed(), + new SetRedCloseTrajectory() + ) + ) + ), + new SetBlueTrajectory() + ), + + ) + + ) + + } +} \ No newline at end of file From 0399d3725aae189a832f5cbc40797cca31528b4f Mon Sep 17 00:00:00 2001 From: alanwong9664 <57013773+alanwong9664@users.noreply.github.com> Date: Mon, 4 Dec 2023 20:41:14 -0500 Subject: [PATCH 04/12] added auto tree in java --- .../BehaviorTrees/Trees/SpikeAutoTree.java | 47 ++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java index 1086b04..99639ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java @@ -36,7 +36,52 @@ public static Node root(){ ), new SetMiddleSpikeTrajectory() ), - new FollowTrajectory() + new FollowTrajectory(), + new Failover( + new Sequence( + new IsClose(), + new Failover( + new Sequence( + new IsRed(), + new SetRedCloseTrajectory() + + + ), + new SetBlueCloseTrajectory() + ) + ), + new Sequence( + new IsRed(), + new SetRedFarTrajectory() + + + ), + new SetBlueFarTrajectory() + ), + new FollowTrajectory(), + new Failover( + new Sequence( + new IsLeftSpike(), + new SetLeftAprilTagTrajectory() + + + ), + new Sequence( + new IsRightSpike(), + new SetRightAprilTagTrajectory() + ), + new SetMiddleAprilTagTrajectory() + ), + new FollowTrajectory(), + new PlacePixel(), + new SetGotoPixelStackTrajectory(), + new TurnOnIntake(), + new Sequence( + new HAS2PIXEls(), + new SetBackBoardTrajectory() + ), + new PlacePixel() + ); } } From 6112d76da142cc3696d4f6c2399a447c51e6e3d3 Mon Sep 17 00:00:00 2001 From: mahin Date: Mon, 30 Oct 2023 15:45:49 -0400 Subject: [PATCH 05/12] Added behavior tree for auto --- .../BehaviorTrees/Trees/AutoBehaviorTree.java | 47 +++++++++++++++++++ .../BehaviorTrees/Trees/CycleandPark.java | 44 +++++++++++++++++ .../teamcode/ftc16072/Mechanisms/Hang.java | 27 +++++++++++ .../ftc/teamcode/ftc16072/Robot.java | 33 +------------ 4 files changed, 120 insertions(+), 31 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Hang.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java new file mode 100644 index 0000000..b982d94 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +public class AutoBehaviorTree { +}/* + +-> +| ? +| | -> +| | | (IsLeftSpike) +| | | [SetLeftSpikeTrajectory] +| | -> +| | | (IsRightSpike) +| | | [SetRightSpikeTrajectory] +| | [SetMiddleSpikeTrajectory] +| [FollowTrajectory] +| ? +| | -> (isClose) +| | | +| | | ->(isRed) +| | | |[SetRedCloseTrajectory] +| | | [SetBlueCloseTrajectory] +| | -> (isRed) +| | | [SetRedFarTrajectory] +| | [SetBlueFarTrajectory] +| [FollowTrajectory] +| ? +| | -> (IsLeftSpike) +| | | [SetLeftAprilTagTrajectory] +| | -> (IsRightSpike) +| | | [SetRightAprilTagTrajectory] +| | [SetMiddleAprilTagTrajectory] +| [FollowTrajectory] +| [PlacePixel] +| ? +| | -> (isRed) +| | | -> (IsLeftSpike) +| | | | [SetRedLeftAprilTagTrajectory] +| | | -> (IsRightSpike) +| | | | [SetRedRightAprilTagTrajectory] +| | | [SetRedMiddleAprilTagTrajectory] +| | -> (IsLeftSpike) +| | | [SetBlueLeftAprilTagTrajectory] +| | -> (IsRightSpike) +| | | [SeBlueRightAprilTagTrajectory] +| | [SetBlueMiddleAprilTagTrajectory] +| [CycleandPark] +*/ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java new file mode 100644 index 0000000..8effb70 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; + +public class CycleandPark { +} +/* +-> +| ? (enoughTimeForCycle) +| | -> (isRed) +| | | ? +| | | | -> (isLocationIntake) +| | | | | [setRedIntakeParkTrajectory] +| | | | -> (isLocationPlacing) +| | | | | [setRedPlacingParkTrajectory] +| | -> !(isRed) +| | | ? +| | | | -> (isLocationIntake) +| | | | | [setBlueIntakeParkTrajectory] +| | | | -> (isLocationPlacing) +| | | | | [setBluePlacingParkTrajectory] +| | -> [FollowTrajectory] +| ? +| | -> (isRed) +| | | ? +| | | | -> (isLeftSpike) +| | | | | [setRedLeftTrajectory] +| | | | -> (isRightSpike) +| | | | | [setRedRightTrajectory] +| | | | [setRedMiddleTrajectory] +| | -> +| | | (isLeftSpike) +| | | [setRedLeftTrajectory] +| | -> (isRightSpike) +| | | [setRedRightTrajectory] +| | [setRedMiddleTrajectory] +| [followTrajectory] +| [setLocationIntake] +| ? (has2Pixels) +| | [intakePixel] +| ? +| | -> (isRed) +| | | [setRedPlacingTrajectory] +| | [setBluePlacingTrajectory] +| [followTrajectory] +| [setLocationPlacing] */ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Hang.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Hang.java new file mode 100644 index 0000000..c3e8f2a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Hang.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.ftc16072.QQTest.QQtest; + +import java.util.List; + +public class Hang implements Mechanism { + Servo hangServo; + @Override + public void init(HardwareMap hwMap) { + hangServo = hwMap.get(Servo.class, "hang"); + } + public void hang(){ + hangServo.setPosition(0); + } + public void reset(){ + hangServo.setPosition(1); + } + + @Override + public List getTests() { + return null; + } +} 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 533534e..fc4b343 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 @@ -3,16 +3,12 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Arm; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Camera; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.HoldingCell; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Lift; -import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.LineDetector; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Intake; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Mechanism; import org.firstinspires.ftc.teamcode.ftc16072.Util.Navigation; -import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Intake; import java.util.Arrays; import java.util.List; @@ -22,42 +18,21 @@ public class Robot { public MecanumDrive mecanumDrive; public Navigation nav; public ControlHub controlHub; - public LineDetector lineDetector; public Intake intake; - public HoldingCell holdingCell; - public Camera cameraFront; - public Camera cameraBack; - public Lift lift; public Arm arm; - - public Robot() { - cameraFront = new Camera(Camera.CameraPosition.FRONT); - cameraBack = new Camera(Camera.CameraPosition.BACK); controlHub = new ControlHub(); mecanumDrive = new MecanumDrive(); - lineDetector = new LineDetector(); - nav = new Navigation(controlHub, mecanumDrive, cameraFront,cameraBack); + nav = new Navigation(controlHub, mecanumDrive); intake = new Intake(); - holdingCell = new HoldingCell(); - lift = new Lift(); arm = new Arm(); - - mechanisms = Arrays.asList( mecanumDrive, controlHub, intake, - cameraBack, - cameraFront, - holdingCell, - cameraFront, - holdingCell, - lift, arm - // lineDetector ); } @@ -69,10 +44,6 @@ public void makeDriveOnly() { } public void init(HardwareMap hwMap) { - int[] ids = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.HORIZONTAL); - cameraFront.setViewPortID(ids[0]); - cameraBack.setViewPortID(ids[1]); - for (Mechanism mechanism : mechanisms) { mechanism.init(hwMap); } From 81ddd5e95da569a41fa812ae15cee4e4ff563c0e Mon Sep 17 00:00:00 2001 From: mahin Date: Sun, 5 Nov 2023 11:41:05 -0500 Subject: [PATCH 06/12] Changed and simplified auto behavior trees --- .../BehaviorTrees/Trees/AutoBehaviorTree.java | 20 ++---- .../BehaviorTrees/Trees/CycleandPark.java | 61 ++++++++----------- 2 files changed, 31 insertions(+), 50 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java index b982d94..5489bb2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java @@ -2,7 +2,6 @@ public class AutoBehaviorTree { }/* - -> | ? | | -> @@ -16,9 +15,10 @@ public class AutoBehaviorTree { | ? | | -> (isClose) | | | -| | | ->(isRed) -| | | |[SetRedCloseTrajectory] -| | | [SetBlueCloseTrajectory] +| | | ? +| | | | ->(isRed) +| | | | |[SetRedCloseTrajectory] +| | | | [SetBlueCloseTrajectory] | | -> (isRed) | | | [SetRedFarTrajectory] | | [SetBlueFarTrajectory] @@ -31,17 +31,5 @@ public class AutoBehaviorTree { | | [SetMiddleAprilTagTrajectory] | [FollowTrajectory] | [PlacePixel] -| ? -| | -> (isRed) -| | | -> (IsLeftSpike) -| | | | [SetRedLeftAprilTagTrajectory] -| | | -> (IsRightSpike) -| | | | [SetRedRightAprilTagTrajectory] -| | | [SetRedMiddleAprilTagTrajectory] -| | -> (IsLeftSpike) -| | | [SetBlueLeftAprilTagTrajectory] -| | -> (IsRightSpike) -| | | [SeBlueRightAprilTagTrajectory] -| | [SetBlueMiddleAprilTagTrajectory] | [CycleandPark] */ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java index 8effb70..17a3616 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java @@ -5,40 +5,33 @@ public class CycleandPark { /* -> | ? (enoughTimeForCycle) +| | (isParked) | | -> (isRed) -| | | ? -| | | | -> (isLocationIntake) -| | | | | [setRedIntakeParkTrajectory] -| | | | -> (isLocationPlacing) -| | | | | [setRedPlacingParkTrajectory] +| | | [setRedParkTrajectory] | | -> !(isRed) -| | | ? -| | | | -> (isLocationIntake) -| | | | | [setBlueIntakeParkTrajectory] -| | | | -> (isLocationPlacing) -| | | | | [setBluePlacingParkTrajectory] +| | | [setBlueParkTrajectory] | | -> [FollowTrajectory] -| ? -| | -> (isRed) -| | | ? -| | | | -> (isLeftSpike) -| | | | | [setRedLeftTrajectory] -| | | | -> (isRightSpike) -| | | | | [setRedRightTrajectory] -| | | | [setRedMiddleTrajectory] -| | -> -| | | (isLeftSpike) -| | | [setRedLeftTrajectory] -| | -> (isRightSpike) -| | | [setRedRightTrajectory] -| | [setRedMiddleTrajectory] -| [followTrajectory] -| [setLocationIntake] -| ? (has2Pixels) -| | [intakePixel] -| ? -| | -> (isRed) -| | | [setRedPlacingTrajectory] -| | [setBluePlacingTrajectory] -| [followTrajectory] -| [setLocationPlacing] */ \ No newline at end of file +| -> !(isParked) +| | ? +| | | -> (isRed) +| | | | ? +| | | | | -> (isLeftSpike) +| | | | | | [setRedLeftTrajectory] +| | | | | -> (isRightSpike) +| | | | | | [setRedRightTrajectory] +| | | | | [setRedMiddleTrajectory] +| | | -> +| | | | (isLeftSpike) +| | | | [setBlueLeftTrajectory] +| | | -> (isRightSpike) +| | | | [setBlueRightTrajectory] +| | | [setBlueMiddleTrajectory] +| | [FollowTrajectory] +| | ? (has2Pixels) +| | | [intakePixel] +| | ? +| | | -> (isRed) +| | | | [setRedPlacingTrajectory] +| | | [setBluePlacingTrajectory] +| | [FollowTrajectory] + */ \ No newline at end of file From 9a9c133b17c60d10f4cd420db588beb436331f0b Mon Sep 17 00:00:00 2001 From: alanwong9664 <57013773+alanwong9664@users.noreply.github.com> Date: Sat, 2 Dec 2023 18:45:47 -0500 Subject: [PATCH 07/12] started working on autobehavior tree --- .../BehaviorTrees/Trees/AutoBehaviorTree.java | 51 ++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java index 5489bb2..6b541e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/AutoBehaviorTree.java @@ -1,5 +1,13 @@ package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLeftSpikeTrajectory; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetMiddleSpikeTrajectory; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsLeftSpike; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsRightSpike; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Sequence; + public class AutoBehaviorTree { }/* -> @@ -32,4 +40,45 @@ public class AutoBehaviorTree { | [FollowTrajectory] | [PlacePixel] | [CycleandPark] -*/ \ No newline at end of file +*/ +public class AutoTree{ + public static Node root(){ + new Sequence( + new Failover( + new Sequence( + new IsLeftSpike(), + new SetLeftSpikeTrajectory() + ), + new Sequence( + new IsRightSpike(), + new setRightSpikeTrajectory() + ), + new SetMiddleSpikeTrajectory + + ), + new FollowTrajectory(), + new Failover( + new Sequence( + new SetBlueFarTrajectory(), + new IsRed(), + new SetRedFarTrajectory() + ) + new Sequence( + new isClose(), + new Failover( + new SetBlueCloseTrajectory(), + new Sequence( + new IsRed(), + new SetRedCloseTrajectory() + ) + ) + ), + new SetBlueTrajectory() + ), + + ) + + ) + + } +} \ No newline at end of file From eee47a04b09abe8d68ee76e34d55be0f320ceafc Mon Sep 17 00:00:00 2001 From: alanwong9664 <57013773+alanwong9664@users.noreply.github.com> Date: Mon, 4 Dec 2023 20:41:14 -0500 Subject: [PATCH 08/12] added auto tree in java --- .../BehaviorTrees/Trees/SpikeAutoTree.java | 47 ++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java index 1086b04..99639ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java @@ -36,7 +36,52 @@ public static Node root(){ ), new SetMiddleSpikeTrajectory() ), - new FollowTrajectory() + new FollowTrajectory(), + new Failover( + new Sequence( + new IsClose(), + new Failover( + new Sequence( + new IsRed(), + new SetRedCloseTrajectory() + + + ), + new SetBlueCloseTrajectory() + ) + ), + new Sequence( + new IsRed(), + new SetRedFarTrajectory() + + + ), + new SetBlueFarTrajectory() + ), + new FollowTrajectory(), + new Failover( + new Sequence( + new IsLeftSpike(), + new SetLeftAprilTagTrajectory() + + + ), + new Sequence( + new IsRightSpike(), + new SetRightAprilTagTrajectory() + ), + new SetMiddleAprilTagTrajectory() + ), + new FollowTrajectory(), + new PlacePixel(), + new SetGotoPixelStackTrajectory(), + new TurnOnIntake(), + new Sequence( + new HAS2PIXEls(), + new SetBackBoardTrajectory() + ), + new PlacePixel() + ); } } From 938ca3a31fb2461af32105ba665cbb3fd18dcc3d Mon Sep 17 00:00:00 2001 From: alanwong9664 <57013773+alanwong9664@users.noreply.github.com> Date: Sat, 9 Dec 2023 15:39:46 -0500 Subject: [PATCH 09/12] added auto base file and some conditionals --- .../BehaviorTrees/Conditions/IsClose.java | 18 ++++++ .../BehaviorTrees/Trees/SpikeAutoTree.java | 1 + .../teamcode/ftc16072/OpModes/AutoBase.java | 60 +++++++++++++++++++ .../ftc16072/OpModes/BlueSideAuto.java | 25 ++++++++ .../teamcode/ftc16072/OpModes/QQOpMode.java | 4 ++ .../teamcode/ftc16072/OpModes/TestWiring.java | 4 +- .../ftc/teamcode/ftc16072/Robot.java | 25 +++++++- 7 files changed, 132 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IsClose.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/AutoBase.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueSideAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IsClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IsClose.java new file mode 100644 index 0000000..d315554 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IsClose.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.AutoBase; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; + +public class IsClose extends Node { + AutoBase autoBase; + @Override + public State tick(QQOpMode opmode) { + if (autoBase.getIsCloseSide()){ + return State.SUCCESS; + } else { + return State.FAILURE; + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java index 99639ad..e1b18da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpikeAutoTree.java @@ -4,6 +4,7 @@ import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLeftSpikeTrajectory; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetMiddleSpikeTrajectory; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetRightSpikeTrajectory; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsClose; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsLeftSpike; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsRightSpike; import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/AutoBase.java new file mode 100644 index 0000000..a72f03b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/AutoBase.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.ftc16072.OpModes; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +import org.firstinspires.ftc.teamcode.ftc16072.Util.Navigation; + +public class AutoBase extends QQOpMode{ + Navigation nav; + public enum TeamColor{ + RED, + BLUE + } + public TeamColor teamColor; + boolean isFarSide; + public boolean isCloseSide; + public boolean getIsCloseSide(){ + return isCloseSide; + } + + + @Override + public void init() { + super.init(); + + } + + @Override + public void init_loop() { + super.init_loop(); + if (gamepad1.left_bumper){ + isCloseSide = true; + isFarSide = false; + + } + if (gamepad1.right_bumper){ + isFarSide = true; + isCloseSide = false; + } + } + + public void start(){ + if (isCloseSide){ + nav.setPoseEstimate(new Pose2d(2+36,2,0)); //TODO edit values + } else { + nav.setPoseEstimate(new Pose2d(2-26,2,0)); //TODO edit values + } + } + @Override + + public void loop() { + + + + + } + public void setTeamColor(TeamColor teamColor){ + this.teamColor= teamColor; + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueSideAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueSideAuto.java new file mode 100644 index 0000000..2618234 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueSideAuto.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.ftc16072.OpModes; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees.AutoTree; + +public class BlueSideAuto extends AutoBase{ + Node root = AutoTree.root(); + boolean done; + public void init(){ + setTeamColor(TeamColor.BLUE); + } + + public void loop(){ + if(!done){ + Node.State state = root.tick(this); + if(state == Node.State.SUCCESS){ + done = true; + } + } + + } + + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java index 4c9dd63..b58fd6b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.ftc16072.OpModes; +import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.ftc16072.Robot; @@ -11,6 +12,7 @@ */ abstract public class QQOpMode extends OpMode { + public Robot robot = new Robot(); public BlackBoard board = new BlackBoard(); @@ -18,6 +20,8 @@ abstract public class QQOpMode extends OpMode { * every opmode initializes robot with hardware map */ public void init(){ + + robot.init(hardwareMap); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestWiring.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestWiring.java index 3d72e82..ddf0fea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestWiring.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestWiring.java @@ -1,11 +1,9 @@ package org.firstinspires.ftc.teamcode.ftc16072.OpModes; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Mechanism; import org.firstinspires.ftc.teamcode.ftc16072.QQTest.QQtest; -import org.firstinspires.ftc.teamcode.ftc16072.Robot; import java.util.List; @@ -64,4 +62,6 @@ public void loop() { testList.get(currentTest).run(gamepad1.a, telemetry); } + + } 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 fc4b343..68d4fad 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 @@ -3,10 +3,14 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Arm; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Camera; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.HoldingCell; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Intake; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Lift; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Mechanism; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Placement; import org.firstinspires.ftc.teamcode.ftc16072.Util.Navigation; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Intake; @@ -19,20 +23,35 @@ public class Robot { public Navigation nav; public ControlHub controlHub; public Intake intake; + public Camera cameraBack; + public Camera cameraFront; + public HoldingCell holdingCell; - public Arm arm; + public Placement placement; + public Arm arm; + public Lift lift; public Robot() { controlHub = new ControlHub(); mecanumDrive = new MecanumDrive(); - nav = new Navigation(controlHub, mecanumDrive); + cameraBack = new Camera(Camera.CameraPosition.BACK); + cameraFront = new Camera(Camera.CameraPosition.FRONT); + holdingCell = new HoldingCell(); + nav = new Navigation(controlHub, mecanumDrive,cameraFront,cameraBack); intake = new Intake(); arm = new Arm(); + lift = new Lift(); + placement = new Placement(); mechanisms = Arrays.asList( mecanumDrive, controlHub, intake, - arm + arm, + lift, + cameraFront, + cameraBack, + holdingCell, + placement ); } From 1e1fd9345a591465fdc46fc5af10cf349777bddf Mon Sep 17 00:00:00 2001 From: Alan Wong Date: Mon, 11 Dec 2023 16:04:07 -0500 Subject: [PATCH 10/12] added tape parking trajectories --- .../Actions/SetLeftSpikeTrajectory.java | 14 +++++++++----- .../Actions/SetMiddleSpikeTrajectory.java | 17 ++++++++++++----- .../Actions/SetRightSpikeTrajectory.java | 15 ++++++++++----- 3 files changed, 31 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetLeftSpikeTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetLeftSpikeTrajectory.java index 6281758..e810a7c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetLeftSpikeTrajectory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetLeftSpikeTrajectory.java @@ -1,5 +1,8 @@ package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; + import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence; @@ -12,13 +15,14 @@ public class SetLeftSpikeTrajectory extends Node { @Override public State tick(QQOpMode opMode) { TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate()); - TrajectorySequence sequence = builder.forward(FORWARD_DISTANCE_INCHES). - strafeLeft(LEFT_DISTANCE_INCHES). - build(); - if (sequence == null){ + TrajectorySequence leftParking = opMode.robot.nav.trajectorySequenceBuilder(new Pose2d(-36.57, -61.50, Math.toRadians(90.00))) + .splineTo(new Vector2d(-43.13, -39.69), Math.toRadians(132.71)) + .build(); + + if (leftParking == null){ return State.FAILURE; } - opMode.board.add("TrajectorySequence", sequence); + opMode.board.add("TrajectorySequence", leftParking); return State.SUCCESS; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetMiddleSpikeTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetMiddleSpikeTrajectory.java index ced2752..130654d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetMiddleSpikeTrajectory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetMiddleSpikeTrajectory.java @@ -1,5 +1,8 @@ package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; + import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence; @@ -7,16 +10,20 @@ public class SetMiddleSpikeTrajectory extends Node { public static final int FORWARD_DISTANCE_INCHES = 20; - + TrajectorySequence Middle; @Override public State tick(QQOpMode opMode) { TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate()); - TrajectorySequence sequence = builder.forward(FORWARD_DISTANCE_INCHES). - build(); - if (sequence == null){ + + TrajectorySequence middleParking = opMode.robot.nav.trajectorySequenceBuilder(new Pose2d(-36.90, -62.65, Math.toRadians(90.00))) + .splineTo(new Vector2d(-36.57, -33.62), Math.toRadians(88.50)) + .build(); + + + if (middleParking == null){ return State.FAILURE; } - opMode.board.add("TrajectorySequence", sequence); + opMode.board.add("TrajectorySequence", middleParking); return State.SUCCESS; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetRightSpikeTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetRightSpikeTrajectory.java index cf58866..e630f52 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetRightSpikeTrajectory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetRightSpikeTrajectory.java @@ -1,5 +1,8 @@ package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; + import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence; @@ -12,13 +15,15 @@ public class SetRightSpikeTrajectory extends Node { @Override public State tick(QQOpMode opMode) { TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate()); - TrajectorySequence sequence = builder.forward(FORWARD_DISTANCE_INCHES). - strafeRight(RIGHT_DISTANCE_INCHES). - build(); - if (sequence == null){ + TrajectorySequence rightParking = opMode.robot.nav.trajectorySequenceBuilder(new Pose2d(-36.90, -59.86, Math.toRadians(90.00))) + .splineTo(new Vector2d(-30.01, -39.03), Math.toRadians(44.05)) + .build(); + + + if (rightParking == null){ return State.FAILURE; } - opMode.board.add("TrajectorySequence", sequence); + opMode.board.add("TrajectorySequence", rightParking); return State.SUCCESS; } } From 11f0b934eb797efd5b3579c81238cb021a3c9226 Mon Sep 17 00:00:00 2001 From: Alan Wong Date: Mon, 11 Dec 2023 18:17:40 -0500 Subject: [PATCH 11/12] added parking trajectories --- .../SetLeftSpikeToBackBoardTrajectory.java | 33 +++++++++++++++++++ .../SetMiddleSpikeToBackBoardTrajectory.java | 33 +++++++++++++++++++ .../SetRightSpikeToBackBoardTrajectory.java | 32 ++++++++++++++++++ 3 files changed, 98 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetLeftSpikeToBackBoardTrajectory.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetMiddleSpikeToBackBoardTrajectory.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetRightSpikeToBackBoardTrajectory.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetLeftSpikeToBackBoardTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetLeftSpikeToBackBoardTrajectory.java new file mode 100644 index 0000000..cfe030f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetLeftSpikeToBackBoardTrajectory.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; +import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequenceBuilder; + +public class SetLeftSpikeToBackBoardTrajectory extends Node { + public static final int FORWARD_DISTANCE_INCHES = 20; + public static final int RIGHT_DISTANCE_INCHES = 5; + + @Override + public State tick(QQOpMode opMode) { + TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate()); + TrajectorySequence leftSpikeToParking = opMode.robot.nav.trajectorySequenceBuilder(new Pose2d(-39.53, -40.67, Math.toRadians(130.73))) + .splineTo(new Vector2d(-31.33, -55.11), Math.toRadians(-31.81)) + .splineTo(new Vector2d(-6.89, -59.54), Math.toRadians(0.00)) + .splineTo(new Vector2d(64.29, -60.68), Math.toRadians(-0.92)) + .build(); + + + + + if (leftSpikeToParking== null){ + return State.FAILURE; + } + opMode.board.add("TrajectorySequence", leftSpikeToParking); + return State.SUCCESS; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetMiddleSpikeToBackBoardTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetMiddleSpikeToBackBoardTrajectory.java new file mode 100644 index 0000000..340eb8f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetMiddleSpikeToBackBoardTrajectory.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; +import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequenceBuilder; + +public class SetMiddleSpikeToBackBoardTrajectory extends Node { + public static final int FORWARD_DISTANCE_INCHES = 20; + public static final int RIGHT_DISTANCE_INCHES = 5; + + @Override + public State tick(QQOpMode opMode) { + TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate()); + TrajectorySequence middleSpikeToParkingTrajectory = opMode.robot.nav.trajectorySequenceBuilder(new Pose2d(-36.25, -37.72, Math.toRadians(92.49))) + .splineTo(new Vector2d(-36.74, -57.73), Math.toRadians(-12.77)) + .splineTo(new Vector2d(-3.61, -59.54), Math.toRadians(-3.12)) + .splineTo(new Vector2d(61.34, -60.52), Math.toRadians(-0.40)) + .build(); + + + + + if (middleSpikeToParkingTrajectory== null){ + return State.FAILURE; + } + opMode.board.add("TrajectorySequence", middleSpikeToParkingTrajectory); + return State.SUCCESS; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetRightSpikeToBackBoardTrajectory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetRightSpikeToBackBoardTrajectory.java new file mode 100644 index 0000000..c39cec9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SetRightSpikeToBackBoardTrajectory.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; + +import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; +import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.rr_trajectorysequence.TrajectorySequenceBuilder; + +public class SetRightSpikeToBackBoardTrajectory extends Node { + public static final int FORWARD_DISTANCE_INCHES = 20; + public static final int RIGHT_DISTANCE_INCHES = 5; + + @Override + public State tick(QQOpMode opMode) { + TrajectorySequenceBuilder builder = opMode.robot.nav.trajectorySequenceBuilder(opMode.robot.nav.getPoseEstimate()); + TrajectorySequence rightSpikeToParking = opMode.robot.nav.trajectorySequenceBuilder(new Pose2d(-29.19, -35.26, Math.toRadians(55.76))) + .splineTo(new Vector2d(-33.95, -53.47), Math.toRadians(-56.15)) + .splineTo(new Vector2d(-22.47, -59.86), Math.toRadians(-2.57)) + .splineTo(new Vector2d(59.70, -60.36), Math.toRadians(-1.08)) + .build(); + + + + if (rightSpikeToParking == null){ + return State.FAILURE; + } + opMode.board.add("TrajectorySequence", rightSpikeToParking); + return State.SUCCESS; + } +} From bd0a05659152d0b35ccc8e07d5fbb23a40e93d95 Mon Sep 17 00:00:00 2001 From: mahin Date: Sat, 27 Jan 2024 16:47:12 -0500 Subject: [PATCH 12/12] Cycle and Park tree --- .../teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java index 17a3616..ac1c816 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/CycleandPark.java @@ -34,4 +34,5 @@ public class CycleandPark { | | | | [setRedPlacingTrajectory] | | | [setBluePlacingTrajectory] | | [FollowTrajectory] - */ \ No newline at end of file + */ +// \ No newline at end of file