Vision

Vision testing has begun. Looking at using GRIP

[todo: insert image of vision workflow in GRIP]

untested code for getting lines:

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

import java.util.ArrayList;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;

/**
 * jumpman jumpman jumpman them boys up to something
 */
public class VisionNetwork extends Subsystem {
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
	
	NetworkTable table;
	
	public VisionNetwork() {
		table = NetworkTable.getTable("GRIP/linesReport");
	}

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    // i wish we had ez pz json in java
    
    /**
     * kinda a nasty way to get lines. returns a double[][]
     * lines[0] -> double[] of lengths
     * lines[1] -> double[] of x1
     * lines[2] -> double[] of x2
     * lines[3] -> double[] of y1
     * lines[4] -> double[] of y2
     * lines[5] -> double[] of angles
     * 
     * JSON would make this 1000x easier...
     * @return double[][] lines
     */
    
    public double[][] getLines() {
    	
    	double[] defaultValue = new double[0];
    	
    	double[] lengths = table.getNumberArray("length", defaultValue);
    	double[] x1s = table.getNumberArray("x1", defaultValue);
    	double[] x2s = table.getNumberArray("x2", defaultValue);
    	double[] y1s = table.getNumberArray("y1", defaultValue);
    	double[] y2s = table.getNumberArray("y2", defaultValue);
    	double[] angles = table.getNumberArray("angle", defaultValue);
    	
    	double[][] ultimate = {lengths, x1s, x2s, y1s, y2s, angles};
    	
    	return ultimate;
    }
}