Java 类edu.wpi.first.wpilibj.networktables.NetworkTable 实例源码

public VisionAlignment() {

    table = NetworkTable.getTable("Vision");
    double dist = table.getNumber("est_distance", 0);

    double incr = 0.5;
    int reps = (int) (dist / incr);

    for(int i = 0; i<reps; i++) {
        addSequential(new VisionGyroAlign(), 1.5);
        addSequential(new WaitCommand(0.7));
        addSequential(new DriveForwardDistance(-.2,-.2, -incr/1.5, -incr/1.5, true));
        addSequential(new WaitCommand(0.5));
    }

}

public DFDSMove(double speedL, double speedR, double distanceL, double distanceR) {
    requires(Robot.driveTrain);
    _initLeftSpeed = speedL;
    _initRightSpeed = speedR;
    _initDistanceL = distanceL*TICKSPERMETER;
    _initDistanceR = distanceR*TICKSPERMETER;
    table = NetworkTable.getTable("Console");
    if(speedL > 0)
        table.putString("Message", "Backwards DFD Speed");
    else
        table.putString("Message", "Forwards DFD Speed");
    Robot.driveTrain.resetEnc();
    Robot.driveTrain.resetGyro();
    Robot.driveTrain.speedControl();
    Robot.driveTrain.setBrakeMode();
}

/**
 * Creates the instance of VisionSubsystem.
 */
public Vision() {
  logger.trace("Initialize");

  ledRing0 = new Relay(RobotMap.RELAY_LED_RING_0);
  ledRing1 = new Relay(RobotMap.RELAY_LED_RING_1);

  gearVision.table = NetworkTable.getTable("Vision_Gear");
  boilerVision.table = NetworkTable.getTable("Vision_Boiler");

  initPhoneVars(gearVision, DEFAULT_GEAR_TARGET_WIDTH, DEFAULT_GEAR_TARGET_HEIGHT);
  initPhoneVars(boilerVision, DEFAULT_BOILER_TARGET_WIDTH, DEFAULT_BOILER_TARGET_HEIGHT);

  Thread forwardThread = new Thread(this::packetForwardingThread);
  forwardThread.setDaemon(true);
  forwardThread.setName("Packet Forwarding Thread");
  forwardThread.start();

  Thread dataThread = new Thread(this::dataThread);
  dataThread.setDaemon(true);
  dataThread.setName("Vision Data Thread");
  dataThread.start();
}

public static void init() {
        nt = NetworkTable.getTable("vision");
        nt.addTableListener((source, key, value, isNew) -> {
//          System.out.println("Value changed. Key = " + key + ", Value = " + value);
            switch (key) {
                case FOUND_TARGET_KEY:
                    foundTarget = (Boolean) value;
                    break;
                case AREA_KEY:
                    area = (Double) value;
                    break;
                case OFFSET_KEY:
                    offset = (Double) value;
                    break;
                case SKEW_KEY:
                    skew = (Double) value;
                    break;
                case ANGLE_KEY:
                    angle = (Double) value;
                    break;
            }
        });
    }

/**
 * Initializes a few items related to writing elements of a codex to a network table.
 * Do this ahead of time to prevent issues with timing on the first cycle.
 * @param pEnum
 */
public <V, E extends Enum<E> & CodexOf<V>> void registerCodex(Class<E> pEnum) {
  Integer hash = EnumUtils.hashOf(pEnum);
  String tablename = pEnum.getSimpleName().toUpperCase();
  mLog.debug("Registering codex " + tablename + " with hash " + hash);
  mTables.put(hash, NetworkTable.getTable(tablename));

  mLog.info(tablename + " is connected: " + mTables.get(hash).isConnected());

  Class<V> type = CodexMagic.getTypeOfCodex(pEnum);
  if(type.equals(Double.class)) {
    mWriters.put(hash, ((nt, key, val) -> nt.putNumber(key, (double)val)));
  } else if(type.equals(Integer.class)) {
    mWriters.put(hash, ((nt, key, val) -> nt.putNumber(key, (int)val)));
  } else if(type.equals(Boolean.class)) {
    mWriters.put(hash, ((nt, key, val) -> nt.putBoolean(key, (boolean)val)));
  } else if(type.equals(Float.class)) {
    mWriters.put(hash, ((nt, key, val) -> nt.putNumber(key, (float)val)));
  } else {
    throw new IllegalArgumentException("Type " + type.getSimpleName() + " is not supported by CodexNetworkTables.");
  }
}

/**
 * Writes the elements and metadata values of the codex to the NetworkTables that
 * corresponds to the Codex's enum class name.s
 * @param pCodex
 */
public <V, E extends Enum<E> & CodexOf<V>> void send(Codex<V,E> pCodex) {
  int hash = EnumUtils.hashOf(pCodex.meta().getEnum());

  if(!mWriters.containsKey(hash) || !mTables.containsKey(hash)) {
    mLog.warn("Cannot send codex " + pCodex.meta().getEnum().getSimpleName() + " because it has not been registered.");
    return;
  }

  @SuppressWarnings("unchecked")
  Put<V> writer = (Put<V>)mWriters.get(hash);
  NetworkTable nt = mTables.get(hash);

  nt.putNumber("ID", pCodex.meta().id());
  nt.putNumber("KEY", pCodex.meta().key());
  nt.putNumber("TIME_NS", pCodex.meta().timestamp());
  for(E e : EnumSet.allOf(pCodex.meta().getEnum())) {
    if(pCodex.isSet(e)) {
      writer.write(nt, e.name().toUpperCase(), pCodex.get(e));
    }

    // grumble grumble.... NT doesn't have a way to clear a field.
  }
}

public SplinePlannerWidget()
{
    super(false, 10);
    setLayout(new BorderLayout());
    mPanel = new SplinePlotterPanel();
    add(mPanel);

    mTable = NetworkTable.getTable(SmartDashBoardNames.sSPLINE_NAMESPACE);

    mLastIndex = 0;

    mIdealSplineName = SmartDashBoardNames.sSPLINE_IDEAL_POINTS;
    mRealSplineName = SmartDashBoardNames.sSPLINE_REAL_POINT;

    addPathListener();
}

public PlotPlannerWidget()
{
    super(false, 10);
    setLayout(new BorderLayout());
    mPanel = new PathPlotterPanel();
    add(mPanel);

    mLastIndex = 0;

    mTableNamespace = SmartDashBoardNames.sPATH_NAMESPACE;
    mSDIdealPathName = SmartDashBoardNames.sPATH_IDEAL_POINTS;
    mSDRealPathname = SmartDashBoardNames.sPATH_POINT;

    mTable = NetworkTable.getTable(mTableNamespace);

    addPathListener();
}

/**
 * Initialize all the LiveWindow elements the first time we enter LiveWindow mode. By holding off
 * creating the NetworkTable entries, it allows them to be redefined before the first time in
 * LiveWindow mode. This allows default sensor and actuator values to be created that are replaced
 * with the custom names from users calling addActuator and addSensor.
 */
private static void initializeLiveWindowComponents() {
  System.out.println("Initializing the components first time");
  livewindowTable = NetworkTable.getTable("LiveWindow");
  statusTable = livewindowTable.getSubTable("~STATUS~");
  for (Enumeration e = components.keys(); e.hasMoreElements(); ) {
    LiveWindowSendable component = (LiveWindowSendable) e.nextElement();
    LiveWindowComponent liveWindowComponent = (LiveWindowComponent) components.get(component);
    String subsystem = liveWindowComponent.getSubsystem();
    String name = liveWindowComponent.getName();
    System.out.println("Initializing table for '" + subsystem + "' '" + name + "'");
    livewindowTable.getSubTable(subsystem).putString("~TYPE~", "LW Subsystem");
    ITable table = livewindowTable.getSubTable(subsystem).getSubTable(name);
    table.putString("~TYPE~", component.getSmartDashboardType());
    table.putString("Name", name);
    table.putString("Subsystem", subsystem);
    component.initTable(table);
    if (liveWindowComponent.isSensor()) {
      sensors.addElement(component);
    }
  }
}

private void loadConfig(String aFile)
{

    try
    {
        if (!Files.exists(Paths.get(aFile)))
        {
            System.err.println("Could not read properties file, will use defaults and will overwrite the file if it exists");
            Files.copy(Paths.get("_default_properties.properties"), Paths.get(aFile));
        }

        Properties p = new Properties();
        p.load(new FileInputStream(new File(aFile)));

        mClassName = p.getProperty("robot_class");
        mSimulatorClassName = p.getProperty("simulator_class");
        NetworkTable.setPersistentFilename(sUSER_CONFIG_DIR + mClassName + ".preferences.ini");
    }
    catch (Exception e)
    {
        e.printStackTrace();
        System.err.println("Could not read properties file");
    }
}

public AutonomousFactory(Snobot2017 aSnobot, IDriverJoystick aDriverJoystick)
{
    mPositionChooser = new SendableChooser<StartingPositions>();
    mCommandParser = new CommandParser(aSnobot, mPositionChooser);
    mAutoModeTable = NetworkTable.getTable(SmartDashBoardNames.sAUTON_TABLE_NAME);

    mPositioner = aSnobot.getPositioner();

    mAutonModeChooser = new SnobotAutonCrawler(Properties2017.sAUTON_FILE_FILTER.getValue())
            .loadAutonFiles(Properties2017.sAUTON_DIRECTORY.getValue() + "/", Properties2017.sAUTON_DEFAULT_FILE.getValue());

    SmartDashboard.putData(SmartDashBoardNames.sAUTON_CHOOSER, mAutonModeChooser);


    for (StartingPositions pos : StartingPositions.values())
    {
        mPositionChooser.addObject(pos.toString(), pos);
    }

    SmartDashboard.putData(SmartDashBoardNames.sPOSITION_CHOOSER, mPositionChooser);
    addListeners();    
    }

public AutonFactory(IPositioner aPositioner, Snobot aSnobot)
{
    mSelectStartPosition = new SelectStartPosition(aPositioner);
    mDefenseInFront = new DefenseInFront();
    mSelectAutonomous = new SelectAutonomous();


    mDefenseTable = NetworkTable.getTable(SmartDashBoardNames.sDEFENSE_AUTON_TABLE);
    mPostDefenseTable = NetworkTable.getTable(SmartDashBoardNames.sPOST_DEFENSE_AUTON_TABLE);

    mDefenseCommandParser = new CommandParser(aSnobot, mDefenseTable, mSelectStartPosition, "Defense");
    mPostDefenseCommandParser = new CommandParser(aSnobot, mPostDefenseTable, "PostDefense", mSelectStartPosition, mDefenseCommandParser, mDefenseInFront);

    this.putOnDash();
    addListeners();
}

public void init() { // This sets everything up to listen!
    try {
        new ProcessBuilder(CLEAR_TMP_CMD).inheritIO().start();
        // new ProcessBuilder(GRIP_CMD).inheritIO().start();
    } catch (IOException e) {
        System.out.println(e);
    }

    time.reset();
    time.start();

    GRIPTable = NetworkTable.getTable("GRIP/myLinesReport");
    GRIPTable.addTableListener(updater);

    // new Thread(crashCheck).start();
}

public static void main(String[] args) throws Exception {
    NetworkTable.setClientMode();
    InetAddress address = InetAddress.getByName("roborio-1458-frc.local");
    NetworkTable.setIPAddress(address.getHostAddress());

    NetworkTable SmartDashboard = NetworkTable.getTable("SmartDashboard");

    Scanner s = new Scanner(System.in);

    //SmartDashboard.putNumber("TestValueJetson", 42);

    while(true) {
        if(s.hasNextInt()) {
            SmartDashboard.putNumber("TestValueJetson", s.nextInt());
        }
    }
}

public AimWithCamera()
{
    requires((Subsystem) driveTrain);
    requires((Subsystem) shooterArticulator);       

    if (CommandBase.lightSystem != null)
        requires((Subsystem) lightSystem);

    table = NetworkTable.getTable("IMGPROC");   

    TOLERANCE = Double.parseDouble(BadPreferences.getValue(toleranceKey, "" + TOLERANCE));
    TURN_SPEED = Double.parseDouble(BadPreferences.getValue(turnKey, "" + TURN_SPEED));
    NUMBER_CYCLES_TO_VERIFY = Integer.parseInt(
            BadPreferences.getValue(neededCyclesKey, "" + NUMBER_CYCLES_TO_VERIFY));

    SWEET_SPOT_X = Double.parseDouble(BadPreferences.getValue(sweetXKey, "" + SWEET_SPOT_X));
    SWEET_SPOT_Y = Double.parseDouble(BadPreferences.getValue(sweetYKey, "" + SWEET_SPOT_Y));
}

public void robotInit() {
        // instantiate the command used for the autonomous period
        //autonomousCommand = new ExampleCommand();

        // Initialize all subsystems
        CommandBase.init();

        table = NetworkTable.getTable("CRIO");
        table.putBoolean("bool", true);
        table.putNumber("double", 3.1415927);
        table.putString("sring", "a string");

        LogDebugger.log("robot init!!!");

//  LiveWindow.addActuator("compressor", "alt relay", RobotMap.testCompRelay);
//  LiveWindow.addActuator("compressor", "act compressor", RobotMap.airCompressor);
//  LiveWindow.addSensor("compressor", "sensor compressor", RobotMap.airCompressor);
    }

/**
 * Setup Network Tables, and get the NetworkTable for the SmartDashboard.
 * @return The network table for the SmartDashboard.
 */
private NetworkTable getNetworkTable()
{
    NetworkTable.setTeam(1899);
    NetworkTable.setServerMode();
    try
    {
        NetworkTable.initialize();
    }
    catch (IOException exception)
    {
        Logger.log(exception);
    }

    return NetworkTable.getTable("SmartDashboard");
}

public LeftPeg() {

        Robot.driveTrain.resetEnc();
        table = NetworkTable.getTable("Vision");

        addSequential(new DFDSpeed(-200, -200, 1.55, 1.55));
        addSequential(new WaitCommand(0.2));
        addSequential(new GyroTurn(-150, 50), 2);
        addSequential(new WaitCommand(0.2));
        addSequential(new VisionGyroAlign(), 1);
        addSequential(new WaitCommand(0.2));
        addSequential(new MovingVisionAlignment(), 5); //removed timeout
        addSequential(new WaitCommand(0.5));

        addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
        addSequential(new WaitCommand(0.1));
        addSequential(new MovingVisionAlignment(), 5); //removed timeout
        addSequential(new WaitCommand(0.1));
        addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
        addSequential(new WaitCommand(0.1));
        addSequential(new MovingVisionAlignment(), 5); //removed timeout
        addSequential(new WaitCommand(0.1));
        addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
        addSequential(new WaitCommand(0.1));
        addSequential(new MovingVisionAlignment()); //removed timeout
        addSequential(new WaitCommand(0.1));
        addSequential(new GearOn(false, false), 1);
        addSequential(new WaitCommand(0.5));
        addSequential(new DFDSpeed(200, 200, 1.4, 1.4));

    }

public RightPeg() {
    Robot.driveTrain.resetEnc();
    table = NetworkTable.getTable("Vision");

    addSequential(new DFDSpeed(-200, -200, 1.60, 1.60));
    addSequential(new WaitCommand(0.2));
    addSequential(new GyroTurn(-150, -50), 2); //CHECK + AND -
    addSequential(new WaitCommand(0.2));
    addSequential(new VisionGyroAlign(), 1);
    addSequential(new WaitCommand(0.2));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.2));

    //Move back retry
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment()); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new GearOn(false, false), 1);
    addSequential(new WaitCommand(0.5));
    addSequential(new DFDSpeed(200, 200, 1.4, 1.4));

}

public VisionGyroAlign () {
    requires(Robot.driveTrain);
    table = NetworkTable.getTable("Vision");
    Robot.driveTrain.resetLeftEnc();
    Robot.driveTrain.resetRightEnc();
    Robot.driveTrain.resetGyro();
    _turnPower = -125;
}

public AutoDrive(double speed) {
    requires(Robot.driveTrain);
    table = NetworkTable.getTable("Vision");
    _leftSpeed = speed*1.15;
    _rightSpeed = speed;
    Robot.driveTrain.percentVbusControl();
}

public DriveForwardDistance(double speedL, double speedR, double distanceL, double distanceR, boolean slow) {
    requires(Robot.driveTrain);
    _initLeftSpeed = speedL;
    _initRightSpeed = speedR;
    _initDistanceL = distanceL*TICKSPERMETER;
    _initDistanceR = distanceR*TICKSPERMETER;
    _slow = slow;
    Robot.driveTrain.percentVbusControl();
    table = NetworkTable.getTable("Console");
}

public DriveForwardPosition(double speed, double distanceL, double distanceR) {
    //requires(Robot.driveTrainPID);
    _initLeftSpeed = speed*1.15;
    _initRightSpeed = speed;
    _initDistanceL = distanceL*TICKSPERMETER;
    _initDistanceR = distanceR*TICKSPERMETER;
    //Robot.driveTrainPID.percentVbusControl();
    table = NetworkTable.getTable("Console");
}

public DFDSpeed(double speedL, double speedR, double distanceL, double distanceR) {
    requires(Robot.driveTrain);
    _initLeftSpeed = speedL;
    _initRightSpeed = speedR;
    _initDistanceL = distanceL*TICKSPERMETER;
    _initDistanceR = distanceR*TICKSPERMETER;
    table = NetworkTable.getTable("Console");
    if(speedL > 0)
        table.putString("Message", "Backwards DFD Speed");
    else
        table.putString("Message", "Forwards DFD Speed");
}

@Override
protected void initDefaultCommand() {
    table = NetworkTable.getTable("Vision");
    // TODO Auto-generated method stub
    LEDZ = NetworkTable.getTable("LEDS");

}

public DataLogger()
{
    fields = new LinkedHashMap<String,String>();
    loggables = new ArrayList<>();
    table = NetworkTable.getTable("logging");
    for(String s : table.getKeys())
    {
        table.delete(s);
    }
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {
    RobotMap.init();
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    driveTrain = new DriveTrain();
    pDP = new PDP();
    intake = new Intake();
    climber = new Climber();
    shooter = new Shooter();

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    // OI must be constructed after subsystems. If the OI creates Commands
    //(which it very likely will), subsystems are not guaranteed to be
    // constructed yet. Thus, their requires() statements may grab null
    // pointers. Bad news. Don't move it.
    oi = new OI();

    // initializes networktable
    table = NetworkTable.getTable("HookContoursReport");

    // sets up camera switcher
    server = CameraServer.getInstance();
    server.startAutomaticCapture(0);
    // cameraInit();

    // set up sendable chooser for autonomous
    autoChooser = new SendableChooser();
    autoChooser.addDefault("Middle Hook", new AUTOMiddleHook());
    autoChooser.addObject("Left Hook", new AUTOLeftHook());
    autoChooser.addObject("RightHook", new AUTORightHook());
    SmartDashboard.putData("Auto Chooser", autoChooser);


    // instantiate the command used for the autonomous period

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS


// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
}

/**
 * Updates the local contoursReport table. WARNING, this has to be somewhat
 * syncrhonized so one thing doesn't update the table while another tries to
 * access an element that might not exist in the updated table
 *
 */
public void updateContoursReport() {
    // A contours report contains centerX[], centerY[], solidity[],
    // height[], area[], and width[]

    // I think that it publishes the contours with the smallest area first
    // in the array
    contoursReport = NetworkTable.getTable("GRIP/myContoursReport");
}

public GripContourReport(NetworkTable table) 
{
    area = table.getNumberArray("area", new double[0]);
    if (area.length>0) {
        width = table.getNumberArray("width", new double[0]);
        height = table.getNumberArray("height", new double[0]);
        centerX = table.getNumberArray("centerX", new double[0]);
        centerY = table.getNumberArray("centerY", new double[0]);
        Debug.msg("GRIP", "FOUND");
    }
    else
    {
        width =  new double[0];
        height = new double[0];
        centerX = new double[0];
        centerY = new double[0];
        Debug.msg("GRIP", "NOT FOUND");
    }
    double maxArea = -1.0;
    int max = -1;
    for(int i=0; i<area.length;i++) {
        if(area[i]>maxArea) {
            maxArea=area[i];
            max = i;
        }
    }
    this.max=max;
    if(max>-1&&max<centerX.length)
        maxCenterX=(int)(centerX[max]+0.5);
    else
        maxCenterX=-1;
    if(max>-1&&max<centerY.length)
        maxCenterY=(int)(centerY[max]+0.5);
    else
        maxCenterY=-1;
}

private void initializeTrajectoryListener()
{

    ITable mTable = NetworkTable.getTable(SmartDashBoardNames.sSPLINE_NAMESPACE);
    ITableListener idealSplineListener = new ITableListener()
    {

        @Override
        public void valueChanged(ITable arg0, String arg1, Object arg2, boolean arg3)
        {
            mCoordinateGui.setPath(IdealSplineSerializer.deserializePath(arg2.toString()));
        }
    };
    mTable.addTableListener(SmartDashBoardNames.sSPLINE_IDEAL_POINTS, idealSplineListener, true);
}

/**
 * Constructor for a generic robot program. User code should be placed in the constructor that
 * runs before the Autonomous or Operator Control period starts. The constructor will run to
 * completion before Autonomous is entered.
 *
 * <p>This must be used to ensure that the communications code starts. In the future it would be
 * nice
 * to put this code into it's own task that loads on boot so ensure that it runs.
 */
protected RobotBase() {
  // TODO: StartCAPI();
  // TODO: See if the next line is necessary
  // Resource.RestartProgram();

  NetworkTable.setNetworkIdentity("Robot");
  NetworkTable.setServerMode();// must be before b
  m_ds = DriverStation.getInstance();
  NetworkTable.getTable(""); // forces network tables to initialize
  NetworkTable.getTable("LiveWindow").getSubTable("~STATUS~").putBoolean("LW Enabled", false);
}

/**
 * Creates a CommandParser object.
 * 
 * @param aSnobot
 *            The robot using the CommandParser.
 * @param aPositionChooser
 * @param aStartPosition
 */
public CommandParser(Snobot2017 aSnobot, SendableChooser<StartingPositions> aPositionChooser)
{
    super(NetworkTable.getTable(SmartDashBoardNames.sAUTON_TABLE_NAME), SmartDashBoardNames.sROBOT_COMMAND_TEXT,
            SmartDashBoardNames.sSUCCESFULLY_PARSED_AUTON, " ", "#");
    mSnobot = aSnobot;
    mPositionChooser = aPositionChooser;

}

public TrajectoryPathCommand(IDriveTrain aDrivetrain, IPositioner aPositioner, Path aPath)
{
    mDrivetrain = aDrivetrain;
    mPositioner = aPositioner;
    mPath = aPath;

    mTable = NetworkTable.getTable(SmartDashBoardNames.sSPLINE_NAMESPACE);
    mSDIdealName = SmartDashBoardNames.sSPLINE_IDEAL_POINTS;
    mSDRealName = SmartDashBoardNames.sSPLINE_REAL_POINT;

    double kP = Properties2017.sDRIVE_PATH_KP.getValue();
    double kD = Properties2017.sDRIVE_PATH_KD.getValue();
    double kVelocity = Properties2017.sDRIVE_PATH_KV.getValue();
    double kAccel = Properties2017.sDRIVE_PATH_KA.getValue();

    mKTurn = Properties2017.sSPLINE_TURN_FACTOR.getValue();

    followerLeft.configure(kP, 0, kD, kVelocity, kAccel);
    followerRight.configure(kP, 0, kD, kVelocity, kAccel);

    followerLeft.reset();
    followerRight.reset();

    followerLeft.setTrajectory(aPath.getLeftWheelTrajectory());
    followerRight.setTrajectory(aPath.getRightWheelTrajectory());

    if (mTable.getString(mSDIdealName, "").isEmpty())
    {
        sendIdealPath();
    }
}

public TrajectoryPathCommand(IDriveTrain aDrivetrain, IPositioner aPositioner, Path aPath)
{
    mDrivetrain = aDrivetrain;
    mPositioner = aPositioner;
    mPath = aPath;

    mTable = NetworkTable.getTable(SmartDashBoardNames.sSPLINE_NAMESPACE);
    mSDIdealName = SmartDashBoardNames.sSPLINE_IDEAL_POINTS;
    mSDRealName = SmartDashBoardNames.sSPLINE_REAL_POINT;

    double kP = Properties2016.sDRIVE_PATH_KP.getValue();
    double kD = Properties2016.sDRIVE_PATH_KD.getValue();
    double kVelocity = Properties2016.sDRIVE_PATH_KV.getValue();
    double kAccel = Properties2016.sDRIVE_PATH_KA.getValue();

    mKTurn = Properties2016.sSPLINE_TURN_FACTOR.getValue();

    followerLeft.configure(kP, 0, kD, kVelocity, kAccel);
    followerRight.configure(kP, 0, kD, kVelocity, kAccel);

    followerLeft.reset();
    followerRight.reset();

    followerLeft.setTrajectory(aPath.getLeftWheelTrajectory());
    followerRight.setTrajectory(aPath.getRightWheelTrajectory());

    if (mTable.getString(mSDIdealName, "").isEmpty())
    {
        sendIdealPath();
    }
}

@Override
public void run() {
    while (true) {
        ITable grip = NetworkTable.getTable("GRIP/myLinesReport");
        ;
        VisionHandler.getInstance().update(grip);
        try {
            Thread.sleep(500);
        } catch (InterruptedException e) {
            // TODO Auto-generated catch block
            e.printStackTrace();
        }
    }
}

protected void initialize() {
    firstTurn = true;
    shouldSetTimeout = true;
    waiting = false;

table = NetworkTable.getTable("vision");
  }

protected void initialize() {       
    visionTable = NetworkTable.getTable("vision");
    controlTable = NetworkTable.getTable("control_daemon");

    SmartDashboard.putData("Horizontal align", new HorizontalAlign(true));
    SmartDashboard.putData("Vertical align", new VerticalAlign(true));
    SmartDashboard.putData("Free fire (normal)", new FreeFire(false));
    SmartDashboard.putData("Free fire (Menzie)", new FreeFire(true));
    SmartDashboard.putNumber("Bonus Angle", 0);

    SmartDashboard.putData("Unstick Ball", new UnstickBall());
    SmartDashboard.putData("Calibrate vision", new CalibrateVisionAngle());
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {
    NetworkTable.globalDeleteAll();

 oi = new OI();
 teleop = new Teleop();
 uHGSD = new UpdateHighGoalShooterDashboard();
 autonomousCommand = new Autonomous(2, 2);
 CameraServer server = CameraServer.getInstance();
 server.startAutomaticCapture("cam0"); 

 hood.resetEncoder(hood.HOOD_START);
}

/**
   * This function is called once each time the robot enters Disabled mode.
   * You can use it to reset any subsystem information you want to clear when
* the robot is disabled.
   */
  public void disabledInit() {
    SmartDashboard.putNumber("Auto mode", SmartDashboard.getNumber("Auto mode", 2));
    SmartDashboard.putNumber("Robot in front of defense", SmartDashboard.getNumber("Robot in front of defense", 2));
    table = NetworkTable.getTable("PiTouch");
    controlTable = NetworkTable.getTable("control_daemon");

    SmartDashboard.putNumber("pid error", 0);
    SmartDashboard.putBoolean("On target!", false);
    SmartDashboard.putBoolean("Shoot Horizontally Aligned", false);
    SmartDashboard.putBoolean("Shoot RPM Aligned", false);

    SmartDashboard.putNumber("distance", 0);

    SmartDashboard.putNumber("New flipper angle", 0);

    SmartDashboard.putNumber("New Hood Angle", hood.HOOD_START);
    //SmartDashboard.putData("Set Hood PID", new SetPID("hood", Robot.hood.pidController));
    SmartDashboard.putData("Move Hood", new MoveHoodSmartDashboard());

    SmartDashboard.putNumber("New RPM", 0);
    //SmartDashboard.putData("Set RPM PID", new SetPID("rpm", Robot.shootingWheel.shootingWheelPIDController));
    SmartDashboard.putData("Set RPM", new MoveShootingWheelSmartDashboard());
    SmartDashboard.putData("Stop RPM", new MoveShootingWheel(0));

    SmartDashboard.putNumber("New Turntable 1", 0);
    SmartDashboard.putNumber("New Turntable 2", 0);
    SmartDashboard.putData("Set Turntable PID", new SetPID("turntable", Robot.turntable.pidController));
    SmartDashboard.putData("Set Turntable 1", new MoveTurnTableSmartDashboard("New Turntable 1"));
    SmartDashboard.putData("Set Turntable 2", new MoveTurnTableSmartDashboard("New Turntable 2"));

    SmartDashboard.putData("Shoot", new FireShooter());
    SmartDashboard.putData("Set Flipper", new SetShooterFlipper(1));

uHGSD.start();
  }

protected void initialize() {
    table = NetworkTable.getTable("vision");
    table.putNumber("heartbeat", 1);
    SmartDashboard.putBoolean("Shoot Horizontally Aligned", false);
    firstTime = true;
    aligned = false;
    targetTime = Timer.getFPGATimestamp();
}