diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robotlib/RollingAvg.java')
-rw-r--r-- | src/org/usfirst/frc/team4272/robotlib/RollingAvg.java | 90 |
1 files changed, 90 insertions, 0 deletions
diff --git a/src/org/usfirst/frc/team4272/robotlib/RollingAvg.java b/src/org/usfirst/frc/team4272/robotlib/RollingAvg.java new file mode 100644 index 0000000..665e7bf --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/RollingAvg.java @@ -0,0 +1,90 @@ +/** + * 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.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +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()); + else + return get(); + } + + public void pidWrite(double output) { + push(output); + } + + public void setPIDSourceType(PIDSourceType srcType) { + if (source!=null) + source.setPIDSourceType(srcType); + } + + public PIDSourceType getPIDSourceType() { + if (source!=null) + return source.getPIDSourceType(); + else + return null; + } +} |