diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java index 633d201..4973f2f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/DriveToScorePosition.java +++ b/TeamCode/src/main/java/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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java index 42e582f..682d432 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/Cycle.java +++ b/TeamCode/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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/FourSpecimenTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/FourSpecimenTree.java index a14827d..a703865 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/FourSpecimenTree.java +++ b/TeamCode/src/main/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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenTree.java index 918ecfe..fa7e1bf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/SpecimenTree.java +++ b/TeamCode/src/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 index bea0475..ff32902 100644 --- 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 index 22f7dae..10d1b1c 100644 --- 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();