summaryrefslogtreecommitdiff
path: root/src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java
diff options
context:
space:
mode:
Diffstat (limited to 'src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java')
-rw-r--r--src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java50
1 files changed, 50 insertions, 0 deletions
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);
+ }
+}