diff --git a/src/Controllers.f90 b/src/Controllers.f90
index 8f446d35..7ff6a9ef 100644
--- a/src/Controllers.f90
+++ b/src/Controllers.f90
@@ -347,12 +347,45 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst)
         TYPE(ControlParameters), INTENT(INOUT)    :: CntrPar
         TYPE(LocalVariables), INTENT(INOUT)       :: LocalVar
         TYPE(ObjectInstances), INTENT(INOUT)      :: objInst
-        
+        ! Internal Variables
+        Integer(4)                                :: K
+        REAL(4)                                   :: rootMOOP_F(3)
+        REAL(4)                                   :: RootMyb_Vel(3)
+        REAL(4), SAVE                             :: RootMyb_Last(3)
+        REAL(4)                                   :: RootMyb_VelErr(3)
+
         ! Flap control
-        IF (CntrPar%Flp_Mode == 1) THEN
-            LocalVar%Flp_Angle(1) = CntrPar%Flp_Angle
-            LocalVar%Flp_Angle(2) = CntrPar%Flp_Angle
-            LocalVar%Flp_Angle(3) = CntrPar%Flp_Angle
+        IF (CntrPar%Flp_Mode > 1) THEN
+            IF ((LocalVar%iStatus == 0) .and. (CntrPar%Flp_Mode == 1)) THEN
+                RootMyb_Last(1) = 0 - LocalVar%rootMOOP(1)
+                RootMyb_Last(2) = 0 - LocalVar%rootMOOP(2)
+                RootMyb_Last(3) = 0 - LocalVar%rootMOOP(3)
+            ELSEIF (CntrPar%Flp_Mode == 1) THEN
+                LocalVar%Flp_Angle(1) = CntrPar%Flp_Angle
+                LocalVar%Flp_Angle(2) = CntrPar%Flp_Angle
+                LocalVar%Flp_Angle(3) = CntrPar%Flp_Angle
+                ! IF (MOD(LocalVar%Time,10.0) == 0) THEN
+                !     LocalVar%Flp_Angle(1) = LocalVar%Flp_Angle(1) + 1
+                !     LocalVar%Flp_Angle(2) = LocalVar%Flp_Angle(2) + 1
+                !     LocalVar%Flp_Angle(3) = LocalVar%Flp_Angle(3) + 1
+                ! ENDIF
+            ELSEIF (CntrPar%Flp_Mode == 2) THEN
+                DO K = 1,3
+                    ! LPF Blade root bending moment (tau = 0.1, critically damped to attempt to mitigate phase delays)
+                    RootMOOP_F(K) = SecLPFilter(LocalVar%rootMOOP(K),LocalVar%DT,62.8319, 0.7,LocalVar%iStatus, .FALSE.,objInst%instLPF)
+                    ! Find derivative and derivative error of blade root bending moment
+                    RootMyb_Vel(K) = (RootMOOP_F(K) - RootMyb_Last(K))/LocalVar%DT
+                    RootMyb_VelErr(K) = 0 - RootMyb_Vel(K)!LocalVar%rootMOOP(K)
+                    ! Find flap angle
+                    ! LocalVar%Flp_Angle(K) = PIController(RootMyb_VelErr(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, -10.0, 10.0, LocalVar%DT, 0.0, .FALSE., objInst%instPI)
+                    LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, 0.05, -10.0, 10.0, LocalVar%DT, 0.0, .FALSE., objInst%instPI)
+                    ! Add some integral gain to encourage zero flap angle
+                    ! LocalVar%Flp_Angle(K) = LocalVar%Flp_Angle(K) + PIController(0 - LocalVar%Flp_Angle(K), 0.0, 0.01, -10.0, 10.0, LocalVar%DT, 0.0, .FALSE., objInst%instPI)
+                    ! Saturation Limits
+                    ! LocalVar%Flp_Angle(K) = saturate(LocalVar%Flp_Angle(K),-10.0, 10.0)
+                    RootMyb_Last(K) = RootMOOP_F(K)
+                ENDDO
+            ENDIF
             ! Send to AVRSwap
             avrSWAP(120) = LocalVar%Flp_Angle(1)
             avrSWAP(121) = LocalVar%Flp_Angle(2)
diff --git a/src/ROSCO_Types.f90 b/src/ROSCO_Types.f90
index 34974ff8..dee8b473 100644
--- a/src/ROSCO_Types.f90
+++ b/src/ROSCO_Types.f90
@@ -117,9 +117,11 @@ MODULE ROSCO_Types
     REAL(4)                             :: SD_MaxPit                    ! Maximum blade pitch angle to initiate shutdown, [rad]
     REAL(4)                             :: SD_CornerFreq                ! Cutoff Frequency for first order low-pass filter for blade pitch angle, [rad/s]
     
-    INTEGER(4)                          :: Flp_Mode                     ! Flap actuator mode {0: off, 1: fixed flap position}
-    REAL(4)                             :: Flp_Angle                    ! Flp_Angle         - Blade flap angle (degrees)
-
+    INTEGER(4)                          :: Flp_Mode                     ! Flap actuator mode {0: off, 1: fixed flap position, 2: PI flap control}
+    REAL(4)                             :: Flp_Angle                    ! Fixed flap angle (degrees)
+    REAL(4)                             :: Flp_Kp                       ! PI flap control proportional gain 
+    REAL(4)                             :: Flp_Ki                       ! PI flap control integral gain 
+    
     REAL(4)                             :: PC_RtTq99                    ! 99% of the rated torque value, using for switching between pitch and torque control, [Nm].
     REAL(4)                             :: VS_MaxOMTq                   ! Maximum torque at the end of the below-rated region 2, [Nm]
     REAL(4)                             :: VS_MinOMTq                   ! Minimum torque at the beginning of the below-rated region 2, [Nm]
diff --git a/src/ReadSetParameters.f90 b/src/ReadSetParameters.f90
index c97c8b79..e3a45d91 100644
--- a/src/ReadSetParameters.f90
+++ b/src/ReadSetParameters.f90
@@ -199,6 +199,8 @@ SUBROUTINE ReadControlParameterFileSub(CntrPar)
         !------------ Flaps ------------
         READ(UnControllerParameters, *)      
         READ(UnControllerParameters, *) CntrPar%Flp_Angle  
+        READ(UnControllerParameters, *) CntrPar%Flp_Kp  
+        READ(UnControllerParameters, *) CntrPar%Flp_Ki  
         ! END OF INPUT FILE    
         
         !------------------- CALCULATED CONSTANTS -----------------------