public class CenterDiscMiddleAutoMode extends CommandGroup {

  public CenterDiscMiddleAutoMode(int numLastDiscs) {
    addSequential(new CenterDiscPartialAutoMode());

    // Drive to back of pyramid

    addSequential(new TurnMinAngleCommand(15, 1.25));
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new RunIntakeCommand(0));
    addSequential(new ShooterOnCommand(true));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new DriveProfiledCommand(8.9, 6.4, 15, 3));

    // Shoot last discs
    for (int i = 0; i < numLastDiscs; ++i) {
      addSequential(new ShootSequenceCommand());
    }
    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }

  public CenterDiscMiddleAutoMode() {
    this(5);
  }
} public class CenterDiscSideAutoMode extends CommandGroup {

  public CenterDiscSideAutoMode() {
    addSequential(new CenterDiscPartialAutoMode());

    // Return to starting position
    addSequential(new TurnMinAngleCommand(-112, .5));
    addSequential(new RunIntakeCommand(0));
    addSequential(new ShiftCommand(true));
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new DriveProfiledCommand(-8.75, 10, -112, 1.75));
   // addSequential(new DriveAtSpeedCommand(-11, -15, -112, 10));
    addSequential(new ShiftCommand(false));
    addSequential(new ResetDriveEncodersCommand());
    //addSequential(new DriveAtSpeedCommand(.25, 5, -112, .35));
    addSequential(new ShooterOnCommand(true));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new TurnAngleCommand(10, .5));
    addParallel(new DriveProfiledCommand(8, 7, 10, 10));
    addSequential(new WaitCommand(.6));

    // Shoot remaining discs
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());

    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }
} public class CenterLineBlockAutoMode extends CommandGroup {

  public CenterLineBlockAutoMode() {
    // Shoot first 3
    addSequential(new ShooterOnCommand(true));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new SetIntakeUpCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand(false));
    addSequential(new ShooterOnCommand(false));

    // Drive to center line
    addSequential(new WaitCommand(.25));
    addSequential(new CheckIntakeCalibratedCommand(.5));
    addSequential(new ShiftCommand(false));
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new DriveAtSpeedCommand(-8.5, -6, 0, 5));
    addSequential(new DriveAtSpeedCommand(-8, 6, 0, 0.1));
    addSequential(new TurnMinAngleCommand(95, 5));

    // Drive forward along center line to pick up discs
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new DriveProfiledCommand(-8.5, 6, 100, 10, false));
  }
} public class DriveMotorTestAutoMode extends CommandGroup {
  public DriveMotorTestAutoMode() {
    addSequential(new DriveMotorTestCommand());
  }
} public class EightDiscAutoMode extends CommandGroup {

  CustomProfile profile = new CustomProfile();

  public EightDiscAutoMode() {
    // Shoot first discs
    addSequential(new SetIntakeDownCommand());
    addSequential(new ShooterOnCommand(true));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new ShiftCommand(false));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand(false));

    // Pick up 2 middle discs
    addSequential(new CheckIntakeCalibratedCommand(.5));
    addSequential(new WaitCommand(.35));
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new RunIntakeCommand(1));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_FRONT_PYRAMID));

    // Drive to front of pyramid and shoot 2
    addSequential(new DriveProfiledCommand(5.75, 2.75, 0, 5));
    addSequential(new RunIntakeCommand(0.0));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand(false));

    // Pick up 2 far discs
    addSequential(new RunIntakeCommand(1.0, true));
    addSequential(new DriveAtSpeedCommand(10.5, 2.0, 0, 4.6));

    // Drive to front of pyramid and shoot 2
    addSequential(new DriveProfiledCommand(5.75, 5, 0, 2.5));
    addSequential(new RunIntakeCommand(0.0));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());

    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }
} public class FiveDiscAutoMode extends CommandGroup {

  public FiveDiscAutoMode() {
    // Shoot first set of discs
    addSequential(new ShooterOnCommand(true));
    addSequential(new SetIntakeDownCommand());
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new ShiftCommand(false));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());

    // Drive to get second set
    addSequential(new CheckIntakeCalibratedCommand(.5));
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new RunIntakeCommand(1));
    addSequential(new DriveAtSpeedCommand(2, 2, 0, 5));
    addSequential(new WaitCommand(1));

    // Drive back and shoot
    addSequential(new DriveProfiledCommand(0, 5, 0, 5));
    addSequential(new RunIntakeCommand(0.0));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());

    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }
} public class FourDiscAutoMode extends CommandGroup {

  public FourDiscAutoMode() {
    // Shoot first set of discs
    addSequential(new ShooterOnCommand(true));
    addSequential(new SetIntakeDownCommand());
    addSequential(new ShooterPresetCommand(Shooter.PRESET_FRONT_PYRAMID));
    addSequential(new ShiftCommand(false));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());

    // Drive to get second set
    addSequential(new CheckIntakeCalibratedCommand(.5));
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new RunIntakeCommand(1));
    addSequential(new DriveAtSpeedCommand(3.5, 2, 0, 5));
    addSequential(new WaitCommand(1));

    // Drive back and shoot
    addSequential(new DriveProfiledCommand(0, 5, 0, 5));
    addSequential(new RunIntakeCommand(0.0));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());

    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }
} public class SevenDiscAutoMode extends CommandGroup {

  CustomProfile profile = new CustomProfile();

  public SevenDiscAutoMode() {
    // Shoot first discs
    addSequential(new ShooterOnCommand(true));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new SetIntakeDownCommand());
    addSequential(new ShiftCommand(false));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand(false));

    // Pick up 2 middle discs
    addSequential(new CheckIntakeCalibratedCommand(.5));
    addSequential(new WaitCommand(.35));
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new RunIntakeCommand(1));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_FRONT_PYRAMID));

    // Drive to front of pyramid and shoot 2
    addSequential(new DriveProfiledCommand(5.75, 2.75, 0, 5));
    addSequential(new RunIntakeCommand(0.0));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand(false));

    // Pick up 2 far discs
    addSequential(new RunIntakeCommand(1.0, true));
    addSequential(new DriveAtSpeedCommand(10.5, 2.0, 0, 4.6));

    // Drive to front of pyramid and shoot 2
    addSequential(new DriveProfiledCommand(5.75, 5, 0, 2.5));
    addSequential(new RunIntakeCommand(0.0));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());

    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }
} public class TestAutoMode extends CommandGroup {
  CustomProfile profile = new CustomProfile();

  public TestAutoMode() {
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new SensedIndexerDownCommand());
    addSequential(new ShiftCommand(false));
    addSequential(new RunIntakeCommand(1));
    addSequential(new DriveAtSpeedCommand(5, 1.5, 0, 5));
    addSequential(new RunIntakeCommand(1));
    addSequential(new WaitCommand(.25));
    addSequential(new RunIntakeCommand(1));
    addSequential(new DriveProfiledCommand(0, 8, 0, 2.3));
    addSequential(new RunIntakeCommand(0.0));
 }
} public class ThreeDiscAutoMode extends CommandGroup {

  public ThreeDiscAutoMode() {
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new ShooterOnCommand(true));
    addSequential(new SetIntakeDownCommand());
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new WaitCommand(1.0));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }
} public class TuneDriveAutoMode extends CommandGroup {
  public TuneDriveAutoMode() {
    addSequential(new ShiftCommand(true));
    addSequential(new TuneDriveCommand(10, 2));
  }
} public class TwoDiscAutoMode extends CommandGroup {

  public TwoDiscAutoMode() {
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new ShooterOnCommand(true));
    addSequential(new SetIntakeDownCommand());
    addSequential(new ShooterPresetCommand(Shooter.PRESET_FRONT_PYRAMID));
    addSequential(new WaitCommand(0.25));
    addSequential(new ShootSequenceCommand());
    addSequential(new ShootSequenceCommand());
    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }
} public class AutoHangCommand extends CommandGroup {

  public AutoHangCommand() {
    addSequential(new ShiftCommand(true));
    addSequential(new StopDriveCommand());
    addSequential(new WaitForLiftCommand());
    addSequential(new StowIntakeForHangCommand());
    addSequential(new EngagePtoCommand());
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new HangerGrabBarCommand(-2485, -3190, -2, 5));
    addSequential(new WaitCommand(.5));
    addSequential(new WaitForBackwardSwingCommand(-0.5));
    addSequential(new HangerDownCommand(-2980, 0.75));
    addSequential(new WaitForButtonCommand(CommandBase.controlBoard.operatorJoystick.getIntakeButton()));
    addSequential(new HangerDownCommand(-5, 1));
    addSequential(new WaitCommand(0.25));
    addSequential(new HangerGrabBarCommand(-2485, -3190, -5, 15));
    addSequential(new WaitCommand(.5));
    addSequential(new WaitForBackwardSwingCommand(-0.5));
    addSequential(new HangerDownCommand(-2980, 0.75));
    addSequential(new PtoCommand());
  }
} public abstract class CommandBase extends Command {
  public static ControlBoard controlBoard;

  // Declare a single static instance of each subsystem here.
  public static DriveGearbox motors = new DriveGearbox();
  public static Drive drive = new Drive(motors);
  public static Hanger hanger = new Hanger(motors);
  public static Shooter shooter = new Shooter();
  public static Intake intake = new Intake();
  public static Conveyor conveyor = new Conveyor();
  public static ShootController sc;
  public static Compressor compressor = new Compressor(Constants.pressureSwitch.getInt(),
                                                       Constants.compressorRelay.getInt());
  public static PressureTransducer pressureTransducer =
      new PressureTransducer(Constants.pressureTransducerPort.getInt());

  public static Timer shotTimer = new Timer();
  public static Timer autonTimer = new Timer();

  public static int rapidFireShots = 0;

  public static boolean startedAutoHang = false;

  public static void init() {
    // This MUST be here. If the OI creates Commands (which it very likely will), constructing it
    // during the construction of CommandBase (from which commands extend), subsystems are not
    // guaranteed to be yet. Thus, their requires() statements may grab null pointers.

    sc = new ShootController(shooter, conveyor, intake);

    // Set up operator controls
    controlBoard = new ControlBoard();
    OperatorControlHelper.setupOperationMap(controlBoard);
    compressor.start();
    shotTimer.start();
  }
} public class PtoCommand extends CommandBase {

  boolean reset = false;
  public PtoCommand(boolean reset) {
    requires(drive);
    requires(motors);
    this.reset = reset;
  }

  public PtoCommand() {
    this(false);
  }

  protected void initialize() {
    if (reset)
      motors.getLeftEncoder().reset();
  }

  protected void execute() {
    drive.shift(true);
    hanger.setPto(true);
    motors.set(controlBoard.leftStick.getY());
  }

  protected boolean isFinished() {
    return false;
  }

  protected void end() {
    hanger.setPto(false);
  }

  protected void interrupted() {
    hanger.setPto(false);
  }
} public class Shooter extends Subsystem {

  // Preset enumeration
  public static final int PRESET_BACK_PYRAMID = 0;
  public static final int PRESET_FRONT_PYRAMID = 1;
  public static final int SLOW_SHOOT = 2;
  // Actuators
  private Talon frontMotor = new Talon(Constants.frontShooterPort.getInt());
  private Talon backMotor = new Talon(Constants.backShooterPort.getInt());
  private Solenoid loader = new Solenoid(Constants.shooterLoaderPort.getInt());
  private Solenoid angle = new Solenoid(Constants.shooterAnglePort.getInt());
  private Solenoid indexerLeft = new Solenoid(Constants.indexerLeftPort.getInt());
  private Solenoid indexerRight = new Solenoid(Constants.indexerRightPort.getInt());
  // Sensors
  public DigitalInput indexerDownSensor =
          new DigitalInput(Constants.indexerDownSensorPort.getInt());
  public DigitalInput indexerUpSensor =
          new DigitalInput(Constants.indexerUpSensorPort.getInt());
  private DigitalInput rightDiscSensor =
          new DigitalInput(Constants.rightDiscSensorPort.getInt());
  private DigitalInput leftDiscSensor =
          new DigitalInput(Constants.leftDiscSensorPort.getInt());
  public Counter counter = new Counter(Constants.shootEncoderPort.getInt());
  // Controller helpers
  double goal = 0;

  private class ShooterSource implements ControlSource {

    public double get() {
      int kCountsPerRev = 1;
      double period = counter.getPeriod();
      double rpm = 60.0 / (period * (double) kCountsPerRev);
      if (rpm < 20000) {
        lastRpm = rpm;
      }
      return (lastRpm * Math.PI * 2.0) / 60.0;
    }

    public void updateFilter() {
    }

    public boolean getLowerLimit() {
      return false;
    }

    public boolean getUpperLimit() {
      return false;
    }
  }

  private class ShooterOutput implements ControlOutput {

    public void set(double value) {
      // Leave this for Austin usage
      //System.out.println("D:" + Timer.getFPGATimestamp() + ", " + value*12.0 + ", " + lastRpm + ", " + controller.Xhat.get(0) + ", " + controller.Xhat.get(1) + ", " + DriverStation.getInstance().getBatteryVoltage() + ":D");
      if (value > speedLimit) {
        value = speedLimit;
      }
      if (!controller.isEnabled()) {
        value = 0;
      }
      value = Util.limit(value, 1.0);

      frontMotor.set(-value);
      backMotor.set(-value);
    }
  }
  FlywheelController controller = new FlywheelController("shooter", new ShooterOutput(),
          new ShooterSource(), ShooterGains.getGains()[0], 1.0 / 100.0);
  private double frontPower;
  private double backPower;
  private boolean shooterOn;
  public double lastRpm = 0;
  private Timer stateTimer = new Timer();
  double speedLimit = 15000;

  public void setIndexerUp(boolean up) {
    indexerLeft.set(!up);
    indexerRight.set(!up);
  }

  // Load a frisbee into shooter by retracting the piston
  public void retract() {
    loader.set(false);
  }

  // Shoot already loaded frisbee by extending the piston
  public void extend() {
    loader.set(true);
  }

  public void setHighAngle(boolean high) {
    angle.set(!high);
  }

  public boolean isHighAngle() {
    return !angle.get();
  }

  public boolean getLoaderState() {
    return loader.get();
  }

  // This returns true when the solenoids are on.
  public boolean isIndexerSetDown() {
    return indexerLeft.get() && indexerRight.get();
  }

  // This returns true when the hall effect sensor is tripped.
  public boolean isIndexerSensedDown() {
    return !indexerDownSensor.get();
  }

  // This returns true when the hall effect sensor is tripped.
  public boolean isIndexerSensedUp() {
    return !indexerUpSensor.get();
  }

  public void setVelocityGoal(double rpm) {
    goal = rpm;
    controller.setVelocityGoal((goal * Math.PI * 2.0) / 60.0);
  }

  public boolean isIndexerLoaded() {
    if (Constants.testBumpSensor.getInt() == 0) {
      return !rightDiscSensor.get();
    } else { // if test bumper status is not 0, then use left bumper
      return !rightDiscSensor.get() || !leftDiscSensor.get();
    }
  }

  public Shooter() {
    super();
    setPreset(PRESET_FRONT_PYRAMID);
    shooterOn = false;
    stateTimer.start();
    counter.start();
    controller.enable();
  }

  public final void setPreset(int preset) {
    switch (preset) {
      case PRESET_FRONT_PYRAMID:
        setHighAngle(true);
        setVelocityGoal(Constants.shootRpm.getDouble());
        break;
      case SLOW_SHOOT:
        setVelocityGoal(2000);
        break;
      case PRESET_BACK_PYRAMID:
      default:
        setHighAngle(false);
        setVelocityGoal(Constants.shootRpm.getDouble());
    }
  }

  public void setSpeedLimit(double limit) {
    speedLimit = limit;
  }

  private void setPowers(double frontPower, double backPower) {
  }

  public void setShooterOn(boolean isOn) {
    shooterOn = isOn;
    if (isOn) {
      controller.enable();
    } else {
      controller.disable();
      frontMotor.set(0);
      backMotor.set(0);
      lastRpm = 0;
    }
  }

  protected void initDefaultCommand() {
  }

  public double getRpm() {
    return lastRpm;
  }
  public double getRpmGoal()
  {
    return (controller.getVelocityGoal() * 60.0) / (2 * Math.PI);
  }

  public boolean onSpeedTarget() {
    return controller.onTarget();
  }

  public boolean isOn() {
    return shooterOn;
  }

  public double getCurrentTalon() {
    return frontMotor.get();
  }
} public class ControlUpdater {
  private static ControlUpdater instance = null;
  private Vector systems = new Vector();
  private Timer controlUpdater;
  private double period = 1.0 / 100.0;

  private class UpdaterTask extends TimerTask {
    private ControlUpdater updater;

    public UpdaterTask(ControlUpdater updater) {
      if (updater == null) {
        throw new NullPointerException("Given ControlUpdater was null");
      }
      this.updater = updater;
    }

    public void run() {
      updater.update();
    }
  }

  public void update() {
    for(int i = 0; i < systems.size(); i++) {
      ((Updatable)systems.elementAt(i)).update();
    }
  }

  public static ControlUpdater getInstance() {
    if(instance == null) {
      instance = new ControlUpdater();
    }
    return instance;
  }

  public void start() {
    if(controlUpdater == null) {
      controlUpdater = new Timer();
      controlUpdater.schedule(new UpdaterTask(this), 0L, (long) (this.period * 1000));
    }
  }

  public void stop() {
    if(controlUpdater != null) {
      controlUpdater.cancel();
      controlUpdater = null;
    }
  }

  public void add(Updatable system) {
    systems.addElement(system);
  }

  public void setPeriod(double period) {
    this.period = period;
    stop();
    start();
  }
} public class Debouncer {
  Timer t = new Timer();
  double time;
  boolean first = true;

  public Debouncer(double time) {
    this.time = time;
  }

  public boolean update(boolean val) {
    if (first) {
      first = false;
      t.start();
    }
    if(!val) {
      t.reset();
    }
    return t.get() > time;
  }

  public void reset() {
    t.reset();
  }
} public class Latch {
  private boolean lastVal;

  public boolean update(boolean newVal) {
    boolean result = newVal && !lastVal;
    lastVal = newVal;
    return result;
  }
} public class PIDTuner implements Runnable {
  private static PIDTuner instance = null;
  private final int PORT_NUMBER = 41234;
  private Vector connections = new Vector();
  Thread thread;
  boolean running = false;

  public void start() {
    if (thread == null) {
      running = true;
      thread = new Thread(this);
      thread.start();
    }
  }

  public static PIDTuner getInstance() {
    if (instance == null) {
      instance = new PIDTuner();
    }
    return instance;
  }

  synchronized public void pushData(double setpoint, double value, double control) {
    String msg = "{\"S\":" + setpoint + ", \"V\":"  + value + ", \"C\":" + control + "}";
    for (int i = 0; i < connections.size(); ++i) {
      Connection c = (Connection) connections.elementAt(i);
      try {
        c.sendData(msg);
      } catch (IOException ex) {
        ex.printStackTrace();
        connections.removeElementAt(i);
      }
    }
  }

  public void run() {
    ServerSocketConnection socket = null;

    try {
      socket = (ServerSocketConnection) Connector.open("serversocket://:" + PORT_NUMBER);
      while(true) {
        SocketConnection socketConnect = (SocketConnection) socket.acceptAndOpen();
        System.out.println("Got a new socket connection");
        Connection c = new Connection(socketConnect);
        connections.addElement(c);
      }
    } catch (IOException e) {
       System.out.println("ERROR: " + e.getMessage());
    }
  }

  private class Connection {
    private SocketConnection client;
    OutputStream output = null;

    public Connection(SocketConnection client) {
      this.client = client;
      try {
        output = client.openOutputStream();
        System.out.println(output);
      } catch (IOException ex) {
        ex.printStackTrace();
      }
    }

    synchronized public void sendData(String data) throws IOException  {
      if (output == null) {
        System.out.println("null");
        return;
      }
      System.out.println(data);
      output.write(data.getBytes());
      output.flush();
    }
  }
}
import com.team254.frc2013.auto.CenterDiscMiddleAutoMode;
import com.team254.frc2013.auto.CenterDiscSideAutoMode;
import com.team254.frc2013.auto.CenterLineBlockAutoMode;
import com.team254.frc2013.auto.DriveMotorTestAutoMode;
import com.team254.frc2013.auto.EightDiscAutoMode;
import com.team254.frc2013.auto.FiveDiscAutoMode;
import com.team254.frc2013.auto.FourDiscAutoMode;
import com.team254.frc2013.auto.SevenDiscAutoMode;
import com.team254.frc2013.auto.TestAutoMode;
import com.team254.frc2013.auto.ThreeDiscAutoMode;
import com.team254.frc2013.auto.TuneDriveAutoMode;
import com.team254.frc2013.auto.TwoDiscAutoMode;
import com.team254.frc2013.commands.AutoHangCommand;
import com.team254.frc2013.commands.CommandBase;
import com.team254.frc2013.commands.PtoCommand;
import com.team254.frc2013.subsystems.Shooter;
import com.team254.lib.control.ControlUpdater;
import com.team254.lib.util.Debouncer;
import com.team254.lib.util.Latch;
import com.team254.lib.util.PIDTuner;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.Scheduler;
package com.team254.frc2013;


/**
 * Main class of the robot.
 *
 * @author richard@team254.com (Richard Lin)
 */
public class Overkill extends IterativeRobot {
  private AutoModeSelector autoModeSelector;
  private CommandGroup currentAutoMode;
  private AutoHangCommand autoHangCommand;
  private boolean autoHangStarted;
  private boolean lastStage1HangButton;
  private Latch autonSelectLatch = new Latch();
  private Latch reinitGyroLatch = new Latch();
  Debouncer gyroDriftDetector = new Debouncer(1.0);
  double lastAngle = 0;
  int gyroReinits = 0;

  /**
   * Called when the robot is first started up and should be used for any
   * initialization code.
   */
  public void robotInit() {
    // Initialize all subsystems.
    PIDTuner.getInstance().start();
    CommandBase.init();
    ControlUpdater.getInstance().start();

    // Set up autonomous modes.
    autoModeSelector = new AutoModeSelector();
    autoModeSelector.addAutoCommand("8 Disc", EightDiscAutoMode.class);
    autoModeSelector.addAutoCommand("7 Disc", SevenDiscAutoMode.class);
    autoModeSelector.addAutoCommand("3 Disc", ThreeDiscAutoMode.class);
    autoModeSelector.addAutoCommand("Center (Middle)", CenterDiscMiddleAutoMode.class);
    autoModeSelector.addAutoCommand("Center (Side)", CenterDiscSideAutoMode.class);
    autoModeSelector.addAutoCommand("Center BLOCK", CenterLineBlockAutoMode.class);
    autoModeSelector.addAutoCommand("5 Disc", FiveDiscAutoMode.class);
    autoModeSelector.addAutoCommand("4 Disc", FourDiscAutoMode.class);