Exemple #1
0
  public Shooter() throws CANTimeoutException {

    loadingStick = new CANJaguar(5);
    // http://www.chiefdelphi.com/forums/showthread.php?t=101737
    // Configuring firstShootMotor in order to control speed with encoder
    firstShootingMotor = new CANJaguar(6, CANJaguar.ControlMode.kSpeed);
    firstShootingMotor.configEncoderCodesPerRev(256);
    firstShootingMotor.enableControl();
    firstShootingMotor.changeControlMode(CANJaguar.ControlMode.kVoltage);
    firstShootingMotor.enableControl();

    // Configuring secondShootMotor in order to control speed with encoder
    secondShootingMotor = new CANJaguar(7, CANJaguar.ControlMode.kSpeed);
    secondShootingMotor.configEncoderCodesPerRev(256);
    secondShootingMotor.enableControl();
    secondShootingMotor.changeControlMode(CANJaguar.ControlMode.kVoltage);
    secondShootingMotor.enableControl();

    angleMotor = new CANJaguar(8);

    frontSwitch = new DigitalInput(1);
    backSwitch = new DigitalInput(1);

    table = NetworkTable.getTable("shooter");

    table.putNumber("firstSpeed", 0);
    table.putNumber("angle", 0);
  }
Exemple #2
0
  public void robotInit() {
    log("RobotInit called. Initializing NetworkTabes...");
    try {
      NetworkTable.setIPAddress("10.36.99.2");
      NetworkTable.setServerMode();
      NetworkTable.setTeam(3699);
      NetworkTable.initialize();

    } catch (Exception be) {
      error("Error in robotInit: \n " + be.getMessage(), 0);
      error("Reocourrence Test! You shouldnt see this!", 0);
    }
    log("Init SmartDashboard...");
    this.server = NetworkTable.getTable("SmartDashboard");

    log("" + this.server.containsKey("/SmartDashboard/"));
    log("" + this.server.containsKey("/SmartDashboard/FPS"));
    log("" + NetworkTable.DEFAULT_PORT);
    this.server.putString("_PREINIT0", "_PREINIT::0");
    this.server.putString("_PREINIT1", "_PREINIT::1");
    this.server.putString("_PREINIT2", "_PREINIT::2");

    /*LiveWindow.addActuator("Elevator", "Elevator Intake", Elevator_intake);
    LiveWindow.addActuator("Elevator", "Elevator Gearbox", Elevator_motor);
    LiveWindow.addActuator("Elevator", "Elevator Outtake", Elevator_outtake);
    LiveWindow.addActuator("Intake", "Intake", Elevator_motor);*/

    log("Disabling RobotDrive Saftey... :D");
    robotdrive.setSafetyEnabled(false);

    // autoChooser = new SendableChooser();
    // autoChooser.addDefault("Default 3 Discs", this.auto_shoot);
    // SmartDashboard.putData("Autonomous mode chooser",autoChooser);

  }
 /**
  * Returns value of a preference as a String
  *
  * @param key - The name of the preference
  * @return The value of the preference
  */
 public static String getString(String key) {
   if (table.containsKey(key)) {
     return table.getString(key);
   } else {
     System.err.println("Preference not found: " + key);
     set(key, "0");
     return "0";
   }
 }
 public static void setPort(int port) {
   if (_port == port) return;
   _port = port;
   try {
     NetworkTable.shutdown();
   } catch (IllegalStateException ex) {
   }
   NetworkTable.setPort(port);
   NetworkTable.initialize();
 }
  public void testValue() {
    if (oi.getLJoystick().getTrigger()) {

      boolean distcheck = NetworkTable.getTable("camera").containsKey("distance");
      NetworkTable.getTable("rob").putNumber("x", 8008135);
      System.out.println(distcheck);
      if (distcheck) {
        distance = NetworkTable.getTable("camera").getNumber("distance");
      }
      System.out.println("Distance:" + " " + distance);
    }
  }
 private void Constructor(
     String ptableName, String pbooleanKey, boolean pDefaultValue, String id) {
   pId = id;
   booleanKey = pbooleanKey;
   defaultValue = pDefaultValue;
   tableName = ptableName;
   defCommPrefix = bCommPrefix;
   if (tableName.startsWith("/")) {
     defCommPrefix = "";
     tableName = tableName.substring(1);
   }
   table = NetworkTable.getTable(defCommPrefix + tableName);
   boolean temp = table.getBoolean(booleanKey, defaultValue);
   table.putBoolean(booleanKey, temp);
   // table.putString(booleanKey+".ID", id);
   table.addTableListener(
       booleanKey,
       new ITableListener() {
         public void valueChanged(ITable source, String key, Object pvalue, boolean isNew) {
           if (key.equals(Tables.Constants.IS_REFRESH_KEY)) {}
           if (isLocked && (oldValue != (Boolean) pvalue)) {
             table.putBoolean(booleanKey, value);
             oldValue = value;
             return;
           }
           value = (Boolean) pvalue;
           oldValue = value;
         }
       },
       true);
 }
  /**
   * 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
    elevator = new Elevator();
    gripper = new Gripper();
    drivetrain = new Drivetrain();
    purpleSensor = new PurpleSensor();
    camera = new Camera();
    lights = new Lights();

    // 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();

    // instantiate the command used for the autonomous period
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
    autonomousCommand = new DoNothingAuto();

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
    Robot.gripper.closedLoopControl();

    table = NetworkTable.getTable("datatable");
    drivetrain.smartDashboardInit();
    purpleSensor.zeroHeading();
  }
 /**
  * Maps the specified key to the specified value in this table. The key can not be null. The value
  * can be retrieved by calling the get method with a key that is equal to the original key.
  *
  * @param key the key
  * @param data the value
  * @throws IllegalArgumentException if key is null
  */
 @SuppressWarnings("unchecked")
 public static void putData(String key, Sendable data) {
   ITable dataTable = table.getSubTable(key);
   dataTable.putString("~TYPE~", data.getSmartDashboardType());
   data.initTable(dataTable);
   tablesToData.put(data, key);
 }
 /**
  * Returns the value at the specified key.
  *
  * @deprecated Use {@link #getNumber(java.lang.String, double) getNumber} instead
  * @param key the key
  * @param defaultValue the value returned if the key is undefined
  * @return the value
  * @throws TableKeyNotDefinedException if there is no value mapped to by the key
  * @throws IllegalArgumentException if the value mapped to by the key is not an int
  * @throws IllegalArgumentException if the key is null
  */
 public static int getInt(String key, int defaultValue) throws TableKeyNotDefinedException {
   try {
     return (int) table.getNumber(key);
   } catch (NoSuchElementException ex) {
     return defaultValue;
   }
 }
Exemple #10
0
  public double getSpeed(int motorNumber) throws CANTimeoutException {

    double speed = -1;

    if (motorNumber == 1) {

      speed = firstShootingMotor.getSpeed();
      table.putNumber("firstSpeed", speed);
    }

    if (motorNumber == 2) {
      speed = secondShootingMotor.getSpeed();
      table.putNumber("secondSpeed", speed);
    }

    return speed;
  }
 /**
  * Returns the value at the specified key.
  *
  * @param key the key
  * @return the value
  * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
  * @throws IllegalArgumentException if the key is null
  */
 public static Sendable getData(String key) {
   ITable subtable = table.getSubTable(key);
   Object data = tablesToData.get(subtable);
   if (data == null) {
     throw new IllegalArgumentException("SmartDashboard data does not exist: " + key);
   } else {
     return (Sendable) data;
   }
 }
 public static void setHost(String host) {
   if (host != "") {
     // Use the given host
   } else if (_usemDNS) {
     host = "roboRIO-" + _team + "-frc.local";
   } else {
     host = "10." + (_team / 100) + "." + (_team % 100) + ".2";
   }
   if (_host.equals(host)) return;
   _host = host;
   System.out.println("Host: " + host);
   try {
     NetworkTable.shutdown();
   } catch (IllegalStateException ex) {
   }
   NetworkTable.setIPAddress(host);
   NetworkTable.setNetworkIdentity(identity);
   NetworkTable.initialize();
 }
  public Robot() {
    server = CameraServer.getInstance();
    server.setQuality(50);
    // the camera name (ex "cam0") can be found through the roborio web
    // interface
    server.startAutomaticCapture("cam0");

    table = NetworkTable.getTable("SharkCV/contours/0");

    autonomousCommand = new Auton();
  }
 /** Reads the preferences file and starts listening to the network table */
 public static void init() {
   read();
   table.addTableListener(
       new ITableListener() {
         @Override
         public void valueChanged(ITable source, String key, Object value, boolean isNew) {
           // Make sure the code knows all the keys
           if (!keys.contains(key)) keys.add(key);
         }
       },
       true);
 }
 /** Overwrites the file with current preferences */
 public static void write() {
   BufferedWriter out;
   if (keys.isEmpty()) { // So we don't accidentally delete the preferences file
     return;
   }
   try {
     out = new BufferedWriter(new FileWriter("/home/lvuser/Preferences.txt"));
     for (int i = 0; i < keys.size(); i++) {
       if (!keys.get(i).equals("")) { // Empty String indicates a blank line
         String line = keys.get(i) + DIVIDER + table.getString(keys.get(i));
         out.write(line);
       }
       out.newLine();
     }
     out.flush();
     out.close();
   } catch (IOException e) {
     e.printStackTrace();
   }
 }
 /**
  * Maps the specified key to the specified value in this table.
  *
  * <p>The key can not be null. The value can be retrieved by calling the get method with a key
  * that is equal to the original key.
  *
  * @deprecated Use{@link #putNumber(java.lang.String, double) putNumber} instead
  * @param key the key
  * @param value the value
  * @throws IllegalArgumentException if key is null
  */
 public static void putDouble(String key, double value) {
   table.putNumber(key, value);
 }
/**
 * The {@link SmartDashboard} class is the bridge between robot programs and the SmartDashboard on
 * the laptop.
 *
 * <p>When a value is put into the SmartDashboard here, it pops up on the SmartDashboard on the
 * laptop. Users can put values into and get values from the SmartDashboard
 *
 * @author Joe Grinstead
 */
@SuppressWarnings("deprecation")
public class SmartDashboard {
  /** The {@link NetworkTable} used by {@link SmartDashboard} */
  private static final NetworkTable table = NetworkTable.getTable("SmartDashboard");
  /**
   * A table linking tables in the SmartDashboard to the {@link SmartDashboardData} objects they
   * came from.
   */
  @SuppressWarnings("rawtypes")
  private static final Hashtable tablesToData = new Hashtable();

  /**
   * Maps the specified key to the specified value in this table. The key can not be null. The value
   * can be retrieved by calling the get method with a key that is equal to the original key.
   *
   * @param key the key
   * @param data the value
   * @throws IllegalArgumentException if key is null
   */
  @SuppressWarnings("unchecked")
  public static void putData(String key, Sendable data) {
    ITable dataTable = table.getSubTable(key);
    dataTable.putString("~TYPE~", data.getSmartDashboardType());
    data.initTable(dataTable);
    tablesToData.put(data, key);
  }

  // TODO should we reimplement NamedSendable?
  /**
   * Maps the specified key (where the key is the name of the {@link NamedSendable}
   * SmartDashboardNamedData to the specified value in this table. The value can be retrieved by
   * calling the get method with a key that is equal to the original key.
   *
   * @param value the value
   * @throws IllegalArgumentException if key is null
   */
  public static void putData(NamedSendable value) {
    putData(value.getName(), value);
  }

  /**
   * Returns the value at the specified key.
   *
   * @param key the key
   * @return the value
   * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
   * @throws IllegalArgumentException if the key is null
   */
  public static Sendable getData(String key) {
    ITable subtable = table.getSubTable(key);
    Object data = tablesToData.get(subtable);
    if (data == null) {
      throw new IllegalArgumentException("SmartDashboard data does not exist: " + key);
    } else {
      return (Sendable) data;
    }
  }

  /**
   * Maps the specified key to the specified value in this table. The key can not be null. The value
   * can be retrieved by calling the get method with a key that is equal to the original key.
   *
   * @param key the key
   * @param value the value
   * @throws IllegalArgumentException if key is null
   */
  public static void putBoolean(String key, boolean value) {
    table.putBoolean(key, value);
  }

  /**
   * Returns the value at the specified key.
   *
   * @param key the key
   * @return the value
   * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not a boolean
   * @throws IllegalArgumentException if the key is null
   */
  public static boolean getBoolean(String key) throws TableKeyNotDefinedException {
    return table.getBoolean(key);
  }

  /**
   * Returns the value at the specified key.
   *
   * @param key the key
   * @param defaultValue returned if the key doesn't exist
   * @return the value
   * @throws IllegalArgumentException if the value mapped to by the key is not a boolean
   * @throws IllegalArgumentException if the key is null
   */
  public static boolean getBoolean(String key, boolean defaultValue) {
    return table.getBoolean(key, defaultValue);
  }

  /**
   * Maps the specified key to the specified value in this table. The key can not be null. The value
   * can be retrieved by calling the get method with a key that is equal to the original key.
   *
   * @param key the key
   * @param value the value
   * @throws IllegalArgumentException if key is null
   */
  public static void putNumber(String key, double value) {
    table.putNumber(key, value);
  }

  /**
   * Returns the value at the specified key.
   *
   * @param key the key
   * @return the value
   * @throws TableKeyNotDefinedException if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not a double
   * @throws IllegalArgumentException if the key is null
   */
  public static double getNumber(String key) throws TableKeyNotDefinedException {
    return table.getNumber(key);
  }

  /**
   * Returns the value at the specified key.
   *
   * @param key the key
   * @param defaultValue the value returned if the key is undefined
   * @return the value
   * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not a double
   * @throws IllegalArgumentException if the key is null
   */
  public static double getNumber(String key, double defaultValue) {
    return table.getNumber(key, defaultValue);
  }

  /**
   * Maps the specified key to the specified value in this table. Neither the key nor the value can
   * be null. The value can be retrieved by calling the get method with a key that is equal to the
   * original key.
   *
   * @param key the key
   * @param value the value
   * @throws IllegalArgumentException if key or value is null
   */
  public static void putString(String key, String value) {
    table.putString(key, value);
  }

  /**
   * Returns the value at the specified key.
   *
   * @param key the key
   * @return the value
   * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not a string
   * @throws IllegalArgumentException if the key is null
   */
  public static String getString(String key) throws TableKeyNotDefinedException {
    return table.getString(key);
  }

  /**
   * Returns the value at the specified key.
   *
   * @param key the key
   * @param defaultValue The value returned if the key is undefined
   * @return the value
   * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not a string
   * @throws IllegalArgumentException if the key is null
   */
  public static String getString(String key, String defaultValue) {
    return table.getString(key, defaultValue);
  }

  /*
   * Deprecated Methods
   */
  /**
   * Maps the specified key to the specified value in this table.
   *
   * <p>The key can not be null. The value can be retrieved by calling the get method with a key
   * that is equal to the original key.
   *
   * @deprecated Use {@link #putNumber(java.lang.String, double) putNumber method} instead
   * @param key the key
   * @param value the value
   * @throws IllegalArgumentException if key is null
   */
  public static void putInt(String key, int value) {
    table.putNumber(key, value);
  }

  /**
   * Returns the value at the specified key.
   *
   * @deprecated Use {@link #getNumber(java.lang.String) getNumber} instead
   * @param key the key
   * @return the value
   * @throws TableKeyNotDefinedException if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not an int
   * @throws IllegalArgumentException if the key is null
   */
  public static int getInt(String key) throws TableKeyNotDefinedException {
    return (int) table.getNumber(key);
  }

  /**
   * Returns the value at the specified key.
   *
   * @deprecated Use {@link #getNumber(java.lang.String, double) getNumber} instead
   * @param key the key
   * @param defaultValue the value returned if the key is undefined
   * @return the value
   * @throws TableKeyNotDefinedException if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not an int
   * @throws IllegalArgumentException if the key is null
   */
  public static int getInt(String key, int defaultValue) throws TableKeyNotDefinedException {
    try {
      return (int) table.getNumber(key);
    } catch (NoSuchElementException ex) {
      return defaultValue;
    }
  }

  /**
   * Maps the specified key to the specified value in this table.
   *
   * <p>The key can not be null. The value can be retrieved by calling the get method with a key
   * that is equal to the original key.
   *
   * @deprecated Use{@link #putNumber(java.lang.String, double) putNumber} instead
   * @param key the key
   * @param value the value
   * @throws IllegalArgumentException if key is null
   */
  public static void putDouble(String key, double value) {
    table.putNumber(key, value);
  }

  /**
   * Returns the value at the specified key.
   *
   * @deprecated Use {@link #getNumber(java.lang.String) getNumber} instead
   * @param key the key
   * @return the value
   * @throws TableKeyNotDefinedException if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not a double
   * @throws IllegalArgumentException if the key is null
   */
  public static double getDouble(String key) throws TableKeyNotDefinedException {
    return table.getNumber(key);
  }

  /**
   * Returns the value at the specified key.
   *
   * @deprecated Use {@link #getNumber(java.lang.String, double) getNumber} instead.
   * @param key the key
   * @param defaultValue the value returned if the key is undefined
   * @return the value
   * @throws TableKeyNotDefinedException if there is no value mapped to by the key
   * @throws IllegalArgumentException if the value mapped to by the key is not a double
   * @throws IllegalArgumentException if the key is null
   */
  public static double getDouble(String key, double defaultValue) {
    return table.getNumber(key, defaultValue);
  }
}
 /**
  * Returns the value at the specified key.
  *
  * @deprecated Use {@link #getNumber(java.lang.String) getNumber} instead
  * @param key the key
  * @return the value
  * @throws TableKeyNotDefinedException if there is no value mapped to by the key
  * @throws IllegalArgumentException if the value mapped to by the key is not an int
  * @throws IllegalArgumentException if the key is null
  */
 public static int getInt(String key) throws TableKeyNotDefinedException {
   return (int) table.getNumber(key);
 }
  public static void captureFrame() {
    // ---------These objects allow us to edit the variables used in the scalar polygon
    // recognition------
    JLabel blueMaxValueLabel = new JLabel("Max Blue Value");
    JTextField blueMaxValueField = new JTextField("255.0", 5);

    JLabel blueMinValueLabel = new JLabel("Min Blue Value");
    JTextField blueMinValueField = new JTextField("100.0", 5);

    JLabel greenMaxValueLabel = new JLabel("Max Green Value");
    JTextField greenMaxValueField = new JTextField("255.0", 5);

    JLabel greenMinValueLabel = new JLabel("Min Green Value");
    JTextField greenMinValueField = new JTextField("215.0", 5);

    JLabel redMaxValueLabel = new JLabel("Max Red Value");
    JTextField redMaxValueField = new JTextField("45.0", 5);

    JLabel redMinValueLabel = new JLabel("Min Red Value");
    JTextField redMinValueField = new JTextField("0.0", 5);

    JLabel yLabel = new JLabel("Y");
    JTextField yValue = new JTextField(5);
    boolean hotZone = false;

    // ---------------------------------End object lists-------------------------------------
    String source = "http://*****:*****@10.12.59.11/mjpg/video.mjpg";
    FrameGrabber grabber = new OpenCVFrameGrabber(source); // new VideoInputFrameGrabber(0);
    try {
      Thread.sleep(1000);
    } catch (InterruptedException ex) {
      Logger.getLogger(CaptureImage.class.getName()).log(Level.SEVERE, null, ex);
    }

    CanvasFrame canvas = new CanvasFrame("WebCam");
    canvas.setDefaultCloseOperation(javax.swing.JFrame.EXIT_ON_CLOSE);
    CanvasFrame before = new CanvasFrame("before");
    before.setDefaultCloseOperation(javax.swing.JFrame.HIDE_ON_CLOSE);
    GridBagLayout gridBag = new GridBagLayout();
    before.setLayout(gridBag);
    GridBagConstraints gbc = new GridBagConstraints();
    gbc.fill = GridBagConstraints.VERTICAL;
    gridBag.setConstraints(before.getComponent(0), gbc);
    gbc.fill = GridBagConstraints.NONE;
    gbc.gridx = 1;
    gbc.gridy = 0;
    before.add(blueMaxValueLabel, gbc);
    gbc.gridx = 2;
    before.add(blueMaxValueField, gbc);
    gbc.gridx = 3;
    before.add(blueMinValueLabel, gbc);
    gbc.gridx = 4;
    before.add(blueMinValueField, gbc);
    // before.add(blueMinValueField);
    gbc.gridx = 1;
    gbc.gridy = 2;
    before.add(greenMaxValueLabel, gbc);
    gbc.gridx = 2;
    before.add(greenMaxValueField, gbc);
    gbc.gridx = 3;
    before.add(greenMinValueLabel, gbc);
    gbc.gridx = 4;
    before.add(greenMinValueField, gbc);
    // before.add(greenMinValueField);
    gbc.gridx = 1;
    gbc.gridy = 3;
    before.add(redMaxValueLabel, gbc);
    gbc.gridx = 2;
    before.add(redMaxValueField, gbc);
    gbc.gridx = 3;
    before.add(redMinValueLabel, gbc);
    gbc.gridx = 4;
    before.add(redMinValueField, gbc);
    before.add(yLabel);
    before.add(yValue);
    // before.add(redMinValueField);

    int numOfScreens = 39;
    //        String loadingPic = "C:\\loadingScreen\\loadingScreen"
    //                + (((int) (Math.random() * numOfScreens)) + 1) + ".bmp";
    int failedGrabs = 0;
    NetworkTable.setTeam(
        1259); // Capture image requires the setTeam method because it does not have use
               // Smartdashboard which already assigns a team number in the from of a network table
    NetworkTable.setIPAddress("10.12.59.2");
    NetworkTable.setClientMode();
    NetworkTable.getTable("SmartDashboard").putBoolean("TestTable", hotZone);
    while (true) {
      try {
        //                try {
        //                    IplImage splashScreen = new IplImage(cvLoadImage(loadingPic));
        //                    canvas.showImage(splashScreen);
        //                } catch (Exception e) {
        //                }
        grabber.start();
        IplImage img;
        // IplImage hsv;
        // IplImage canny;
        IplImage displayImg;
        IplImage dst;
        PolygonFinder polyFind = new PolygonFinder();

        //                try {
        //                    NetworkTable.getTable("camera").putNumber("distance", 0);
        //                    NetworkTable.getTable("camera").putBoolean("hotZone", false);
        //                } catch (Exception e) {
        //                }

        while (true) {

          while (true) {
            try {
              // System.out.println("grabbing...");
              img = new IplImage(grabber.grab());
              displayImg = new IplImage(cvCreateImage(img.cvSize(), img.depth(), img.nChannels()));
              cvCopy(img, displayImg, null);
              // System.out.println("Frame GRABBED!");
              break;
            } catch (Exception e) {
              failedGrabs++;
              System.out.println(failedGrabs);
              if (failedGrabs > 10) {
                grabber = new OpenCVFrameGrabber(source);
                grabber.start();
                failedGrabs = 0;
              }
              continue;
            }
          }

          //            PolygonFinder polyFind = new PolygonFinder(img);
          //            ArrayList<PolygonStructure> polygons = polyFind.findPolygons(4);
          //            for (int i = 0; i < polygons.size(); i++) {
          //                polygons.get(i).drawShape(img, CvScalar.MAGENTA);
          //                }
          //            canvas.showImage(img);
          //            return;
          if (img != null) {
            // cvSaveImage("originalcapture.jpg", img);
          }
          // IplImage gray = cvCreateImage(cvGetSize(img), img.depth(), 1 );
          // hsv = new IplImage(cvCreateImage(cvGetSize(img), img.depth(), 3));
          dst = new IplImage(cvCreateImage(cvGetSize(img), img.depth(), 1));
          // cvCvtColor(img, gray, CV_RGB2GRAY);

          // cvCvtColor(img, hsv, CV_BGR2HSV);
          //            if(hsv != null) {
          //                cvSaveImage("hsv.jpg", hsv);
          //            }
          // 30 20 0; 70 140 60
          // 50 175 75 //// 100 255 225
          // cvInRangeS(hsv, cvScalar(0, 200, 0, 0), cvScalar(150, 255, 255, 0), dst);
          // cvDrawLine(img, new CvPoint(0, 360), new CvPoint(639, 360), CvScalar.BLACK, 240, 8, 0);
          // cvInRangeS(img, cvScalar(100, 215, 0, 0), cvScalar(255, 255, 45, 0), dst); This is the
          // original
          // Code used to set max and min values for bgr scale in scalars VVV
          cvInRangeS(
              img,
              cvScalar(
                  (new Double(blueMinValueField.getText())).doubleValue(),
                  (new Double(greenMinValueField.getText())).doubleValue(),
                  (new Double(redMinValueField.getText())).doubleValue(),
                  0),
              cvScalar(
                  (new Double(blueMaxValueField.getText())).doubleValue(),
                  (new Double(greenMaxValueField.getText())).doubleValue(),
                  (new Double(redMaxValueField.getText())).doubleValue(),
                  0),
              dst);
          // NEED TO FLIP MAX IN MIN POSITION, MIN IS IN MAX POSITION

          // cvInRangeS(img, cvScalar(0, 0, 0, 0), cvScalar(255, 255, 255, 0), dst);
          // cvDilate( dst, dst, null, 1 );
          cvSmooth(dst, dst, CV_MEDIAN, 1, 1, 0, 0);
          // cvCanny(gray, dst, 50, 100, 3);
          // cvCvtColor(hsv, gray, CV_RGB2GRAY);
          // canvas.showImage(img);
          // before.showImage(dst);
          IplImage newDst =
              new IplImage(cvCloneImage(img)); // cvCreateImage(cvGetSize(img), img.depth(), 3));
          cvCvtColor(dst, newDst, CV_GRAY2BGR);
          // cvConvexHull2(newDst, null, CV_CLOCKWISE, 0);
          before.showImage(newDst);
          polyFind.setImage(newDst);
          ArrayList<PolygonStructure> polygons = new ArrayList<PolygonStructure>();
          int i;
          // before.showImage(newDst);
          for (i = 4; i < 5; i++) {
            for (PolygonStructure ps : polyFind.findPolygons(i)) {
              polygons.add(ps);
            }
          }
          for (int c = 0; c < polygons.size(); c++) {
            for (int d = 0; d < polygons.size(); d++) {
              if (c == d) {
                break;
              }
              if (polygons
                  .get(c)
                  .getPolygon()
                  .contains(polygons.get(d).getPolygon().getBounds2D())) {
                polygons.remove(d);
                if (d < c) {
                  c--;
                }
                d--;
              }
            }
          }
          for (int c = 0; c < polygons.size(); c++) {
            for (int d = 0; d < polygons.size(); d++) {
              if (c == d) {
                break;
              }
              if (polygons
                  .get(c)
                  .getPolygon()
                  .contains(
                      polygons.get(d).getPolygon().getBounds().getCenterX(),
                      polygons.get(d).getPolygon().getBounds().getCenterY())) {
                polygons.remove(d);
                if (d < c) {
                  c--;
                }
                d--;
              }
            }
          }
          //                    if
          // (tempPoly.getVertex(0).x()<vertices[0].x()&&tempPoly.getVertex(1).x()>vertices[0].x()
          //
          // &&tempPoly.getVertex(1).y()<vertices[0].y()&&tempPoly.getVertex(2).y()>vertices[0].y())
          //                    {
          //                        isInside = true;
          //                        break;
          //                    }
          for (i = 0; i < polygons.size(); i++) {
            if (polygons.size() < 2) {
              hotZone = false;
            } else {
              hotZone = true;
            }
            CvScalar polyColor = CvScalar.MAGENTA;
            switch (i) {
              case 0:
                {
                  polyColor = CvScalar.RED;
                  //                            System.out.println("Center X: "
                  //                                    + (320 - ((polygons.get(i).getVertex(3).x()
                  // + polygons.get(i).getVertex(2).x()) / 2))
                  //                                    + "\tCenter Y: "
                  //                                    + (240 - ((polygons.get(i).getVertex(3).y()
                  // + (polygons.get(i).getVertex(2).y())) / 2)));
                  double x =
                      (320
                          - ((polygons.get(i).getVertex(3).x() + polygons.get(i).getVertex(2).x())
                              / 2));
                  double angle =
                      (480
                          - ((polygons.get(i).getVertex(3).y() + (polygons.get(i).getVertex(2).y()))
                              / 2));
                  // double distance = 5182.2043151825 * Math.pow(angle, -1);

                  double distance = 514.7318 * Math.pow(angle - 220, -1.2);
                  // double distance;
                  if (angle < 317) {
                    distance = -0.370786516853933 * angle + 133.977528089888;
                  } else if (angle > 316 && angle < 325) {
                    distance = -0.184697808038669 * angle + 74.9375184489134;
                  } else if (angle > 324 && angle < 362) {
                    distance = -0.140145789191636 * angle + 60.5198480748917;
                  } else if (angle > 360) {
                    distance = -0.0702258215380136 * angle + 35.3150512441271;
                  }

                  for (int c = 0; c < 4; c++) {
                    System.out.println("Vertex " + c + ":" + polygons.get(i).getVertex(c).y());
                  }
                  yValue.setText("" + angle);

                  // hotZone = true;

                  break;
                }
              case 1:
                {
                  // Hot zone
                  polyColor = CvScalar.BLUE;
                  break;
                }
              case 2:
                {
                  polyColor = CvScalar.GREEN;
                  break;
                }
              case 3:
                {
                  polyColor = CvScalar.YELLOW;
                  break;
                }
            }
            // polygons.get(i).drawShape(img, polyColor);
            cvLine(
                displayImg,
                polygons.get(i).getVertices()[3],
                polygons.get(i).getVertices()[0],
                polyColor,
                3,
                CV_AA,
                0);
            cvLine(
                displayImg,
                polygons.get(i).getVertices()[0],
                polygons.get(i).getVertices()[1],
                polyColor,
                3,
                CV_AA,
                0);
            cvDrawCircle(displayImg, polygons.get(i).getVertices()[0], 3, CvScalar.GRAY, 1, 8, 0);
            cvLine(
                displayImg,
                polygons.get(i).getVertices()[1],
                polygons.get(i).getVertices()[2],
                polyColor,
                3,
                CV_AA,
                0);
            cvDrawCircle(
                displayImg, polygons.get(i).getVertices()[1], 3, CvScalar.MAGENTA, 1, 8, 0);
            cvLine(
                displayImg,
                polygons.get(i).getVertices()[2],
                polygons.get(i).getVertices()[3],
                polyColor,
                3,
                CV_AA,
                0);
            cvDrawCircle(displayImg, polygons.get(i).getVertices()[2], 3, CvScalar.BLACK, 1, 8, 0);
            cvDrawCircle(displayImg, polygons.get(i).getVertices()[3], 3, CvScalar.CYAN, 1, 8, 0);
            // System.out.println("Polygon " + i + "\t" + polygons.get(i).getVertices()[0]);
          }

          try {
            // NetworkTable.getTable("camera").beginTransaction();
            //                        NetworkTable.getTable("camera").putNumber("distance",
            // distance);
            //                        NetworkTable.getTable("camera").putNumber("x", x);
            NetworkTable.getTable("camera").putBoolean("hotZone", hotZone);
            // NetworkTable.getTable("camera").endTransaction();

          } catch (Exception e) {
          }

          if (displayImg != null) {
            // cvSaveImage("aftercapture.jpg", dst);
            cvDrawLine(
                displayImg, new CvPoint(300, 240), new CvPoint(340, 240), CvScalar.WHITE, 2, 8, 0);
            cvDrawLine(
                displayImg, new CvPoint(320, 220), new CvPoint(320, 260), CvScalar.WHITE, 2, 8, 0);
            canvas.showImage(displayImg);
          } else {
            // System.out.println("Null Image");
          }

          // cvReleaseImage(gray);
          cvReleaseImage(newDst);
          // cvReleaseImage(img);
          // cvReleaseImage(hsv);
          cvReleaseImage(displayImg);
          cvReleaseImage(dst);
          // Thread.sleep(50);
        }
        // System.out.println("5");
        // grabber.stop();
      } catch (Exception e) {
        e.printStackTrace();
      }
      grabber = new OpenCVFrameGrabber(source);
    }
  }
 /**
  * Maps the specified key to the specified value in this table.
  *
  * <p>The key can not be null. The value can be retrieved by calling the get method with a key
  * that is equal to the original key.
  *
  * @deprecated Use {@link #putNumber(java.lang.String, double) putNumber method} instead
  * @param key the key
  * @param value the value
  * @throws IllegalArgumentException if key is null
  */
 public static void putInt(String key, int value) {
   table.putNumber(key, value);
 }
 public static void removeConnectionListener(IRemoteConnectionListener listener) {
   System.out.println("Removing connection listener");
   NetworkTable.getTable(TABLE_NAME).removeConnectionListener(listener);
 }
 /**
  * Maps the specified key to the specified value in this table. The key can not be null. The value
  * can be retrieved by calling the get method with a key that is equal to the original key.
  *
  * @param key the key
  * @param value the value
  * @throws IllegalArgumentException if key is null
  */
 public static void putBoolean(String key, boolean value) {
   table.putBoolean(key, value);
 }
 /**
  * Returns the value at the specified key.
  *
  * @param key the key
  * @return the value
  * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
  * @throws IllegalArgumentException if the value mapped to by the key is not a boolean
  * @throws IllegalArgumentException if the key is null
  */
 public static boolean getBoolean(String key) throws TableKeyNotDefinedException {
   return table.getBoolean(key);
 }
 /**
  * Returns the value at the specified key.
  *
  * @deprecated Use {@link #getNumber(java.lang.String) getNumber} instead
  * @param key the key
  * @return the value
  * @throws TableKeyNotDefinedException if there is no value mapped to by the key
  * @throws IllegalArgumentException if the value mapped to by the key is not a double
  * @throws IllegalArgumentException if the key is null
  */
 public static double getDouble(String key) throws TableKeyNotDefinedException {
   return table.getNumber(key);
 }
 /**
  * Returns the value at the specified key.
  *
  * @deprecated Use {@link #getNumber(java.lang.String, double) getNumber} instead.
  * @param key the key
  * @param defaultValue the value returned if the key is undefined
  * @return the value
  * @throws TableKeyNotDefinedException if there is no value mapped to by the key
  * @throws IllegalArgumentException if the value mapped to by the key is not a double
  * @throws IllegalArgumentException if the key is null
  */
 public static double getDouble(String key, double defaultValue) {
   return table.getNumber(key, defaultValue);
 }
 /**
  * Maps the specified key to the specified value in this table. Neither the key nor the value can
  * be null. The value can be retrieved by calling the get method with a key that is equal to the
  * original key.
  *
  * @param key the key
  * @param value the value
  * @throws IllegalArgumentException if key or value is null
  */
 public static void putString(String key, String value) {
   table.putString(key, value);
 }
 /**
  * Returns the value at the specified key.
  *
  * @param key the key
  * @return the value
  * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
  * @throws IllegalArgumentException if the value mapped to by the key is not a string
  * @throws IllegalArgumentException if the key is null
  */
 public static String getString(String key) throws TableKeyNotDefinedException {
   return table.getString(key);
 }
 /**
  * Returns the value at the specified key.
  *
  * @param key the key
  * @param defaultValue The value returned if the key is undefined
  * @return the value
  * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
  * @throws IllegalArgumentException if the value mapped to by the key is not a string
  * @throws IllegalArgumentException if the key is null
  */
 public static String getString(String key, String defaultValue) {
   return table.getString(key, defaultValue);
 }
 /**
  * Returns the value at the specified key.
  *
  * @param key the key
  * @param defaultValue returned if the key doesn't exist
  * @return the value
  * @throws IllegalArgumentException if the value mapped to by the key is not a boolean
  * @throws IllegalArgumentException if the key is null
  */
 public static boolean getBoolean(String key, boolean defaultValue) {
   return table.getBoolean(key, defaultValue);
 }
 public static void addConnectionListener(
     IRemoteConnectionListener listener, boolean immediateNotify) {
   System.out.println("Adding connection listener");
   NetworkTable.getTable(TABLE_NAME).addConnectionListener(listener, immediateNotify);
 }