diff options
Diffstat (limited to 'src/org/usfirst/frc')
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;
+}
|