Changes for remote league meet #1.

HF-Robotics · Dec 22, 2020 · d35c574 · d35c574
1 parent d3e5cbd
commit d35c574
Showing 1 changed file with 62 additions and 4 deletions.
diff --git a/TeamCode/src/main/java/com/hfrobots/tnt/season1920/GrungyUltimateGoalAuto.java b/TeamCode/src/main/java/com/hfrobots/tnt/season1920/GrungyUltimateGoalAuto.java
@@ -25,6 +25,7 @@ of this software and associated documentation files (the "Software"), to deal
 import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
 import com.ftc9929.corelib.control.DebouncedButton;
 import com.ftc9929.corelib.control.NinjaGamePad;
+import com.ftc9929.corelib.control.OnOffButton;
 import com.ftc9929.corelib.control.RangeInput;
 import com.ftc9929.corelib.state.State;
 import com.ftc9929.corelib.state.StateMachine;
@@ -45,6 +46,19 @@ of this software and associated documentation files (the "Software"), to deal
 public class GrungyUltimateGoalAuto extends OpMode {
     private Ticker ticker;
 
+    private OnOffButton unsafe = new OnOffButton() {
+        @Override
+        public boolean isPressed() {
+            return true;
+            //Danger Will Robinson
+        }
+
+        @Override
+        public DebouncedButton debounced() {
+            return null;
+        }
+    };
+
     private RoadRunnerMecanumDriveREV driveBase;
 
     private StateMachine stateMachine;
@@ -112,6 +126,8 @@ public void init() {
 
         deliveryMechanism = new DeliveryMechanism(simplerHardwareMap, telemetry, ticker);
 
+        deliveryMechanism.setUnsafe(unsafe);
+
         skystoneGrabber = new SkystoneGrabber(simplerHardwareMap);
 
         capstoneMechanism = new CapstoneMechanism(simplerHardwareMap, telemetry, ticker);
@@ -311,7 +327,7 @@ protected Trajectory createTrajectory() {
             }
         };
 
-        State depositWobbleGoalState = newDelayState("Deposit Wobble Goal", 4);
+        State wobbleGoalWaitState = newDelayState("Deposit Wobble Goal", 4);
 
         State toParkedPosition = new TrajectoryFollowerState("Parking",
                 telemetry, driveBase, ticker, TimeUnit.SECONDS.toMillis(20 * 1000)) {
@@ -321,7 +337,7 @@ protected Trajectory createTrajectory() {
 
                 switch (target) {
                     case A:
-                        trajectoryBuilder.forward(8);
+                        trajectoryBuilder.strafeLeft(22).forward(12);
                         break;
                     case B:
                         trajectoryBuilder.back(14);
@@ -336,11 +352,53 @@ protected Trajectory createTrajectory() {
         };
 
         // toParkedPosition
-        // depositWobbleGoalState
+        // ejectWobbleGoalState
+        // wobbleGoalWaitState
+        // wobbleGoalCoolDownState
+
         // toTargetZone
 
+        State ejectWobbleGoalState = new State("Eject wobble goal", telemetry) {
+
+            @Override
+            public State doStuffAndGetNextState() {
+                deliveryMechanism.setIntakeVelocity(-.2);
+                return nextState;
+            }
+
+            @Override
+            public void resetToStart() {
+
+            }
+
+            @Override
+            public void liveConfigure(NinjaGamePad gamePad) {
+
+            }
+        };
+
+        State wobbleGoalCoolDownState = new State("Stop the delivey Mech",telemetry){
+            @Override
+            public State doStuffAndGetNextState() {
+                deliveryMechanism.setIntakeVelocity(0);
+                return nextState;
+            }
+
+            @Override
+            public void resetToStart() {
+
+            }
+
+            @Override
+            public void liveConfigure(NinjaGamePad gamePad) {
+
+            }
+        };
+
         stateMachine.addSequential(toTargetZone);
-        stateMachine.addSequential(depositWobbleGoalState);
+        stateMachine.addSequential(ejectWobbleGoalState);
+        stateMachine.addSequential(wobbleGoalWaitState);
+        stateMachine.addSequential(wobbleGoalCoolDownState);
         stateMachine.addSequential(toParkedPosition);
         stateMachine.addSequential(newDoneState("Done!"));
     }