summaryrefslogtreecommitdiff
path: root/src/org/mckenzierobotics/lib/robot/FeedForward.java
diff options
context:
space:
mode:
authorLuke Shumaker <lukeshu@sbcglobal.net>2015-02-16 17:40:02 -0500
committerLuke Shumaker <lukeshu@sbcglobal.net>2015-02-16 17:40:02 -0500
commit74b7077270fdd9d4d7a6eca0bb7011283ae999a8 (patch)
tree1776cc38d7e64bb980df5436655aeaa37dd27b30 /src/org/mckenzierobotics/lib/robot/FeedForward.java
initial commit
Diffstat (limited to 'src/org/mckenzierobotics/lib/robot/FeedForward.java')
-rw-r--r--src/org/mckenzierobotics/lib/robot/FeedForward.java76
1 files changed, 76 insertions, 0 deletions
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);
+}