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);
}
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;
}
}
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);
}