Vision

[verlander13]

Dos this code work?

package org.usfirst.frc.team1025.robot.subsystems;

import org.opencv.core.Rect;
import org.opencv.imgproc.Imgproc;
import org.usfirst.frc.team1025.robot.GripPipeline;

import com.ctre.CANTalon;

import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.vision.VisionRunner;
import edu.wpi.first.wpilibj.vision.VisionThread;

public class Vision extends Subsystem {

public static final int IMG_WIDTH = 320;
private static final int IMG_HEIGHT = 240;

private VisionThread visionThread;
UsbCamera camera;
public double centerX = 0.0;
//public RobotDrive drive;

public final Object imgLock = new Object();

public Vision() {
camera = CameraServer.getInstance().startAutomaticCapture() ;
// camera.setResolution(640, 480);
// CvSink cvSink = CameraServer.getInstance().getVideo();
// CvSource cvSource = CameraServer.getInstance().putVideo("Rectangle", IMG_WIDTH, IMG_HEIGHT);


visionThread = new VisionThread(camera, new GripPipeline(), pipeline -> {
if (!pipeline.filterContoursOutput().isEmpty()) {
Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput ().get(0));
synchronized (imgLock) {
centerX = r.x + (r.width / 2);
}
}
});
visionThread.start();

//drive = new RobotDrive(1, 2);
}

public void initDefaultCommand() {

}

public Object getImgLock() {
return imgLock;
}

public double centerX() {
return centerX;
}

public int imageWidth() {
return IMG_WIDTH;
}
public int imageHeight(){
return IMG_HEIGHT;
}