Library of Gongolierium Test

Robot.java

package org.usfirst.frc.team5112.robot;

import org.opencv.core.Range;

import com.thegongoliers.geometry.Point;
import com.thegongoliers.geometry.Pose;
import com.thegongoliers.geometry.Quaternion;
import com.thegongoliers.input.EnhancedXboxController;
import com.thegongoliers.input.camera.Camera;
import com.thegongoliers.input.camera.LEDColor;
import com.thegongoliers.input.camera.MicrosoftLifeCam;
import com.thegongoliers.input.camera.TargetNotFoundException;
import com.thegongoliers.input.camera.TargetSpecifications;
import com.thegongoliers.input.camera.TargetTF;
import com.thegongoliers.math.TF;
import com.thegongoliers.input.camera.Camera.TargetReport;
import com.thegongoliers.output.EnhancedRobotDrive;
import com.thegongoliers.output.EnhancedSolenoid;
import com.thegongoliers.output.Relay;

import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Talon;

public class Robot extends IterativeRobot {

	EnhancedRobotDrive drivetrain;
	EnhancedXboxController xbox;
	Camera camera;
	TF transformMap;

	public void robotInit() {
		AnalogGyro gyro = new AnalogGyro(0);
		gyro.initGyro();

		drivetrain = new EnhancedRobotDrive.Builder(new Talon(0), new Talon(1))
				.addStabilization(gyro)
				.addTransmission(getTransmission())
				.build();

		camera = new Camera.Builder(new MicrosoftLifeCam(0))
				.addTargetSpecification("High Goal", getHighGoalSpecs())
				.build();

		xbox = new EnhancedXboxController(0);

		transformMap = new TF();
		// The camera is offset from the center of the robot
		transformMap.put("camera", new Pose(new Point(4, 5, 2), Quaternion.zero)); 
	}

	public void autonomousInit() {

	}

	public void autonomousPeriodic() {
		drivetrain.forward(1);
	}

	@Override
	public void teleopInit() {
		drivetrain.stop();

		// Target locating and transformation to the origin
		new Thread(() -> {
			while (isEnabled() && isOperatorControl()) {
				try {
					TargetReport target = camera.findTarget("High Goal", 5);
					if (target.confidence() >= 75) {
						TargetReport targetFromCenterOfRobot = TargetTF.transform(target, transformMap, "camera");
						double angle = targetFromCenterOfRobot.angle();
						double distance = targetFromCenterOfRobot.distance();
						// do something cool
					}

				} catch (TargetNotFoundException e) {
					e.printStackTrace();
				}
			}
		}).start();
	}

	public void teleopPeriodic() {
		drivetrain.arcadeStabilized(xbox.getLeftTrigger() + xbox.getRightTrigger(), xbox.getRightX());
		
		if (xbox.getStartButton() && drivetrain.isInLowGear()) {
			drivetrain.highGear();
		} else if (xbox.getBackButton() && drivetrain.isInHighGear()) {
			drivetrain.lowGear();
		}
		
	}

	public void testPeriodic() {

	}

	private TargetSpecifications getHighGoalSpecs() {
		return new TargetSpecifications() {

			@Override
			public double getWidth() {
				return 20;
			}

			@Override
			public Range getValue() {
				return LEDColor.Green.getValue();
			}

			@Override
			public Range getSaturation() {
				return LEDColor.Green.getSaturation();
			}

			@Override
			public Range getHue() {
				return LEDColor.Green.getHue();
			}

			@Override
			public double getHeight() {
				return 12;
			}

			@Override
			public double getArea() {
				return 96;
			}
		};
	}

	private Relay getTransmission() {
		EnhancedSolenoid solenoid = new EnhancedSolenoid(0);
		return new Relay() {

			@Override
			public Relay on() {
				solenoid.extend();
				return this;
			}

			@Override
			public Relay off() {
				solenoid.retract();
				return this;
			}

			@Override
			public boolean isOn() {
				return solenoid.isExtended();
			}

			@Override
			public boolean isOff() {
				return solenoid.isRetracted();
			}
		};
	}

}