diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robotlib')
4 files changed, 86 insertions, 216 deletions
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;
+}
|