Peg Lift Tracking

robot.java

package org.usfirst.frc.team496.robot;

import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;

//import com.kauailabs.navx.frc.AHRS;

import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.HttpCamera;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.vision.VisionThread;

public class Robot extends SampleRobot {
	// RobotDrive myRobot = new RobotDrive(0, 1, 2, 3);
	RobotDrive myRobot = new RobotDrive(0, 1);

	// Talon climbingMotor = new Talon(4);
	// Joystick stick = new Joystick(0);
	XboxController xbox = new XboxController(1);
	final String defaultAuto = "Default";
	final String customAuto = "My Auto";
	SendableChooser<String> chooser = new SendableChooser<>();

	PowerDistributionPanel pdp;

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

	private VisionThread pegVisionThread;
	private double centerX = 0.0;
	private final Object imgLock = new Object();
	private boolean hasTarget;
	private double prevTurn;
	private double targetDistance;
	private double distance;

	public Robot() {

		pdp = new PowerDistributionPanel();

		HttpCamera camera = CameraServer.getInstance().addAxisCamera("10.4.96.20");
		camera.setResolution(IMG_WIDTH, IMG_HEIGHT);
		camera.setFPS(15);

		CvSink cvSink = CameraServer.getInstance().getVideo();
		CvSource outputStream = CameraServer.getInstance().putVideo("Peg Vision", IMG_WIDTH, IMG_HEIGHT);
		Mat source = new Mat();
		pegVisionThread = new VisionThread(camera, new PegPipeline(), pipeline -> {

			cvSink.grabFrame(source);

			if (pipeline.filterContoursOutput().size() == 2) {

				Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput().get(0));
				Rect r1 = Imgproc.boundingRect(pipeline.filterContoursOutput().get(1));

				Imgproc.rectangle(source, new Point(r.x, r.y), new Point(r.x + r.width, r.y + r.height),
						new Scalar(0, 0, 255), 2);
				Imgproc.rectangle(source, new Point(r1.x, r1.y), new Point(r1.x + r1.width, r1.y + r1.height),
						new Scalar(0, 0, 255), 2);
				outputStream.putFrame(source);

				synchronized (imgLock) {
					centerX = (r.x + (r1.x + r1.width)) / 2;
					targetDistance = r1.x - r.x;
					distance = 0.68 * IMG_WIDTH / (2 * targetDistance * Math.tan(37.4));
					hasTarget = true;
					SmartDashboard.putNumber("CenterX", centerX);
					// System.out.println(centerX);

				}
			} else {
				synchronized (imgLock) {
					hasTarget = false;

				}
				outputStream.putFrame(source);
			}

		});
		pegVisionThread.setDaemon(true);
		pegVisionThread.start();

		LiveWindow.addSensor("PowerSystem", "Current", pdp);

		LiveWindow.run();

	}

	@Override
	public void robotInit() {
		chooser.addDefault("Default Auto", defaultAuto);
		chooser.addObject("My Auto", customAuto);
		SmartDashboard.putData("Auto modes", chooser);
	}

	@Override
	public void autonomous() {

		myRobot.setSafetyEnabled(false);
		double centerX;
		double turnRate;
		boolean hasTarget;
		double distance;

		while (isAutonomous() && isEnabled()) {

			synchronized (imgLock) {
				centerX = this.centerX;
				distance = this.targetDistance;
				hasTarget = this.hasTarget;

				// System.out.println(centerX);
			}
			// double distance = Fft*FOVpixel/(2Tpixel*Math.tan(37.4)) ;
			// double distance = 0.68 * IMG_WIDTH / (2 * targetDistance *
			// Math.tan(37.4));
			// double distance = 142.304 * 1/targetDistance;
			// distance = distance - 3.45;

			if (hasTarget && distance < 0) {

				double turn = centerX - (IMG_WIDTH / 2);
				prevTurn = turn;
				if (centerX == 0.0) {
					turnRate = 0.0;
				} else {
					turnRate = -turn * 0.008;
				}

				myRobot.arcadeDrive(-0.8, turnRate);
			} else {
				myRobot.arcadeDrive(0, -prevTurn * .008);
			}

		}
	}

	@Override
	public void operatorControl() {
		myRobot.setSafetyEnabled(true);
		while (isOperatorControl() && isEnabled()) {

		}
	}

	@Override
	public void test() {
		LiveWindow.run();
		System.out.println("Current of 14: " + pdp.getCurrent(14));
	}

}