initial commit

16 files changed, 1133 insertions, 0 deletions

diff --git a/src/org/mckenzierobotics/lib/robot/CommandBase.java b/src/org/mckenzierobotics/lib/robot/CommandBase.java
new file mode 100644
index 0000000..0e70893
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/CommandBase.java
@@ -0,0 +1,73 @@
+/**
+ * CommandBase - defines robot hardware; superclass of all commands.
+ * 
+ * Copyright (c) FIRST 2008. 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) 2012 Precise Path Robotics, Inc
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * 
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+package org.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.CANJaguar;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * The base for all commands, also the class defining robot hardware/config.
+ * All atomic commands should subclass CommandBase. CommandBase stores creates
+ * and stores each control system.
+ * 
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public abstract class CommandBase extends Command {
+
+	private boolean has_subsystems = true;
+	protected synchronized void requires(Subsystem subsystem) {
+		if (subsystem == null) {
+			has_subsystems = false;
+		}
+		super.requires(subsystem);
+	}
+	
+	public synchronized void start() {
+		if (has_subsystems) { super.start(); }
+	}
+
+	/**
+	 * Instantiate a CANJaguar, handling exceptions.
+	 * 
+	 * @param id CANJaguar ID to use.
+	 * @return the CANJaguar
+	 */
+	public static CANJaguar getJaguar(int id) {
+		CANJaguar ret = null;
+		try {
+			ret = new CANJaguar(id);
+		} catch(Exception e) {
+			e.printStackTrace();
+		}
+		return ret;
+	}
+	
+	public CommandBase() { super(); }
+	public CommandBase(double timeout) { super(timeout); }
+	public CommandBase(String name) { super(name); }
+	public CommandBase(String name, double timeout) { super(name, timeout); }
+}
diff --git a/src/org/mckenzierobotics/lib/robot/CommandRobot.java b/src/org/mckenzierobotics/lib/robot/CommandRobot.java
new file mode 100644
index 0000000..35534c2
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/CommandRobot.java
@@ -0,0 +1,65 @@
+/**
+ * CommandFramework base class for FRC 1024.
+ * 
+ * Copyright (c) 2012 Precise Path Robotics, Inc
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * 
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+
+package org.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the manifest file in the resource
+ * directory.
+ * 
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public abstract class CommandRobot extends IterativeRobot {
+
+	protected Command autonomousCommand;
+	protected Command teleopCommand;
+	
+	public abstract void robotInit();
+
+	public void autonomousInit() {
+		autonomousCommand.start();
+	}
+
+	public void autonomousPeriodic() {
+		Scheduler.getInstance().run();
+	}
+
+	public void teleopInit() {
+		autonomousCommand.cancel();
+		teleopCommand.start();
+	}
+
+	public void teleopPeriodic() {
+		Scheduler.getInstance().run();
+	}
+	
+	public void disabledInit() {
+		autonomousCommand.cancel();
+		teleopCommand.cancel();
+	}
+}
diff --git a/src/org/mckenzierobotics/lib/robot/FeedForward.java b/src/org/mckenzierobotics/lib/robot/FeedForward.java
new file mode 100644
index 0000000..a275366
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/FeedForward.java
@@ -0,0 +1,76 @@
+/**
+ * Copyright (c) 2012 Precise Path Robotics, Inc
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * 
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+package org.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+
+/**
+ *
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public abstract class FeedForward implements PIDOutput {
+	private class PIDTrigger implements PIDOutput {
+		private FeedForward ff;
+		public PIDTrigger(FeedForward ff) {
+			this.ff = ff;
+		}
+		public void pidWrite(double output) {
+			ff.update(output);
+		}
+	}
+	private PIDController pid;
+	private PIDOutput pidOutput;
+	public FeedForward(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+		pidOutput = output;
+		pid = new PIDController(Kp, Ki, Kd, source, new PIDTrigger(this));
+	}
+	public FeedForward(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
+		pidOutput = output;
+		pid = new PIDController(Kp, Ki, Kd, source, new PIDTrigger(this), period);
+	}
+	
+	public FeedForward(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, boolean autodisable) {
+		pidOutput = output;
+		pid = new PIDController(Kp, Ki, Kd, source, new PIDTrigger(this), autodisable);
+	}
+	public FeedForward(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period, boolean autodisable) {
+		pidOutput = output;
+		pid = new PIDController(Kp, Ki, Kd, source, new PIDTrigger(this), period, autodisable);
+	}
+	
+	public PIDController getPID() {
+		return pid;
+	}
+	
+	public void pidWrite(double output) {
+		pid.setSetpoint(output);
+	}
+	
+	public void update(double pidResult) {
+		if (pid.isEnable()) {
+			double ffResult = calculate(pid.getSetpoint());
+			pidOutput.pidWrite(ffResult+pidResult);
+		} else {
+			pidOutput.pidWrite(0);
+		}
+	}
+	
+	public abstract double calculate(double setpoint);
+}
diff --git a/src/org/mckenzierobotics/lib/robot/PIDController.java b/src/org/mckenzierobotics/lib/robot/PIDController.java
new file mode 100644
index 0000000..dc2301a
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/PIDController.java
@@ -0,0 +1,104 @@
+/**
+ * Copyright (c) 2011, 2012, 2015 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.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.SpeedController;
+//import edu.wpi.first.wpilibj.PIDController;
+
+/**
+ * TODO: Write JavaDocs
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public class PIDController extends edu.wpi.first.wpilibj.PIDController implements SpeedController {
+	private boolean autodisable = false;
+	/**
+	 * Default to true, so if we don't use autodisable, we don't enable when we
+	 * shouldn't, as we will think it's already enabled.
+	 */
+	private boolean enabled = true;
+	public final PIDSource source;
+	public final PIDOutput output;
+	public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+		super(Kp, Ki, Kd, source, output);
+		this.source = source;
+		this.output = output;
+	}
+	public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
+		super(Kp, Ki, Kd, source, output, period);
+		this.source = source;
+		this.output = output;
+	}
+	
+	public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, boolean autodisable) {
+		super(Kp, Ki, Kd, source, output);
+		this.autodisable = autodisable;
+		enabled = this.isEnable();
+		this.source = source;
+		this.output = output;
+	}
+	public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period, boolean autodisable) {
+		super(Kp, Ki, Kd, source, output, period);
+		this.autodisable = autodisable;
+		enabled = this.isEnable();
+		this.source = source;
+		this.output = output;
+	}
+	
+	public void setSetpoint(double output) {
+		if ((output == 0) && autodisable) {
+			disable();
+			enabled = false;
+		} else {
+			if (!enabled) {
+				enable();
+				enabled = true;
+			}
+			super.setSetpoint(output);
+		}
+	}
+	
+	public void pidWrite(double output) {
+		setSetpoint(output);
+	}
+	
+	public void set(double output) {
+		setSetpoint(output);
+	}
+	
+	/**
+	 * Don't use this; it is leaking up from CANJaguar
+	 * @param output
+	 * @param syncGroup 
+	 */
+	public void set(double output, byte syncGroup) {
+		setSetpoint(output);
+	}
+}
diff --git a/src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java b/src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java
new file mode 100644
index 0000000..b4e7713
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java
@@ -0,0 +1,80 @@
+/**
+ * Copyright (c) 2011, 2012, 2015 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.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.PIDOutput;
+
+/**
+ * TODO: Write JavaDocs
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public class PIDOutputSplitter implements PIDOutput {
+	private final PIDOutput[] outputs;
+	private final double[] scalars;
+
+	public PIDOutputSplitter(PIDOutput[] outputs, double[] scalars) {
+		this.outputs = outputs;
+		this.scalars = scalars;
+		if (outputs.length != scalars.length) {
+			throw new RuntimeException("outputs and scalars must be same len");
+		}
+	}
+
+	public PIDOutputSplitter(PIDOutput... outputs) {
+		double[] s = new double[outputs.length];
+		for (int i=0; i<s.length; i++) {
+			s[i]=1;
+		}
+		this.outputs = outputs;
+		this.scalars = s;
+	}
+
+	public void pidWrite(double output) {
+		for(int i=0; i<outputs.length; i++) {
+			try {
+				outputs[i].pidWrite(output*scalars[i]);
+			} catch (Exception e) {
+				error(e);
+			}
+		}
+	}
+	
+	/**
+	 * Handles exceptions from pidWrite().
+	 * 
+	 * By default it writes to System.out; you may extend+override this to
+	 * change the behavior.
+	 * 
+	 * @param e Exception
+	 */
+	public void error(Exception e) {
+		e.printStackTrace();
+	}
+}
diff --git a/src/org/mckenzierobotics/lib/robot/PIDServo.java b/src/org/mckenzierobotics/lib/robot/PIDServo.java
new file mode 100644
index 0000000..fdbd1ce
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/PIDServo.java
@@ -0,0 +1,36 @@
+/**
+ * Copyright (c) 2012 Precise Path Robotics, Inc
+ *
+ * This program is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+package org.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj.PIDOutput;
+
+/**
+ * TODO: Write JavaDocs
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public class PIDServo extends Servo implements PIDOutput {
+	public PIDServo(int channel) {
+		super(channel);
+	}
+	
+	public void pidWrite(double degrees) {
+		setAngle(getAngle()+degrees);
+	}
+}
diff --git a/src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java b/src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java
new file mode 100644
index 0000000..ed36345
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java
@@ -0,0 +1,50 @@
+/**
+ * Copyright (c) 2012 Precise Path Robotics, Inc
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * 
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+
+package org.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ *
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public class RateLimitedPIDOutput implements PIDOutput {
+	PIDOutput o;
+	double m;
+	double prev_rate = 0;
+	double prev_time = 0;
+	
+	public RateLimitedPIDOutput(PIDOutput out, double maxChange) {
+		o = out;
+		m = maxChange;
+	}
+	
+	public void pidWrite(double rate) {
+		double time = Timer.getFPGATimestamp();
+		double dTime = time-prev_time;
+		double dRate = rate-prev_rate;
+		if (Math.abs(dRate/dTime)>m) {
+			int sign = (dRate<0?-1:1);
+			rate = prev_rate+(m*dTime*sign);
+		}
+		o.pidWrite(rate);
+	}
+}
diff --git a/src/org/mckenzierobotics/lib/robot/RollingAvg.java b/src/org/mckenzierobotics/lib/robot/RollingAvg.java
new file mode 100644
index 0000000..fcbf8e4
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/RollingAvg.java
@@ -0,0 +1,75 @@
+/**
+ * Copyright (c) 2011 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.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDOutput;
+
+/**
+ * TODO: Write JavaDocs
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public class RollingAvg implements PIDSource, PIDOutput {
+	private PIDSource source = null;
+	private double[] points;
+	private double avg;
+	private int i;
+
+	public RollingAvg(int len) {
+		points = new double[len];
+		i = 0;
+		avg = 0;
+	}
+	public RollingAvg(int len, PIDSource src) {
+		this(len);
+		source = src;
+	}
+
+	public double push(double v) {
+		avg -= points[i];
+		points[i] = v/points.length;
+		avg += points[i];
+		i++; i %= points.length;
+		return avg;
+	}
+
+	public double get() {
+		return avg;
+	}
+
+	public double pidGet() {
+		if (source!=null) return push(source.pidGet());
+		return get();
+	}
+	
+	public void pidWrite(double output) {
+		push(output);
+	}
+}
diff --git a/src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java b/src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java
new file mode 100644
index 0000000..43852b1
--- /dev/null
+++ b/src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java
@@ -0,0 +1,42 @@
+/**
+ * Copyright (c) 2012, 2015 Precise Path Robotics, Inc
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * 
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+
+package org.mckenzierobotics.lib.robot;
+
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ * @author Luke Shumaker <lukeshu@sbcglobal.net>
+ */
+public class SendablePIDOutput implements PIDOutput {
+	PIDOutput o;
+	String n;
+	
+	public SendablePIDOutput(String name, PIDOutput out) {
+		n = name;
+		o = out;
+	}
+	
+	public void pidWrite(double val) {
+		o.pidWrite(val);
+		SmartDashboard.putNumber(n, val);
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2015/HwOI.java b/src/org/usfirst/frc/team4272/robot2015/HwOI.java
new file mode 100644
index 0000000..9d09b65
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/HwOI.java
@@ -0,0 +1,11 @@
+package org.usfirst.frc.team4272.robot2015;
+
+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);
+}
diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
new file mode 100644
index 0000000..22a7afc
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
@@ -0,0 +1,75 @@
+package org.usfirst.frc.team4272.robot2015;
+
+import org.mckenzierobotics.lib.robot.PIDController;
+import org.mckenzierobotics.lib.robot.PIDOutputSplitter;
+import org.usfirst.frc.team4272.robotlib.DoubleSolenoid;
+import org.usfirst.frc.team4272.robotlib.LimitSwitchedPIDOutput;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSource.PIDSourceParameter;
+import edu.wpi.first.wpilibj.Relay;
+import edu.wpi.first.wpilibj.Talon;
+
+public class HwRobot {
+	/* Relay == a Spike */
+	/* PCM = Pneumatics Control Module */
+
+	/* All of the numbered inputs are in the classes:
+	 * - DIO: 0-9
+	 * - Relay: 0-3
+	 * - Analog In: 0-3
+	 * - PWM: 0-9
+	 * - PCM: 0-7 (the PCM is connected via CAN).
+	 * - CAN
+	 * 
+	 * For completeness, the roboRIO also has: i2c, RS-232, SPI,
+	 * RSL, 2xUSB-A, an accelerometer, and an expansion port.
+	 *
+	 * And, for communication: USB-B and Ethernet.
+	 */
+	
+	static class OutDumper implements PIDOutput {
+		private String p;
+		public OutDumper(String prefix) { p = prefix; }
+		public void pidWrite(double v) {
+			System.out.println(p+": "+v);
+		}
+	}
+	static class InDumper implements PIDSource {
+		private String p;
+		public final PIDSource r;
+		public InDumper(String prefix, PIDSource real) { p = prefix; r = real; }
+		public double pidGet() {
+			double v = r.pidGet();
+			System.out.println(p+": "+v);
+			return v;
+		}
+	}
+	public Encoder   lDriveE = new Encoder(/*DIO*/2,/*DIO*/3);
+	public PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0),
+	                                                 new Talon(/*PWM*/1));
+	public Encoder   rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true);
+	public PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2),
+	                                                 new Talon(/*PWM*/3));
+	public Encoder   winchE = new Encoder(/*DIO*/4,/*DIO*/5);//),
+	public PIDOutput winchM = new LimitSwitchedPIDOutput(
+		new PIDOutputSplitter(new Talon(/*PWM*/4),
+		                      new Talon(/*PWM*/5)),
+		/*bottom*/new DigitalInput(/*DIO*/6),
+		/*top*/   new DigitalInput(/*DIO*/7),
+		true);
+	public DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3);
+	public DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1);
+	public Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth);
+	public Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth);
+	public Relay lights  = new Relay(/*Relay*/2, Relay.Direction.kForward);
+	
+	public HwRobot() {
+		lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
+		rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
+		winchE.setPIDSourceParameter(PIDSourceParameter.kRate);
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java
new file mode 100644
index 0000000..95a47b7
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java
@@ -0,0 +1,105 @@
+
+package org.usfirst.frc.team4272.robot2015;
+
+import org.usfirst.frc.team4272.robotlib.Toggler;
+import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
+import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Relay;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the manifest file in the resource
+ * directory.
+ */
+public class Robot extends IterativeRobot {
+	private HwRobot robot = new HwRobot();
+	private HwOI oi;
+    /**
+     * This function is run when the robot is first started up and should be
+     * used for any initialization code.
+     */
+    public void robotInit() {
+
+    }
+
+    public void autonomousInit() {
+    	
+    }
+    public void autonomousPeriodic() {
+
+    }
+
+    private Toggler grabButton = new Toggler();
+    private Toggler pushButton = new Toggler();
+    private Toggler ledsButton = new Toggler();
+    public void teleopInit() {
+    	//robot.lDrive.reset();
+    	//robot.rDrive.reset();
+    	//robot.winch.reset();
+    	//robot.lDrive.enable();
+    	//robot.rDrive.enable();
+    	//robot.winch.enable();
+    	robot.grab.setEnabled(true);
+    	robot.push.setEnabled(true);
+    	oi = new HwOI();
+    	grabButton.update(false);
+    	pushButton.update(false);
+    	ledsButton.update(false);
+    }
+    public void teleopPeriodic() {
+        /* Drive */
+    	robot.lDriveM.pidWrite(-oi.lStick.getY()*Math.abs(oi.lStick.getY()));
+    	robot.rDriveM.pidWrite( oi.rStick.getY()*Math.abs(oi.rStick.getY()));
+    	
+    	/* Winch */
+    	double w = oi.xbox.getAxis(Axis.LY);
+    	if (Math.abs(w) < 0.1) { w = 0; }
+    	robot.winchM.pidWrite(-w);
+    	
+    	/* left intake */
+    	if (oi.xbox.getButton(Button.LB)) {
+    		robot.lIntake.set(Relay.Value.kReverse);
+    	} else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
+    		robot.lIntake.set(Relay.Value.kForward);
+    	} else {
+    		robot.lIntake.set(Relay.Value.kOff);
+    	}
+    	/* right intake */
+    	if (oi.xbox.getButton(Button.RB)) {
+    		robot.rIntake.set(Relay.Value.kReverse);
+    	} else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
+    		robot.rIntake.set(Relay.Value.kForward);
+    	} else {
+    		robot.rIntake.set(Relay.Value.kOff);
+    	}
+    	
+    	/* grab */
+    	robot.grab.setForward(grabButton.update(oi.xbox.getButton(Button.A)));
+    	robot.push.setForward(pushButton.update(oi.xbox.getButton(Button.B)));
+    	if (ledsButton.update(oi.xbox.getButton(Button.RT))) {
+    		robot.lights.set(Relay.Value.kForward);
+    	} else {
+    		robot.lights.set(Relay.Value.kOff);
+    	}
+    }
+    
+    public void disabledInit() {
+    	//robot.lDrive.reset();
+    	//robot.rDrive.reset();
+    	//robot.winch.reset();
+    	robot.grab.setEnabled(false);
+    	robot.push.setEnabled(false);
+    }
+    
+    /**
+     * This function is called periodically during test mode
+     */
+    public void testPeriodic() {
+    
+    }
+    
+}
diff --git a/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java b/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java
new file mode 100644
index 0000000..8698277
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team4272.robotlib;
+
+public class DoubleSolenoid extends edu.wpi.first.wpilibj.DoubleSolenoid {
+	private boolean enabled = true;
+	private Value value;
+	
+	public DoubleSolenoid(int forwardChannel, int reverseChannel) {
+		super(forwardChannel, reverseChannel);
+		value = get();
+	}
+	public DoubleSolenoid(int moduleNumber, int forwardChannel, int reverseChannel) {
+		super(moduleNumber, forwardChannel, reverseChannel);
+		value = get();
+	}
+	
+	public void setEnabled(boolean enabled) {
+		this.enabled = enabled;
+		if (enabled) {
+			set(value);
+		} else {
+			set(Value.kOff);
+		}
+	}
+	
+	public void setForward(boolean forward) {
+		set(forward ? Value.kForward : Value.kReverse);
+	}
+	
+	public boolean getForward() {
+		return value == Value.kForward;
+	}
+	
+	public void set(Value value) {
+		this.value = value;
+		if (enabled) {
+			super.set(value);
+		}
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java
new file mode 100644
index 0000000..73073c2
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java
@@ -0,0 +1,38 @@
+package org.usfirst.frc.team4272.robotlib;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.PIDOutput;
+
+public class LimitSwitchedPIDOutput implements PIDOutput {
+	private final PIDOutput out;
+	private final DigitalInput forward;
+	private final DigitalInput backward;
+	private final boolean i_dont_know_which_way_is_which;
+	
+	public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward, boolean i_dont_know_which_way_is_which) {
+		this.out = out;
+		this.forward = forward;
+		this.backward = backward;
+		this.i_dont_know_which_way_is_which = i_dont_know_which_way_is_which;
+	}
+	
+	public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) {
+		this(out, forward,backward, false);
+	}
+	
+	public void pidWrite(double v) {
+		if (i_dont_know_which_way_is_which) {
+			if (forward.get() || backward.get()) {
+				v = 0;
+			}
+		} else {
+			if (forward.get()) {
+				v = Math.min(v, 0);
+			}
+			if (backward.get()) {
+				v = Math.max(v, 0);
+			}
+		}
+		out.pidWrite(v);
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robotlib/Toggler.java b/src/org/usfirst/frc/team4272/robotlib/Toggler.java
new file mode 100644
index 0000000..03fffec
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robotlib/Toggler.java
@@ -0,0 +1,13 @@
+package org.usfirst.frc.team4272.robotlib;
+
+public class Toggler {
+	private boolean prev = false;
+	private boolean state = false;
+	public boolean update(boolean next) {
+		if (next && ! prev) {
+			state = !state;
+		}
+		prev = next;
+		return state;
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java b/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java
new file mode 100644
index 0000000..d04fa23
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java
@@ -0,0 +1,251 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Copyright (c) Luke Shumaker 2015. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+package org.usfirst.frc.team4272.robotlib;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+/**
+ * Handle input from a wired Xbox 360 controller connected to the
+ * Driver Station.
+ */
+public class Xbox360Controller extends Joystick {
+	/* Constants ************************************************/
+
+	/**
+	 * Represents an analog axis on an Xbox 360 controller.
+	 */
+	public static enum Axis {
+		LX(0), LY(1), /** left trigger */ LT(2),
+		RX(4), RY(5), /** right trigger */ RT(3),
+		/** D-Pad X */ DX(6), /** D-Pad Y */ DY(7);
+
+		private final int id;
+		private Axis(int id) { this.id = id; }
+		public int getId() { return id; }
+	}
+
+	/**
+	 * Represents a digital button on Xbox 360 controller.
+	 */
+	public static enum Button {
+		A(0), B(1),
+		X(2), Y(3),
+		/** left bumper */ LB(4), /** right bumper */RB( 5),
+		Back(6), Start(7), /*Home(8),*/
+		/** left thumb */ LT(8), /** right thumb */ RT(9);
+
+		public final int id;
+		private Button(int id) { this.id = id+1; }
+	}
+
+	/* Constructor **********************************************/
+
+	/**
+	 * Construct an instance of a joystick.
+	 * The joystick index is the USB port on the drivers station.
+	 *
+	 * @param port The port on the driver station that the joystick is plugged into.
+	 */
+	public Xbox360Controller(final int port) {
+		super(port);
+	}
+
+	/* Core functions *******************************************/
+	
+	/**
+	 * Get the value of an axis base on an enumerated type.
+	 *
+	 * @param axis The axis to read.
+	 * @return The value of the axis.
+	 */
+	public double getAxis(Axis axis) {
+		return getRawAxis(axis.id);
+	}
+
+	/**
+	 * Get buttons based on an enumerated type.
+	 *
+	 * @param button The button to read.
+	 * @return The state of the button.
+	 */
+	public boolean getButton(Button button) {
+		return getRawButton(button.id);
+	}
+
+
+	/* The actual code is done.  The rest is boilerplate.  See,
+	 * this is why Java is terrible.  Just 30% of the file
+	 * actually doing useful stuff, the rest just filling
+	 * interfaces. */
+	/* Stupid little wrappers ***********************************/
+
+	/**
+	 * Get the X value of a thumb-stick.
+	 *
+	 * @param hand Left stick or right?
+	 * @return The X value of the joystick.
+	 */
+	public double getX(final Hand hand) {
+		if (hand.value == Hand.kLeft.value)
+			return getAxis(Axis.LX);
+		if (hand.value == Hand.kRight.value)
+			return getAxis(Axis.RX);
+		return 0.0;
+	}
+
+	/**
+	 * Get the Y value of a thumb-stick.
+
+	 * @param hand Left stick or right?
+	 * @return The Y value of the joystick.
+	 */
+	public double getY(final Hand hand) {
+		if (hand.value == Hand.kLeft.value)
+			return getAxis(Axis.LY);
+		if (hand.value == Hand.kRight.value)
+			return getAxis(Axis.RY);
+		return 0.0;
+	}
+
+	/**
+	 * Get the value of a trigger.
+	 *
+	 * @param hand Left trigger or right?
+	 * @return The trigger value.
+	 */
+	public double getZ(final Hand hand) {
+		if (hand.value == Hand.kLeft.value)
+			return getAxis(Axis.LT);
+		if (hand.value == Hand.kRight.value)
+			return getAxis(Axis.RT);
+		return 0.0;
+	}
+
+	/**
+	 * Get the state of a bumper.
+	 *
+	 * @param hand Left trigger or right?
+	 * @return the state of the bumper.
+	 */
+	public boolean getBumper(Hand hand) {
+		if (hand.value == Hand.kLeft.value)
+			return getButton(Button.LB);
+		if (hand.value == Hand.kRight.value)
+			return getButton(Button.RB);
+		return false;
+	}
+
+	/**
+	 * Get the state of a thumb-stick button.
+	 *
+	 * @param hand Left trigger or right?
+	 * @return the state of the button.
+	 */
+	public boolean getTop(Hand hand) {
+		if (hand.value == Hand.kLeft.value)
+			return getButton(Button.LT);
+		if (hand.value == Hand.kRight.value)
+			return getButton(Button.RB);
+		return false;
+	}
+	
+	/**
+	 * Get the state of a trigger; whether it is more than
+	 * half-way pressed or not.
+	 *
+	 * @param hand Left trigger or right?
+	 * @return The state of the trigger.
+	 */
+	public boolean getTrigger(Hand hand) {
+		return getZ(hand) > 0.75;
+	}
+
+	/**
+	 * Get the magnitude of the direction vector formed by the thumb-stick's
+	 * current position relative to its origin
+	 *
+	 * @return The magnitude of the direction vector
+	 */
+	public double getMagnitude(Hand hand) {
+		return Math.sqrt(Math.pow(getX(hand), 2) + Math.pow(getY(hand), 2));
+	}
+
+	public double getMagnitude() {
+		return getMagnitude(Hand.kRight);
+	}
+
+	/**
+	 * Get the direction of the vector formed by the thumb-stick and its origin
+	 * in radians
+	 *
+	 * @return The direction of the vector in radians
+	 */
+	public double getDirectionRadians(Hand hand) {
+		return Math.atan2(getX(hand), -getY(hand));
+	}
+
+	public double getDirectionRadians() {
+		return getDirectionRadians(Hand.kRight);
+	}
+
+	/**
+	 * Get the direction of the vector formed by the thumb-stick and its origin
+	 * in degrees
+	 *
+	 * uses acos(-1) to represent Pi due to absence of readily accessable Pi
+	 * constant in C++
+	 *
+	 * @return The direction of the vector in degrees
+	 */
+	public double getDirectionDegrees(Hand hand) {
+		return Math.toDegrees(getDirectionRadians(hand));
+	}
+
+	public double getDirectionDegrees() {
+		return Math.toDegrees(getDirectionRadians(Hand.kRight));
+	}
+
+
+	/* Unused wrappers for GenericHID/Joystick ******************/
+
+	/**
+	 * This method is only here to complete the GenericHID interface.
+	 *
+	 * @return Always 0.0
+	 */
+	public double getTwist() {
+		return 0.0;
+	}
+
+	/**
+	 * This method is only here to complete the GenericHID interface.
+	 *
+	 * @return Always 0.0
+	 */
+	public double getThrottle() {
+		return 0.0;
+	}
+	
+	/**
+	 * This method is only here to complete the Joystick interface.
+	 *
+	 * @param axis unused
+	 * @return Always 0
+	 */
+	public int getAxisChannel(AxisType axis) {
+		return 0;
+	}
+
+	/**
+	 * This method is only here to complete the Joystick interface.
+	 *
+	 * @param axis unused
+	 * @param channel unused
+	 */
+	public void setAxisChannel(AxisType axis, int channel) {}
+}