Other changes sitting on delldell-2

9 files changed, 211 insertions, 285 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
index 756c33d..5cba22f 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
@@ -14,18 +14,24 @@ public class Autonomous {
 	}
 
 	public Control run(Control c) {
-		if (robot.armL.get()) {
-			c.arm = -0.5;
-		} else {
-			c.arm = 0;
-			robot.armE.reset();
-		}
 
-		if (time.get() < 4) {
-			c.lDrive1 = c.rDrive1 = -0.4;
-		} else {
-			c.lDrive1 = c.rDrive1 = 0;
+//		if (time.get() < 5) {
+//			c.lDrive1 = c.lDrive2 = 0.2;
+//			c.rDrive1 =c.rDrive2 = -0.2;
+//		} else {
+//			c.lDrive1 = c.rDrive1 = 0;
+//			c.lDrive2 = c.rDrive2 = 0;
+//		}
+		if(Math.abs(robot.driveE.getRaw()) > 1){
+			c.lDrive1 = c.lDrive2 = 0;
+			c.rDrive1 =c.rDrive2 = 0;
+		}
+		else
+		{
+			c.lDrive1 = c.lDrive2 = 0.2;
+			c.rDrive1 =c.rDrive2 = 0.2;
 		}
+		
 		SmartDashboard.putNumber("autoTime", time.get());
 
 		return c;
diff --git a/src/org/usfirst/frc/team4272/robot2016/Control.java b/src/org/usfirst/frc/team4272/robot2016/Control.java
index 40fcb5a..4c9be6f 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Control.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Control.java
@@ -2,12 +2,14 @@ package org.usfirst.frc.team4272.robot2016;
 
 public class Control {
 	double lDrive1 = 0,
-	       lDrive2 = 0,
+	       lDrive2 = 0, 
 	       rDrive1 = 0,
 	       rDrive2 = 0,
 	       fBall   = 0,
 	       bBall   = 0,
-	       arm     = 0;
-
+	       arm     = 0,
+	camRotate = 0,
+	camTilt = 0;
+	boolean PCM = false;
 	public Control() {}
 }
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwOI.java b/src/org/usfirst/frc/team4272/robot2016/HwOI.java
index fc5cc78..6ae63eb 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwOI.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwOI.java
@@ -5,7 +5,5 @@ import org.usfirst.frc.team4272.robotlib.Xbox360Controller;
 import edu.wpi.first.wpilibj.Joystick;
 
 public class HwOI {
-	public Joystick lStick = new Joystick(0);
-	public Joystick rStick = new Joystick(1);
-	public Xbox360Controller xbox = new Xbox360Controller(2);
+	public Xbox360Controller xbox = new Xbox360Controller(1);
 }
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
index e61d87f..d0108b7 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -9,8 +9,11 @@ import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.Talon;
+import edu.wpi.first.wpilibj.Jaguar;
+import edu.wpi.first.wpilibj.Solenoid;
 
 import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
+import org.usfirst.frc.team4272.robotlib.PIDServo;
 
 public class HwRobot {
 	/* Relay == a Spike */
@@ -33,27 +36,34 @@ public class HwRobot {
 	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
 
 	private final PIDOutput
-	    rDriveM1 = new Talon(/*PWM*/2), /* PDP 2, 3 */
-	    rDriveM2 = new Talon(/*PWM*/4), /* PDP 4 */
-	    lDriveM1 = new Talon(/*PWM*/1), /* PDP 12, 13 */
-	    lDriveM2 = new Talon(/*PWM*/0), /* PDP 11 */
-	    armM     = new Talon(/*PWM*/8), /* PDP 15 */
-	    rfBallM  = new Talon(/*PWM*/3), /* PDP 0 */
-	    lfBallM  = new Talon(/*PWM*/9), /* PDP 14 */
-	    fBallM   = new PIDOutputSplitter(rfBallM, lfBallM),
-	    bBallM   = new Talon(/*PWM*/6); /* PDP 10, 5 */
-
-	public final Encoder   armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2, true);
+	    rDriveM1 = new Jaguar(/*PWM*/0), /* PDP 2, 3 */
+	    rDriveM2 = new Jaguar(/*PWM*/1), /* PDP 4 */
+	    lDriveM1 = new Jaguar(/*PWM*/2), /* PDP 12, 13 */
+	    lDriveM2 = new Jaguar(3),/*PWM*/ /* PDP 11 */
+	    //armM     = new Talon(/*PWM*/8), /* PDP 15 */
+	    //rfBallM  = new Talon(/*PWM*/4), /* PDP 0 */
+	    //lfBallM  = new Talon(/*PWM*/9), /* PDP 14 */
+	    //fBallM   = new PIDOutputSplitter(rfBallM, lfBallM),
+	    //bBallM   = new Talon(/*PWM*/6),/* PDP 10, 5 */
+	    camRotate = new PIDServo(8),
+	    camTilt = new PIDServo(9);
+		
+	
+	
+	//public final Encoder   driveE = new Encoder(/*DIO*/0, /*DIO*/1, true);
+	public final Encoder driveE = new Encoder(2,3,true);
 	public final DigitalInput armL = new DigitalInput(/*DIO*/3);
 	public final DigitalInput ballL = new DigitalInput(/*DIO*/4);
 	public final Encoder   rDriveE = new Encoder(/*DIO*/5,/*DIO*/6);
 	public final Encoder   lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true);
 	public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
 
+	private final Solenoid PCM = new Solenoid(/*PCM*/ 0);
+
 	public HwRobot() {
-		armE.setPIDSourceType(PIDSourceType.kDisplacement);
-		lDriveE.setPIDSourceType(PIDSourceType.kRate);
-		rDriveE.setPIDSourceType(PIDSourceType.kRate);
+		driveE.setPIDSourceType(PIDSourceType.kDisplacement);
+//		lDriveE.setPIDSourceType(PIDSourceType.kRate);
+//		rDriveE.setPIDSourceType(PIDSourceType.kRate);
 		PDP.initTable(NetworkTable.getTable("PDP"));
 	}
 
@@ -62,23 +72,30 @@ public class HwRobot {
 		lDriveM2.pidWrite( c.lDrive2);
 		rDriveM1.pidWrite(-c.rDrive1);
 		rDriveM2.pidWrite(-c.rDrive2);
-		armM.pidWrite(c.arm * 0.5);
-		fBallM.pidWrite(-c.fBall);
-		bBallM.pidWrite(-c.bBall);
-
+		//armM.pidWrite(c.arm * 0.5);
+		//fBallM.pidWrite(-c.fBall);
+		//bBallM.pidWrite(-c.bBall);
+		camRotate.pidWrite(c.camRotate);
+		camTilt.pidWrite(c.camTilt);
+		PCM.set(c.PCM);
 		SmartDashboard.putNumber("fBall", c.fBall);
 		SmartDashboard.putNumber("bBall", c.bBall);
 		SmartDashboard.putNumber("armM", c.arm);
 
-		SmartDashboard.putNumber("armE", armE.pidGet());
-		SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
-		SmartDashboard.putNumber("lDriveE rate", lDriveE.getRate());
-		SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
-		SmartDashboard.putNumber("rDriveE rate", rDriveE.getRate());
-		SmartDashboard.putBoolean("ballL", ballL.get());
-		SmartDashboard.putBoolean("armL", armL.get());
+		System.out.println("driveEncoder: " + driveE.getRaw());
+		System.out.println("driveEncoder: " + driveE.getRate());
+		System.out.println("driveEncoder: " + driveE.pidGet());
+		
+		SmartDashboard.putNumber("armE", driveE.getRaw());
+//		SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
+//		SmartDashboard.putNumber("lDriveE rate", lDriveE.getRate());
+//		SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
+//		SmartDashboard.putNumber("rDriveE rate", rDriveE.getRate());
+//		SmartDashboard.putBoolean("ballL", ballL.get());
+//		SmartDashboard.putBoolean("armL", armL.get());
 
 		PDP.updateTable();
 		SmartDashboard.putData("PDP", PDP);
 	}
+	
 }
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index 73b7b60..ed86427 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -6,6 +6,7 @@ import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
 import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;
 import org.usfirst.frc.team4272.robotlib.PushButton;
 
+
 public class Teleop {
 	private final HwRobot robot;
 	private final double armSoftStop = 95;//295;
@@ -15,49 +16,63 @@ public class Teleop {
 
 	public Teleop(HwRobot robot) {
 		this.robot = robot;
-		robot.armE.reset();
+//		robot.armE.reset();
 	}
 
-	private static double jsScale(Joystick j) {
-		double y = -j.getY();/* +:forward; -:backward */
-		double z = -j.getZ();/* +:more-sensitive; -:less-sensitive */
-		return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
-	}
-
-	private static double bound(double min, double v, double max) {
-		return Math.min(Math.max(v, min), max);
-	}
+//	private static double jsScale(Joystick j) {
+//		double y = -j.getY();/* +:forward; -:backward */
+//		double z = -j.getZ();/* +:more-sensitive; -:less-sensitive */
+//		return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
+//	}
+//
+//	private static double bound(double min, double v, double max) {
+//		return Math.min(Math.max(v, min), max);
+//	}
 
 	private static double oneIf(boolean b) {
 		return b ? 1 : 0;
 	}
 
+	//boolean DriverControl = true;
 	public Control run(Control control, HwOI oi) {
 		/* Drive */
-		control.lDrive1 = jsScale(oi.lStick);
-		control.rDrive1 = jsScale(oi.rStick);
-		control.lDrive2 = oi.lStick.getTrigger() ? control.lDrive1 : 0;
-		control.rDrive2 = oi.rStick.getTrigger() ? control.rDrive1 : 0;
-		control.arm = -oi.xbox.getAxis(Axis.LY);
+		/*if (oi.lStick.getButton(Joystick.ButtonType.kTop) || oi.rStick.getButton(Joystick.ButtonType.kTop)) {
+			DriverControl =! DriverControl;
+			System.out.println("switched");
+		}*/
+		if (oi.xbox.getAxis(Axis.LY) > 0.2) {
+			control.lDrive1 = (oi.xbox.getAxis(Axis.LY));
+			control.lDrive2 = (oi.xbox.getAxis(Axis.LY));
+		}
+		if (oi.xbox.getAxis(Axis.RY) > 0.2) {
+			control.rDrive1 = (oi.xbox.getAxis(Axis.RY));
+			control.rDrive2 = (oi.xbox.getAxis(Axis.RY));
+		}
+		//control.arm = -oi.xbox.getAxis(Axis.LY);
+		/*if (DriverControl == true && Math.abs(oi.lStick.getAxis(Joystick.AxisType.kY)) <= 0.1 && (Math.abs(oi.rStick.getAxis(Joystick.AxisType.kY)) <= 0.1) ){
+			control.lDrive1 = 0;
+			control.rDrive1 = 0;
+			System.out.println("stopped");
+		}*/
 		if (control.arm < 0)
 			control.arm = control.arm*0.2;
 		
-		double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger);
-		double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B));
+		//double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger);
+		//double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B));
 
-		boolean ballL = robot.ballL.get();
-		control.fBall = bound(-oneIf(ballL), fBall < 0 ? 0.5*fBall : fBall, 1);
-		control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.5*bBall : bBall, 1);
+		//boolean ballL = robot.ballL.get();
+		//control.fBall = bound(-oneIf(ballL), fBall < 0 ? 0.5*fBall : fBall, 1);
+		//control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.5*bBall : bBall, 1);
 
 		/* I'm eyeballing 10 degrees ≈ 50 clicks */
-		if (robot.armE.pidGet() < 50 && !robot.armL.get()) {
-			robot.armE.reset();
-		}
+//		if (robot.armE.pidGet() < 50 && !robot.armL.get()) {
+//			robot.armE.reset();
+//		}
 
-		if (oi.xbox.getButton(Button.LBumper))
-			control.arm = 1-(robot.armE.pidGet()/armSoftStop);
-		else if (oi.xbox.getButton(Button.RBumper))
-			control.arm = robot.armE.pidGet()/armSoftStop;
+//		if (oi.xbox.getButton(Button.LBumper))
+//			control.arm = 1-(robot.armE.pidGet()/armSoftStop);
+//		else if (oi.xbox.getButton(Button.RBumper))
+//			control.arm = robot.armE.pidGet()/armSoftStop;
 
 		if (shootButton.update(oi.xbox.getButton(Button.X))) {
 			time.reset();
@@ -74,8 +89,26 @@ public class Teleop {
 				control.fBall = 1;
 			}
 		}
+		
+		if(oi.xbox.getAxis(Axis.RTrigger) > 0)
+			control.camRotate = 1;
+		else if(oi.xbox.getAxis(Axis.LTrigger) > 0)
+			control.camRotate = -1;
+		else
+			control.camRotate = 0;
+		
+//		if(Math.abs(oi.xbox.getAxis(Axis.RY)) > 0.2)
+//			control.camTilt = oi.xbox.getAxis(Axis.RY);
+//		else
+//			control.camTilt = 0;
+
+//		if(oi.xbox.getButton(Button.A)); /* Don't delete me!!*/ 
+//			control.setSolnoid(true)
+//		else:
+//			control.PCM.
+			
 
-		//if (robot.armE.pidGet() > armSoftStop && control.arm > 0)
+		//if (robot.armE.piedGet() > armSoftStop && control.arm > 0)
 		//	control.arm = 0;
 
 		/* Take pictures */
diff --git a/src/org/usfirst/frc/team4272/robotlib/PIDController.java b/src/org/usfirst/frc/team4272/robotlib/PIDController.java
deleted file mode 100644
index 13f5666..0000000
--- a/src/org/usfirst/frc/team4272/robotlib/PIDController.java
+++ /dev/null
@@ -1,216 +0,0 @@
-/**
- * Copyright (c) FIRST 2008-2012. All Rights Reserved.
- * Open Source Software - may be modified and shared by FRC teams. The code
- * must be accompanied by the FIRST BSD license file in the root directory of
- * the project.
- *
- * Copyright (c) 2011-2012, 2015-2016 Luke Shumaker
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the FIRST nor the
- *       names of its contributors may be used to endorse or promote products
- *       derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
- * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
- * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
- * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * @author Luke Shumaker <lukeshu@sbcglobal.net>
- */
-package org.usfirst.frc.team4272.robotlib;
-
-import edu.wpi.first.wpilibj.PIDOutput;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.SpeedController;
-//import edu.wpi.first.wpilibj.PIDController;
-
-/**
- * An enhanced variant of {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController}
- * (implements a PID Control Loop).
- * <p>
- * It is enhanced from
- * {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController}
- * in that it that also:
- * <ul>
- *  <li> implements SpeedController (and therefore PIDOutput)
- *  <li> disables self if the setpoint is NaN (see {@link PIDNanny}).
- * </ul>
- */
-public class PIDController extends edu.wpi.first.wpilibj.PIDController implements SpeedController {
-	private boolean NaNDisabled = false;
-	private boolean inverted = false;
-	public final PIDSource source;
-	public final PIDOutput output;
-
-	/**
-	 * Allocate a PID object with the given constants for P, I, D, and F
-	 *$
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param Kf the feed forward term
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 * @param period the loop time for doing calculations. This particularly
-	 *        effects calculations of the integral and differential terms. The
-	 *        default is 50ms.
-	 */
-	public PIDController(double Kp, double Ki, double Kd, double Kf,
-	                     PIDSource source, PIDOutput output,
-	                     double period) {
-		super(Kp, Ki, Kd, Kf, source, output, period);
-		this.source = source;
-		this.output = output;
-	}
-
-	/* Convenience constructors *******************************************/
-	/* `Kf`, and `period` are optional */
-
-	/**
-	 * Convenience constructor with {@code Kf} defaulting to
-	 * {@code 0.0}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 * @param period the loop time for doing calculations. This particularly
-	 *        effects calculations of the integral and differential terms. The
-	 *        default is 50ms.
-	 */
-	public PIDController(double Kp, double Ki, double Kd,
-	                     PIDSource source, PIDOutput output,
-	                     double period) {
-		this(Kp, Ki, Kd, 0.0, source, output, period);
-	}
-
-	/**
-	 * Convenience constructor with {@code period} defaulting to
-	 * {@link #kDefaultPeriod}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param Kf the feed forward term
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 */
-	public PIDController(double Kp, double Ki, double Kd, double Kf,
-	                     PIDSource source, PIDOutput output) {
-		this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
-	}
-
-	/**
-	 * Convenience constructor with {@code Kf} defaulting to
-	 * {@code 0.0}, and {@code period} defaulting to
-	 * {@link #kDefaultPeriod}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 */
-	public PIDController(double Kp, double Ki, double Kd,
-	                     PIDSource source, PIDOutput output) {
-		this(Kp, Ki, Kd, 0.0, source, output, kDefaultPeriod);
-	}
-
-	/* Override to auto-disable if setpoint is NaN, and invert ************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void setSetpoint(double setpoint) {
-		if (Double.isNaN(setpoint)) {
-			NaNDisabled = true;
-			super.disable();
-		} else {
-			if (NaNDisabled && !isEnabled()) {
-				enable();
-			}
-			super.setSetpoint((inverted ? -1 : 1) * setpoint);
-		}
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void enable() {
-		NaNDisabled = false;
-		super.enable();
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void disable() {
-		NaNDisabled = false;
-		super.disable();
-	}
-
-	/* Implement PIDOutput (a parent of SpeedController) *******************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void pidWrite(double output) {
-		setSetpoint(output);
-	}
-
-	/* Implement SpeedController *******************************************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void set(double output) {
-		setSetpoint(output);
-	}
-
-	/**
-	 * Don't use this.
-	 * @deprecated Don't use this; it is leaking up from
-	 *   {@link edu.wpi.first.wpilibj.CANJaguar} through
-	 *   {@link edu.wpi.first.wpilibj.SpeedController}
-	 */
-	@Override
-	public void set(double output, byte syncGroup) {
-		set(output);
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void setInverted(boolean isInverted) {
-		inverted = isInverted;
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public boolean getInverted() {
-		return inverted;
-	}
-}
diff --git a/src/org/usfirst/frc/team4272/robotlib/Pixy.java b/src/org/usfirst/frc/team4272/robotlib/Pixy.java
new file mode 100644
index 0000000..cc6cb58
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robotlib/Pixy.java
@@ -0,0 +1,71 @@
+package org.usfirst.frc.team4272.robotlib;

+

+import edu.wpi.first.wpilibj.SerialPort;

+import edu.wpi.first.wpilibj.SerialPort.Port;

+//Warning: if the pixy is plugged in through mini usb, this code WILL NOT WORK b/c the pixy is smart and detects where it should send data

+public class Pixy {

+		SerialPort pixy;

+		Port port = Port.kMXP;

+		PixyPacket[] packets;

+		PixyException pExc;

+		String print;

+		public Pixy() {

+			pixy = new SerialPort(19200, port);

+			pixy.setReadBufferSize(14);

+			packets = new PixyPacket[7];

+			pExc = new PixyException(print);

+		}	

+//This method parses raw data from the pixy into readable integers

+		public int cvt(byte upper, byte lower) {

+			return (((int)upper & 0xff) << 8) | ((int)lower & 0xff);

+		}

+		

+		public void pixyReset(){

+			pixy.reset();

+		}

+		

+//This method gathers data, then parses that data, and assigns the ints to global variables

+		public PixyPacket readPacket(int Signature) throws PixyException {

+			int Checksum;

+			int Sig;

+			byte[] rawData = new byte[32];

+			try{

+				rawData = pixy.read(32);

+			} catch (RuntimeException e){

+			}	

+			if(rawData.length < 32){

+				System.out.println("byte array length is broken");

+				return null;

+			}

+			for (int i = 0; i <= 16; i++) {

+				int syncWord = cvt(rawData[i+1], rawData[i+0]); //Parse first 2 bytes

+				if (syncWord == 0xaa55) { //Check is first 2 bytes equal a "sync word", which indicates the start of a packet of valid data

+					syncWord = cvt(rawData[i+3], rawData[i+2]); //Parse the next 2 bytes

+					if (syncWord != 0xaa55){ //Shifts everything in the case that one syncword is sent

+						i -= 2;

+					}

+//This next block parses the rest of the data

+					Checksum = cvt(rawData[i+5], rawData[i+4]);

+					Sig = cvt(rawData[i+7], rawData[i+6]);

+					if(Sig <= 0 || Sig > packets.length){

+						break;

+					}

+					packets[Sig - 1] = new PixyPacket();

+					packets[Sig - 1].X = cvt(rawData[i+9], rawData[i+8]);

+					packets[Sig - 1].Y = cvt(rawData[i+11], rawData[i+10]);

+					packets[Sig - 1].Width = cvt(rawData[i+13], rawData[i+12]);

+					packets[Sig - 1].Height = cvt(rawData[i+15], rawData[i+14]);

+//Checks whether the data is valid using the checksum *This if block should never be entered*

+					if (Checksum != Sig + packets[Sig - 1].X + packets[Sig - 1].Y + packets[Sig - 1].Width + packets[Sig - 1].Height) {

+						packets[Sig - 1] = null;

+						throw pExc;

+					}

+					break;

+				}

+			}

+//Assigns our packet to a temp packet, then deletes data so that we dont return old data

+			PixyPacket pkt = packets[Signature - 1];

+			packets[Signature - 1] = null;

+			return pkt;

+		}

+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team4272/robotlib/PixyException.java b/src/org/usfirst/frc/team4272/robotlib/PixyException.java
new file mode 100644
index 0000000..4aef73b
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robotlib/PixyException.java
@@ -0,0 +1,7 @@
+package org.usfirst.frc.team4272.robotlib;

+

+public class PixyException extends Exception {

+	public PixyException(String message){

+		super(message);

+	}

+}

diff --git a/src/org/usfirst/frc/team4272/robotlib/PixyPacket.java b/src/org/usfirst/frc/team4272/robotlib/PixyPacket.java
new file mode 100644
index 0000000..0227a3e
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robotlib/PixyPacket.java
@@ -0,0 +1,8 @@
+package org.usfirst.frc.team4272.robotlib;

+

+public class PixyPacket {

+	public int X;

+	public int Y;

+	public int Width;

+	public int Height;

+}