summaryrefslogtreecommitdiff
path: root/src/org/usfirst/frc
diff options
context:
space:
mode:
Diffstat (limited to 'src/org/usfirst/frc')
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Autonomous.java26
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Control.java8
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/HwOI.java4
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/HwRobot.java67
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Teleop.java89
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/PIDController.java216
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/Pixy.java71
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/PixyException.java7
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/PixyPacket.java8
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;
+}