package com.team254.frc2013.commands;

/**
 * Checks the pitch of the robot and waits for it to be swinging, indicating that the robot is off
 * the ground.
 *
 * @author pat@team254.com (Patrick Fairbank)
 */
public class WaitForLiftCommand extends CommandBase {
  public WaitForLiftCommand() {
  }
  protected void initialize() {
    drive.shift(true);
  }

  protected void execute() {
    System.out.println("Waiting for lift...");
  }

  protected boolean isFinished() {
    return Math.abs(hanger.getPitchAngle()) > 2;
  }

  protected void end() {
    System.out.println("Off the ground; okay for hanger extension.");
  }

  protected void interrupted() {
  }
}

@@ -32,16 +32,53 @@
 mands;%0A%0A
+import edu.wpi.first.wpilibj.Timer;%0A%0A
 /**%0A * C
@@ -294,16 +294,62 @@
 dBase %7B%0A
+  Timer t = new Timer();%0A  boolean up = true;%0A
   public
@@ -404,24 +404,54 @@
 tialize() %7B%0A
+    t.reset();%0A    t.start();%0A
     drive.sh
@@ -538,24 +538,125 @@
  lift...%22);%0A
+    if (t.get() %3E .5) %7B%0A      up = !up;%0A    %7D%0A    sc.wantIntakeUp = up;%0A    sc.wantIntakeDown = !up;%0A
   %7D%0A%0A  prote
@@ -758,24 +758,24 @@
 oid end() %7B%0A
-
     System.o
@@ -832,16 +832,80 @@
 ion.%22);%0A
+        sc.wantIntakeUp = false;%0A    sc.wantIntakeDown = false;%0A
   %7D%0A%0A  p

make intake shake for hang