diff options
| author | Ido Hadanny <ido.hadanny@gmail.com> | 2011-09-22 21:30:13 +0300 |
|---|---|---|
| committer | Ido Hadanny <ido.hadanny@gmail.com> | 2011-09-22 21:30:13 +0300 |
| commit | 53b947df484159f5934898ba6c0aa69a9c869007 (patch) | |
| tree | 189d5903a67765c390de8f1e3423e6b80cde57d1 /roombacomm-client/src/com/hackingroomba/roombacomm/Pid.java | |
| parent | 654ca6d59e99b5e72b05dfb917bc5888c4ba6b1b (diff) | |
added stuff
Diffstat (limited to 'roombacomm-client/src/com/hackingroomba/roombacomm/Pid.java')
| -rw-r--r-- | roombacomm-client/src/com/hackingroomba/roombacomm/Pid.java | 67 |
1 files changed, 67 insertions, 0 deletions
diff --git a/roombacomm-client/src/com/hackingroomba/roombacomm/Pid.java b/roombacomm-client/src/com/hackingroomba/roombacomm/Pid.java new file mode 100644 index 0000000..97c5297 --- /dev/null +++ b/roombacomm-client/src/com/hackingroomba/roombacomm/Pid.java @@ -0,0 +1,67 @@ +package com.hackingroomba.roombacomm;
+
+/*
+ * This class implements the following PID algorithm
+ * previous_error = 0
+ * start:
+ * error = setpoint - actual_position
+ * P = Kp * error
+ * I = Ki * sum(error)
+ * D = Kd * (error - previous_error)
+ * output = P + I + D
+ * previous_error = error
+ * wait(dt)
+ * goto start
+ */
+public class Pid {
+ double k_p, k_i, k_d, i_state_max; // the PID constants
+ double d_state, i_state; // the PID states
+ boolean disableD = true;
+ int lastError;
+
+ Pid(double p, double i, double d)
+ {
+ k_p = p;
+ k_i = i;
+ k_d = d;
+ d_state = 0.0;
+ i_state = 0.0;
+ i_state_max = 200.0;
+ }
+
+ public double computePid( double target, double value )
+ {
+ double error;
+ double p, i, d;
+ double ret;
+
+ error = target - value;
+ p = k_p * error;
+ d = k_d * (error - d_state );
+ if (disableD) { // prevent an initial kick on the first iteration, before d_state is set.
+ disableD = false;
+ d = 0;
+ }
+ d_state = error;
+ i_state += error;
+
+ // cap I term windup
+ if( i_state > i_state_max )
+ i_state = i_state_max;
+ if( i_state < -i_state_max )
+ i_state = -i_state_max;
+ // clear I term & diable D if we overshoot
+ if (((error > 0) && (lastError < 0)) || ((error < 0) && (lastError > 0))) {
+ i_state = 0;
+ disableD = true;
+ }
+
+ i = k_i * i_state;
+
+ ret = p + i + d;
+ System.out.printf("error %4.1f p: %4.1f i: %4.1f d: %4.1f\n", error, p, i, d);
+
+ return ret;
+ }
+
+}
|