From 53b947df484159f5934898ba6c0aa69a9c869007 Mon Sep 17 00:00:00 2001
From: Ido Hadanny <ido.hadanny@gmail.com>
Date: Thu, 22 Sep 2011 21:30:13 +0300
Subject: added stuff

---
 .../src/com/hackingroomba/roombacomm/Pid.java      | 67 ++++++++++++++++++++++
 1 file changed, 67 insertions(+)
 create mode 100644 roombacomm-client/src/com/hackingroomba/roombacomm/Pid.java

(limited to 'roombacomm-client/src/com/hackingroomba/roombacomm/Pid.java')

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;
+	}
+
+}
-- 
cgit v1.3.1