diff --git a/Source/Filters.f90 b/Source/Filters.f90
index a37bfca8..8be9ebf5 100644
--- a/Source/Filters.f90
+++ b/Source/Filters.f90
@@ -40,8 +40,8 @@ REAL FUNCTION LPFilter( InputSignal, DT, CornerFreq, iStatus, inst)
 
 			! Save signals for next time step
 
-        InputSignalLast (inst)  = InputSignal
-        OutputSignalLast (inst) = LPFilter
+        InputSignalLast(inst)  = InputSignal
+        OutputSignalLast(inst) = LPFilter
 
     END FUNCTION LPFilter
     !-------------------------------------------------------------------------------------------------------------------------------
diff --git a/Source/FunctionToolbox.f90 b/Source/FunctionToolbox.f90
index 162a0e28..8ab41e5d 100644
--- a/Source/FunctionToolbox.f90
+++ b/Source/FunctionToolbox.f90
@@ -7,7 +7,7 @@ MODULE FunctionToolbox
 	!-------------------------------------------------------------------------------------------------------------------------------
 	! Saturates inputValue. Makes sure it is not smaller than minValue and not larger than maxValue
 	REAL FUNCTION saturate(inputValue,minValue,maxValue)
-	!...............................................................................................................................
+	!
 
 		IMPLICIT NONE
 
@@ -19,4 +19,33 @@ REAL FUNCTION saturate(inputValue,minValue,maxValue)
 
 	END FUNCTION saturate
 	!-------------------------------------------------------------------------------------------------------------------------------
+	! PI controller, with output saturation
+	REAL FUNCTION PI(error,kp,ki,minValue,maxValue,DT,I0,inst)
+	!
+
+		IMPLICIT NONE
+
+			! Inputs
+		REAL(4), INTENT(IN)		:: error
+		REAL(4), INTENT(IN)		:: kp
+		REAL(4), INTENT(IN)		:: ki
+		REAL(4), INTENT(IN)		:: minValue
+		REAL(4), INTENT(IN)		:: maxValue
+		REAL(4), INTENT(IN)		:: DT
+		REAL(4), INTENT(IN)		:: inst
+		REAL(4), INTENT(IN)		:: I0
+		
+			! Local
+		REAL(4)                      :: PTerm			! Proportional term
+		REAL(4), DIMENSION(99), SAVE :: ITermLast		! Integral term signal the last time this controller was called. Supports 99 separate instances.
+		
+		
+		PTerm = kp*error
+		ITerm(inst) = ITerm(inst) + DT*ki*error
+		ITerm(inst) = saturate(ITerm(inst),maxValue,minValue)
+		PI = PTerm + ITerm(inst)
+		PI = saturate(PI,maxValue,minValue)
+
+	END FUNCTION saturate
+	!-------------------------------------------------------------------------------------------------------------------------------
 END MODULE FunctionToolbox