Account for pitch saturation in vs statemachine

Ben-Mertz · Feb 26, 2020 · f3f24ac · f3f24ac
1 parent 6ea731f
commit f3f24ac
Showing 1 changed file with 8 additions and 5 deletions.
diff --git a/src/ControllerBlocks.f90 b/src/ControllerBlocks.f90
@@ -78,11 +78,14 @@ SUBROUTINE StateMachine(CntrPar, LocalVar)
             END IF
 
             ! --- Torque control state machine ---
-            IF (LocalVar%PC_PitComT >= CntrPar%VS_Rgn3Pitch) THEN           ! Region 3
-                IF (CntrPar%VS_ControlMode == 1) THEN 
-                    LocalVar%VS_State = 5 ! Constant power tracking
-                ELSE 
-                    LocalVar%VS_State = 4 ! Constant torque tracking
+            IF (LocalVar%PC_PitComT >= CntrPar%VS_Rgn3Pitch) THEN       
+
+                IF (LocalVar%PC_PitComT >= LocalVar%PC_MinPit + CntrPar%PC_Switch) THEN ! Make sure we aren't implement pitch saturation
+                    IF (CntrPar%VS_ControlMode == 1) THEN                   ! Region 3
+                        LocalVar%VS_State = 5 ! Constant power tracking
+                    ELSE 
+                        LocalVar%VS_State = 4 ! Constant torque tracking
+                    END IF
                 END IF
             ELSE
                 IF (LocalVar%GenArTq >= CntrPar%VS_MaxOMTq*1.01) THEN       ! Region 2 1/2 - active PI torque control