messed around with auto posistions

ftc16072 · Feb 14, 2025 · 991f3fb · 991f3fb
1 parent 84be6b8
commit 991f3fb
Show file tree

Hide file tree

Showing 6 changed files with 21 additions and 12 deletions.
diff --git a/...a/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java b/...a/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java
@@ -6,8 +6,10 @@
 import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
 
 public class DriveToScorePosition extends QQTimeoutNode {
-    public DriveToScorePosition(double seconds) {
+    double xPosition;
+    public DriveToScorePosition(double seconds, double xPosition) {
         super(seconds);
+        this.xPosition = xPosition;
     }
     State lastStatus = State.RUNNING;
 
@@ -18,7 +20,7 @@ public State tick(DebugTree debug, QQOpMode opMode) {
         if (lastStatus != State.RUNNING){
             return lastStatus;
         }else{
-           boolean isDoneDriving = opMode.nav.driveToPositionIN(10,50,0,5);
+           boolean isDoneDriving = opMode.nav.driveToPositionIN(xPosition,50,0,4);
         if (isDoneDriving) {
             lastStatus = State.SUCCESS;
             return State.SUCCESS;

diff --git a/...Code/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java b/...Code/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java
@@ -19,12 +19,14 @@
 public class Cycle {
     public static final int TIMEOUT_SECONDS = 10;
 
-    public static Node root(){
+
+
+    public static Node root(double xScorePosition){
         return new Sequence(
                 Intake.root(),
                 new MoveForwardForTime(.1,-1),
                 new Parallel(2,
-                        new DriveToScorePosition(TIMEOUT_SECONDS),
+                        new DriveToScorePosition(TIMEOUT_SECONDS, xScorePosition),
                         new ArmToScore(TIMEOUT_SECONDS)),
                 new StandardScore(TIMEOUT_SECONDS),
                 new MoveForwardForTime(0.25,1),

diff --git a/...in/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/FourSpecimenTree.java b/...in/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/FourSpecimenTree.java
@@ -23,10 +23,10 @@ public static Node root(){
                         new Parallel(2,
                                 new DriveToIntakePosition(TIMEOUT_SECONDS),
                                 new ArmToIntake(TIMEOUT_SECONDS)),
-                        Cycle.root(),
-                        Cycle.root(),
-                        Cycle.root(),
-                        Cycle.root(),
+                        Cycle.root(10),
+                        Cycle.root(10),
+                        Cycle.root(10),
+                        Cycle.root(10),
                         new Parallel(2,
                                 new Park(TIMEOUT_SECONDS),
                                 new ArmToIntake(TIMEOUT_SECONDS))

diff --git a/...c/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenTree.java b/...c/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenTree.java
@@ -28,6 +28,7 @@ public static Node root(){
         return new Failover(
                 new Sequence(
                         new FirstScore(TIMEOUT_SECONDS),
+                        new MoveForwardForTime(0.25,1),
                         new Parallel(2,
                             new ArmToIntake(TIMEOUT_SECONDS),
                             new GetReadyToPushSamples(TIMEOUT_SECONDS)
@@ -41,9 +42,9 @@ public static Node root(){
                        /* new PushSecondSample(TIMEOUT_SECONDS),
                         new MoveRightForTime(.2,1), //square on wall
                         new PushSamplesIn(TIMEOUT_SECONDS),*/
-                        Cycle.root(),
-                        Cycle.root(),
-                        Cycle.root(),
+                        Cycle.root(4),
+                        Cycle.root(12),
+                        Cycle.root(12),
                         //Cycle.root(),
                         new Parallel(2,
                                 new Park(TIMEOUT_SECONDS),

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java
@@ -22,7 +22,7 @@
 @Config
 public class ScoringClaw extends QQMechanism {
     public static final int GRABBABLE_DISTANCE_CM = 3;
-    public static final double OPEN_TIME = 0.15;
+    public static final double OPEN_TIME = 0.2;
     public static final double CLOSED_TIME = 0.25;
     public static double CLAW_CLOSE_POSITION = 0.5;
     public static double CLAW_OPEN_POSITION = 0;
@@ -87,6 +87,9 @@ public void close() {
         clawServo.setPosition(CLAW_CLOSE_POSITION);
         closedTimer.reset();
     }
+    public void resetHasColor(){
+        hasColor = false;
+    }
 
     public boolean isColorRed(){
         if (!hasColor){

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/Auto.java
@@ -19,6 +19,7 @@ public class Auto extends QQOpMode{
     double INIT_MOVE_SPEED = -0.2;
     @Override
     public void init(){
+        robot.scoringClaw.resetHasColor();
         super.init();
         moveTimer.reset();
         robot.scoreArm.goToInit();