[Lesafian]

Hi, everyone. I am currently the only programmer for my team, and we have no programming mentors, so bare with me. This is our 2nd year, and we're making an attempt at vision processing. We're going to use GRIP for our vision processing, and a LifeCam-3000. I've been very confused as to how to use the values from NetworkTables. I decided to tinker around and just start programming. I think I may have came up with something that makes logical sense, and I'm asking for your input.

package org.usfirst.frc.team6077.robot;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends SampleRobot {
	NetworkTable table = NetworkTable.getTable("GRIP/myContoursReport");
	VictorSP leftFront, leftBack, rightFront, rightBack, shooter, shooterPaddle1, shooterPaddle2;
	ADXRS450_Gyro gyro = new ADXRS450_Gyro();
	RobotDrive drive;
	Joystick logitech = new Joystick(0);
	SendableChooser<String> chooser = new SendableChooser<>();
	static double kp = 0.03;
	static final String highGoal = "High Goal";
	static final String gear = "Gear";

	// Vision Processing
	double[] centerX;
	double[] centerY;
	public String shotReady = "Shot Ready";
	int gripTolerance = 3;
	int camWidth = 640;
	int camHeight = 480;
	int imageCenter = (camWidth / 2) + (camHeight / 2);
	boolean isShotReady;

	@Override
	public void robotInit() {
		shooter = new VictorSP(0);
		leftFront = new VictorSP(1);
		leftBack = new VictorSP(2);
		rightFront = new VictorSP(3);
		rightBack = new VictorSP(4);
		drive = new RobotDrive(leftFront, leftBack, rightFront, rightBack);
		drive.setExpiration(0.1);
		gyro.calibrate();
		SmartDashboard.putData("Auto modes", chooser);
		this.updateDashboard();

	}

	@Override
	public void autonomous() {
		String autoSelected = chooser.getSelected();
		System.out.println("Auto selected: " + autoSelected);

		switch (autoSelected) {
		case (gear):
			// Put gear onto the peg, and cross the line
			break;
		case (highGoal):
			// Cross the line, shoot balls into the High Goal
			break;
		default:
			// Drive Straight
			break;
		}
	}

	@Override
	public void operatorControl() {
		drive.setSafetyEnabled(false);
		gyro.reset();
		// drive.setMaxOutput(0.5);
		while (isOperatorControl() && isEnabled()) {
			SmartDashboard.putNumber("Gyro Angle", gyro.getAngle());
			drive.mecanumDrive_Cartesian(logitech.getY(), logitech.getX(), logitech.getTwist(), 0);
			if (logitech.getRawButton(1)) {
				shooter.set(0.55);
			}
			Timer.delay(0.005);
			
			if (logitech.getRawButton(3)) {
				this.lineUp();
			}
		}
	}

	public void lineUp() {

		this.updateDashboard();

		if (!isShotReady) {
			if (centerX[0] < imageCenter) {
				drive.drive(0.3, 0.8);
			} else if (centerX[0] > imageCenter) {
				drive.drive(0.3, -0.8);
			}

			if (centerY[0] < imageCenter) {
				drive.drive(1, 0);
			} else if (centerY[0] > imageCenter) {
				drive.drive(-1, 0);
			}
		} else {
			SmartDashboard.putString(shotReady, "true");
		}

	}

	public void updateDashboard() {
		double[] gripValuesX = new double[0];
		double[] gripValuesY = new double[0];
		centerX = table.getNumberArray("centerX", gripValuesX);
		centerY = table.getNumberArray("centerY", gripValuesY);
		SmartDashboard.putString("centerX", centerX.toString());
		SmartDashboard.putString("centerY", centerY.toString());
		try {
			if (centerX[0] + gripTolerance > imageCenter && centerX[0] - gripTolerance < imageCenter
					&& centerY[0] + gripTolerance > imageCenter && centerY[0] - gripTolerance < imageCenter) {
				isShotReady = true;
			} else {
				isShotReady = false;
			}
		} catch (Exception e) {
			SmartDashboard.putString(shotReady, "Error: No sight");
		}
	}

	@Override
	public void test() {
	}
}

My GRIP file is using findContours to draw a white line around the (assuming that we're using retroreflective tape) green rectangle.

The way I assume it works is that the value centerX is the center x value of said contour, and the centerY value is the center y value of said contour. Therefore I must match up (with a 3 pixel room for error) the center of the box with the center of my camera.

This is just what I assume, clarification would be amazing, because I'm rather confused right now.

Thank you!