summaryrefslogtreecommitdiff
path: root/src/org/mckenzierobotics/lib/robot
diff options
context:
space:
mode:
Diffstat (limited to 'src/org/mckenzierobotics/lib/robot')
-rw-r--r--src/org/mckenzierobotics/lib/robot/CommandBase.java73
-rw-r--r--src/org/mckenzierobotics/lib/robot/CommandRobot.java65
-rw-r--r--src/org/mckenzierobotics/lib/robot/FeedForward.java76
-rw-r--r--src/org/mckenzierobotics/lib/robot/PIDController.java104
-rw-r--r--src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java80
-rw-r--r--src/org/mckenzierobotics/lib/robot/PIDServo.java36
-rw-r--r--src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java50
-rw-r--r--src/org/mckenzierobotics/lib/robot/RollingAvg.java75
-rw-r--r--src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java42
9 files changed, 601 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);
+ }
+}