fixed errors on PID-Blocks

src/main/java/com/ardublock/translator/block/operators/PidBlock.java

@@ -42,7 +42,7 @@ public class PidBlock extends TranslatorBlock
 		//TODO: TEST CONTROLLER FUNCTIONALITY!!!!!!!
 		
 		//FUNCTION CODE ASSEMBLY
-		String functionCode = "int computePID(int input, int setpoint, long interval, int kp";
+		String functionCode = "double computePID(double input, double setpoint, long interval, double kp";
 		
 		if(integrative) {
 			functionCode +=", double ki";
@@ -51,36 +51,45 @@ public class PidBlock extends TranslatorBlock
 			functionCode +=", double kd";
 		}
 		if(limited) {
-			functionCode +=", int limitLow, int limitHigh";
+			functionCode +=", double limitLow, double limitHigh";
 		}
 		
-		functionCode += "){"
-					+ "  static int  lastError = 0;\n"
-					+ "  static int  out       = 0;\n"
+		functionCode += "){";
+		
+		if(derivative) {
+			functionCode +="  static double  lastError = 0;\n";
+		}
+		if(integrative) {
+			functionCode +="static double cumError  = 0; \n";	
+		}
+					
+		functionCode+="  static double  out       = 0;\n"
 					+ "  static long lastTime  = 0L;\n\n"
+					+ "  double intervalSecond = interval / 1000.0;\n"
 					+ "  long currentTime     = millis();\t//get current time\n\n"
-					+ "  if (currentTime >= lastTime + interval) {\n"
+					+ "  if (currentTime - lastTime >= interval) {\n"
 					+ "    long elapsedTime = (double)(currentTime - lastTime);  //compute time elapsed from previous computation\n"
 					+ "    \n" 
 					+ "    long error       = setpoint - input;                  // get error\n" 
 					+ "    out = kp * error;                                     //P output\n\n";
 		if(integrative) {
-			functionCode +="    long cumError    = cumError + error * elapsedTime;    // compute integral\n";
+			functionCode +="    cumError  = cumError + error;    // compute integral\n";
 			if(limited) {
-				functionCode += "    cumError    = constrain(cumError, limitLow, limitHigh);            //limit integral against windup";
+				functionCode += "    cumError    = constrain(cumError, limitLow * 100.0, limitHigh * 100.0);            //limit integral against windup\n";
 			}
-			functionCode +=  "    out = out + ki * cumError;                            //I output\n\n";
+			functionCode +=  "    out = out + (ki * intervalSecond) * cumError;                            //I output\n\n";
 		}
 		if(derivative) {
-			functionCode +="    long rateError   = (error - lastError) / elapsedTime; // compute derivative\n" 
-						+  "    out = out + kd * rateError;                           //D output\n\n";
+			functionCode +="    long rateError   = error - lastError; // compute derivative\n" 
+						+  "    out = out - (kd / intervalSecond) * rateError;                           //D output\n\n";
 		}
 		if(limited) {
 			functionCode +="    out = constrain(out, limitLow, limitHigh);            //limit output\n\n";
 		}
-		
-		functionCode += "    lastError = error;         //remember current error\n" 
-					+	"    lastTime = currentTime;    //remember current time\n" 
+		if(derivative) {
+			functionCode += "    lastError = error;         //remember current error\n";
+		}
+		functionCode += "    lastTime = currentTime;    //remember current time\n" 
 					+   "  }\n" 
 					+   "  return out;                  //return the PID output\n" 
 					+	"}";