1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
|
/**
* Copyright (c) 2011, 2016-2017 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;
/**
* RollingAvg implements a rolling average.
*/
public class RollingAvg implements PIDSource, PIDOutput {
private PIDSource source = null;
private double[] points;
private double avg;
private int i;
/**
* Construct a RollingAvg that must be updated with the
* {@link #push(double)} or {@link #pidWrite(double)} methods.
*
* @param len The number of samples to keep in the average.
*/
public RollingAvg(int len) {
points = new double[len];
i = 0;
avg = 0;
}
/**
* Construct a RollingAvg that will automatically update
* itself from a {@link PIDSource} each time {@link #pidGet()}
* is called.
*
* That is, it wraps the {@link PIDSource} to stabalize any
* noise.
*
* If {@link #pidGet()} is not called an iteration, then it is
* not updated that iteration; so be sure to call it each
* iteration whether or not you actually care about the value.
*
* @param len The number of samples to keep in the average.
* @param src The underlying PIDSource to read from.
*/
public RollingAvg(int len, PIDSource src) {
this(len);
source = src;
}
/**
* Push a new value on to the rolling average (pushing out a
* previous value).
*
* @param v The new value to push.
* @return The new average after pushing v.
*/
public double push(double v) {
avg -= points[i];
points[i] = v/points.length;
avg += points[i];
i++; i %= points.length;
return avg;
}
/**
* Return the current value of the rolling average, without
* mutating anything.
*
* @return The current value of the rolling average.
*/
public double get() {
return avg;
}
/**
* If constructed with an underlying {@link PIDSource}, read from it
* and push the value; returning the new average after pushing
* that value.
*
* If not constructed with an underlying {@link PIDSource},
* then this is simply an alias for {@link #get()}.
*
* @return The value of the rolling average.
*/
public double pidGet() {
if (source!=null)
return push(source.pidGet());
else
return get();
}
/**
* An alias for {@link #push(double)} (but doesn't return the
* new average), in order to implement the {@link PIDOutput}
* interface.
*
* @param output The value to push.
*/
public void pidWrite(double output) {
push(output);
}
/**
* See the documentation for {@link PIDSource#setPIDSourceType(PIDSourceType)}.
*/
public void setPIDSourceType(PIDSourceType srcType) {
if (source!=null)
source.setPIDSourceType(srcType);
}
/**
* See the documentation for {@link PIDSource#getPIDSourceType}.
*/
public PIDSourceType getPIDSourceType() {
if (source!=null)
return source.getPIDSourceType();
else
return null;
}
}
|
