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