Testing 2nd wobble goal

FTC7393 · Apr 17, 2021 · b655097 · b655097
1 parent 78eee48
commit b655097
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/...c/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/GameChangersAutonomous.java b/...c/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/GameChangersAutonomous.java
@@ -355,7 +355,11 @@ public StateMachine buildStates() {
             b.addDrive(S.DRIVE_FORWARD_1, S.DRIVE_FORWARD_2, Distance.fromFeet(1.5), 1.0, 275, 0);
             b.addDrive(S.DRIVE_FORWARD_2, S.DRIVE_FORWARD_3, Distance.fromFeet(.32), 0.5, 180, 0);
             b.addDrive(S.DRIVE_FORWARD_3, S.WAIT_FOR_DROP_5, Distance.fromFeet(1.55), 0.7, 255, 0);
-            b.addWait(S.WAIT_FOR_DROP_5, S.DROP_WOBBLE_GOAL_5, wobbleGoalWaitTime);
+            b.addWait(S.WAIT_FOR_DROP_5, S.MOVE_ARM_DOWN_8, wobbleGoalWaitTime);
+            b.add(S.MOVE_ARM_DOWN_8, () -> {
+                robotCfg.getWobbleGoalArm().moveArmDown();
+                return S.DROP_WOBBLE_GOAL_5;
+            });
             b.addServo(S.DROP_WOBBLE_GOAL_5, S.MOVE_ARM_UP_5, robotCfg.getPincher().getName(), ServoPresets.WobblePincher.OPENED, servoReleaseWaitTime,true);
               b.add(S.MOVE_ARM_UP_5, () -> {
                 robotCfg.getWobbleGoalArm().moveArmUp();
@@ -764,7 +768,7 @@ public enum S implements StateName {
         BLUE_VUFORIA_EXPLORE, BLUE_WAIT, BLUE_DRIVE_1C, BLUE_DRIVE_1B, BLUE_DRIVE_1, SET_VUCALC, WAIT_FOR_VUFORIA_INIT, ACTIVATE_TARGETS,
         DEACTIVATE_TARGETS, BLUE_SET_VUCALC, BLUE_TARGETS_ACTIVATE, BLUE_WAIT_FOR_VUFORIA_INIT, BLUE_DEACTIVATE_TARGETS, ELEVATE_SHOOTER, BLUE_START_FLYWHEEL,
         BLUE_WOBBLE_TURN, BLUE_WOBBLE_TURN_1, BLUE_WOBBLE_TURN_4, AFTER_PARK, PARK_0_A, PARK_1_A, PARK_4_A, BLUE_PARK_0_A, BLUE_PARK_1_A, BLUE_PARK_4_A, BLUE_PARK_4C,
-        SECOND_RING_COLLECTION, DRIVE_BACKWARDS_TO_COLLECT, TURN_ON_COLLECTOR, TURN_OFF_COLLECTOR, ACTIVATE_TARGETS_2, VUFORIA_LINEUP, RETURN_TO_SHOOT, SET_VUCALC_2, START_FLYWHEEL_2, DEACTIVATE_TARGETS_2, TURN_OFF_SHOOTER_2, SHOOT_RINGS_2, TURN_AIM_SHOOT_2, DRIVE_SLOW_SHOOT, THREE_RING_COLLECTION, BUMP_RING_STACK, TURN_ON_COLLECTOR_B, PAUSE_1, THREE_RING_COLLECTION_1, THREE_RING_COLLECTION_2, PAUSE_2, THREE_RING_COLLECTION_3, PAUSE_3, RETURN_FROM_FOUR_RINGS, SET_SHOOTER_SERVO, WAIT_COLLECT, BLUE_SECOND_RING_COLLECTION, BLUE_TURN_ON_COLLECTOR, BLUE_BUMP_RING_STACK, BLUE_DRIVE_BACKWARDS_TO_COLLECT, BLUE_RETURN_TO_SHOOT, BLUE_TURN_OFF_COLLECTOR, BLUE_WAIT_COLLECT, BLUE_TURN_ON_COLLECTOR_B, BLUE_THREE_RING_COLLECTION_1, BLUE_PAUSE_1, BLUE_THREE_RING_COLLECTION_2, BLUE_PAUSE_2, BLUE_RETURN_FROM_FOUR_RINGS, BLUE_SET_VUCALC_2, BLUE_ACTIVATE_TARGETS_2, BLUE_VUFORIA_LINEUP, BLUE_DEACTIVATE_TARGETS_2, BLUE_START_FLYWHEEL_2, BLUE_TURN_AIM_SHOOT_2, BLUE_SET_SHOOTER_SERVO, BLUE_SHOOT_RINGS_2, BLUE_TURN_OFF_SHOOTER_2, DRIVE_BACK, DRIVE_BACK_2, DRIVE_BACK_3, WOBBLE_DRIVE, PICKUP_WOBBLE, DRIVE_BACK_5, DRIVE_FORWARD_1, DRIVE_FORWARD_2, DRIVE_FORWARD_3, MOVE_ARM_DOWN_5, WAIT_FOR_DROP_5, DROP_WOBBLE_GOAL_5, MOVE_ARM_UP_5, MOVE_ARM_UP_6, WAIT_FOR_DROP_6, MOVE_ARM_DOWN_7, MOVE_ARM_UP_7, OPENCV_INIT
+        SECOND_RING_COLLECTION, DRIVE_BACKWARDS_TO_COLLECT, TURN_ON_COLLECTOR, TURN_OFF_COLLECTOR, ACTIVATE_TARGETS_2, VUFORIA_LINEUP, RETURN_TO_SHOOT, SET_VUCALC_2, START_FLYWHEEL_2, DEACTIVATE_TARGETS_2, TURN_OFF_SHOOTER_2, SHOOT_RINGS_2, TURN_AIM_SHOOT_2, DRIVE_SLOW_SHOOT, THREE_RING_COLLECTION, BUMP_RING_STACK, TURN_ON_COLLECTOR_B, PAUSE_1, THREE_RING_COLLECTION_1, THREE_RING_COLLECTION_2, PAUSE_2, THREE_RING_COLLECTION_3, PAUSE_3, RETURN_FROM_FOUR_RINGS, SET_SHOOTER_SERVO, WAIT_COLLECT, BLUE_SECOND_RING_COLLECTION, BLUE_TURN_ON_COLLECTOR, BLUE_BUMP_RING_STACK, BLUE_DRIVE_BACKWARDS_TO_COLLECT, BLUE_RETURN_TO_SHOOT, BLUE_TURN_OFF_COLLECTOR, BLUE_WAIT_COLLECT, BLUE_TURN_ON_COLLECTOR_B, BLUE_THREE_RING_COLLECTION_1, BLUE_PAUSE_1, BLUE_THREE_RING_COLLECTION_2, BLUE_PAUSE_2, BLUE_RETURN_FROM_FOUR_RINGS, BLUE_SET_VUCALC_2, BLUE_ACTIVATE_TARGETS_2, BLUE_VUFORIA_LINEUP, BLUE_DEACTIVATE_TARGETS_2, BLUE_START_FLYWHEEL_2, BLUE_TURN_AIM_SHOOT_2, BLUE_SET_SHOOTER_SERVO, BLUE_SHOOT_RINGS_2, BLUE_TURN_OFF_SHOOTER_2, DRIVE_BACK, DRIVE_BACK_2, DRIVE_BACK_3, WOBBLE_DRIVE, PICKUP_WOBBLE, DRIVE_BACK_5, DRIVE_FORWARD_1, DRIVE_FORWARD_2, DRIVE_FORWARD_3, MOVE_ARM_DOWN_5, WAIT_FOR_DROP_5, DROP_WOBBLE_GOAL_5, MOVE_ARM_UP_5, MOVE_ARM_UP_6, WAIT_FOR_DROP_6, MOVE_ARM_DOWN_7, MOVE_ARM_UP_7, MOVE_ARM_DOWN_8, OPENCV_INIT
     }