TWO SPECIMEN

TWO SPECIMEN RIGHT AUTO
GearUp12499-org · Dec 22, 2024 · b70699d · b70699d
1 parent 39252a5
commit b70699d
Show file tree

Hide file tree

Showing 2 changed files with 46 additions and 15 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java
@@ -96,6 +96,7 @@ private MoveToTask moveTo(Pose target) {
                 scheduler, hardware, target, tracker, loopTimer, speed2Power, ramps, telemetry
         );
     }
+
     private MoveToTask moveToSlow(Pose target) {
         return new MoveToTask(
                 scheduler, hardware, target, tracker, loopTimer, speed2Power, rampsSlowEdition, telemetry
@@ -113,7 +114,7 @@ private BlinkLightsTask blinkenlights(double seconds) {
     private BlinkLightsTask lightColor(double color) {
         return new BlinkLightsTask(
                 scheduler, hardware,
-                0.0 , false, 0,
+                0.0, false, 0,
                 0, 0, color
         );
     }
@@ -234,29 +235,35 @@ private ITask drop() {
         );
     }
 
-    private ITask scoreSpecimen() {
+    private ITask preScoreSpecimen() {
         return groupOf(it -> it.add(run(() -> hardware.claw.setPosition(Hardware.CLAW_CLOSE)))
                 .then(run(() -> hardware.arm.setTargetPosition(Hardware.deg2arm(-99))))
-                .then(vLiftProxy.moveTo(710, 5, 1.0))
-                .then(run(() -> hardware.wrist.setPosition(1)))
-                .then(await(300))
-                .then(moveRel(new Pose(-4.0, 0, 0)))
-                .then(run(() -> hardware.claw.setPosition(Hardware.CLAW_OPEN)))
-                .then(await(200))
-                .then(run(() -> {
+                .then(vLiftProxy.moveTo(710, 5, 1.0)));
+    }
+
+    private ITask postScoreSpecimen() {
+        return groupOf(it -> it.add(run(() -> {
                     hardware.wrist.setPosition(0.28);
                     hardware.arm.setTargetPosition(Hardware.deg2arm(0));
                 }))
                 .then(await(450))
-                .then(vLiftProxy.moveTo(0, 5, .25))
+                .then(vLiftProxy.moveTo(0, 5, .25)));
+    }
+
+    private ITask scoreSpecimen() {
+        return groupOf(it -> it.add(run(() -> hardware.wrist.setPosition(1)))
+                .then(await(300))
+                .then(moveRel(new Pose(-4.0, 0, 0)))
+                .then(run(() -> hardware.claw.setPosition(Hardware.CLAW_OPEN)))
+                .then(await(200))
         );
     }
 
     private ITask pickSpecimen() {
         return groupOf(it -> it.add(run(() -> hardware.claw.setPosition(Hardware.CLAW_OPEN)))
                         .then(await(200))
                         .then(run(() -> hardware.wrist.setPosition(Hardware.WRIST_UP)))
-                        .then(await(500))
+                        .then(await(200))
                         .then(run(() -> hardware.arm.setTargetPosition(50)))
                         .then(await(500))
                         .then(run(() -> hardware.claw.setPosition(Hardware.CLAW_CLOSE)))
@@ -272,9 +279,15 @@ private ITask pickSpecimen() {
 
     public void runAuto() {
         scheduler.add(new OneShot(scheduler, setup))
-                .then(moveTo(new Pose(30, 12, 0)))
+                .then(groupOf(a -> {
+                    a.add(moveTo(new Pose(30, 12, 0)));
+                    a.add(preScoreSpecimen());
+                }))
                 .then(scoreSpecimen())
-                .then(moveTo(new Pose(15.5, -36, 0)))
+                .then(groupOf(a -> {
+                    a.add(moveTo(new Pose(15.5, -36, 0)));
+                    a.add(postScoreSpecimen());
+                }))
                 .then(grab())
                 .then(groupOf(a -> {
                     a.add(transfer())
@@ -288,7 +301,25 @@ public void runAuto() {
                             .then(drop());
                     a.add(moveTo(new Pose(11.5, -46.25, 0)));
                 }))
-                .then(moveTo(new Pose(6, -27, 0)));
+//                .then(moveTo(new Pose(14, -27, 0)))
+//                .then(await(500))
+                .then(moveTo(new Pose(6, -27, 0)))
+                .then(blinkenlights(1.0))
+                .then(moveToSlow(new Pose(2, -27, 0)))
+                .then(run(() -> hardware.driveMotors.setAll(-0.40)))
+                .then(await(500))
+                .then(run(() -> hardware.driveMotors.setAll(0)))
+                .then(pickSpecimen())
+                .then(lightColor(Hardware.LAMP_PURPLE))
+                .then(groupOf(a -> {
+                    a.add(moveTo(new Pose(30, 6, 0)));
+                    a.add(preScoreSpecimen());
+                }))
+                .then(scoreSpecimen())
+                .then(groupOf(a -> {
+                    a.add(moveTo(new Pose(6, -27, 0)));
+                    a.add(postScoreSpecimen());
+                }));
     }
 
     @Override

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/VLiftProxy.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/VLiftProxy.java
@@ -126,7 +126,7 @@ public boolean invokeIsCompleted() {
     public ITaskWithResult<Boolean> moveTo(int target, int range, double maxDuration) {
         ITaskWithResult<Boolean> result;
         // This version has a timer
-        if (maxDuration > 0) result = new TaskWithResultTemplate<Boolean>(scheduler) {
+        if (maxDuration >= 0) result = new TaskWithResultTemplate<Boolean>(scheduler) {
             private final ElapsedTime t = new ElapsedTime();
 
             @Override