Add a working example to help further development of ZMQ for farm con…

…trol
abhineet-gupta · Nov 6, 2023 · 5583fdf · 5583fdf
1 parent dd01047
commit 5583fdf
Show file tree

Hide file tree

Showing 7 changed files with 87 additions and 4 deletions.
diff --git a/Examples/TestZMQ.py b/Examples/TestZMQ.py
@@ -0,0 +1,75 @@
+import platform
+import os
+import matplotlib.pyplot as plt
+from ROSCO_toolbox.inputs.validation import load_rosco_yaml
+from ROSCO_toolbox.utilities import write_DISCON
+from ROSCO_toolbox import control_interface as ROSCO_ci
+from ROSCO_toolbox.control_interface import turbine_zmq_server
+from ROSCO_toolbox import sim as ROSCO_sim
+from ROSCO_toolbox import turbine as ROSCO_turbine
+from ROSCO_toolbox import controller as ROSCO_controller
+import numpy as np
+import multiprocessing as mp
+
+
+def run_zmq():
+    connect_zmq = True
+    s = turbine_zmq_server(network_address="tcp://*:5555", timeout=10000.0, verbose=False)
+    while connect_zmq:
+        #  Get latest measurements from ROSCO
+        measurements = s.get_measurements()
+        identifier = measurements['Identifier']
+
+        # Decide new control input based on measurements
+        current_time = measurements['Time']
+        if current_time <= 10.0:
+            yaw_setpoint = 0.0
+        else:
+            if identifier >= 1.5:
+                yaw_setpoint = -10.0
+            else:
+                yaw_setpoint = -10.0
+
+        # current_time1 = measurements1['Time']
+        # if current_time1 <= 10.0:
+        #     yaw_setpoint1 = 0.0
+        # else:
+        #     yaw_setpoint1 = -20.0
+
+            # Send new setpoints back to ROSCO
+        s.send_setpoints(nacelleHeading=yaw_setpoint)
+        # s1.send_setpoints(nacelleHeading=yaw_setpoint1)
+
+        if measurements['iStatus'] == -1:
+            connect_zmq = False
+            s._disconnect()
+    print('Done with run_zmq')
+
+def sim_openfast():
+    fstfile = '/Users/agupta/Projects/Tools/AG_ROSCO/Examples/IEA-3.4-130-RWT/openfast/IEA-3.4-130-RWT.fst'
+    of_exec = '/Users/agupta/Projects/Tools/AG_openfast/build_v3p5p0/glue-codes/openfast/openfast'
+    os.system(of_exec + ' ' + fstfile)
+    pass
+
+def sim_openfast1():
+    fstfile = '/Users/agupta/Projects/Tools/AG_ROSCO/Examples/IEA-3.4-130-RWT1/openfast/IEA-3.4-130-RWT.fst'
+    of_exec = '/Users/agupta/Projects/Tools/AG_openfast/build_v3p5p0/glue-codes/openfast/openfast'
+    os.system(of_exec + ' ' + fstfile)
+    pass
+
+
+if __name__ == "__main__":
+    # sim_rosco()
+    # run_zmq()
+    # sim_openfast()
+    p1 = mp.Process(target=run_zmq)
+    p1.start()
+    # p2 = mp.Process(target=sim_openfast)
+    # p2.start()
+    p3 = mp.Process(target=sim_openfast1)
+    p3.start()
+    p1.join()
+    # p2.join()
+    p3.join()
+
+
diff --git a/ROSCO/rosco_registry/rosco_types.yaml b/ROSCO/rosco_registry/rosco_types.yaml
@@ -74,6 +74,9 @@ default_types:
 
 
 ControlParameters:
+    Identifier:
+      <<: *integer
+      description: 0000 - 9999, Identifier of the rosco, used for zeromq interface only
     LoggingLevel: 
       <<: *integer
       description: 0 - write no debug files, 1 - write standard output .dbg-file, 2 - write standard output .dbg-file and complete avrSWAP-array .dbg2-file

diff --git a/ROSCO/src/ROSCO_Types.f90 b/ROSCO/src/ROSCO_Types.f90
@@ -8,6 +8,7 @@ MODULE ROSCO_Types
 IMPLICIT NONE
 
 TYPE, PUBLIC :: ControlParameters
+    INTEGER(IntKi)                :: Identifier                  ! 0000 - 9999, Identifier of the rosco, used for zeromq interface only
     INTEGER(IntKi)                :: LoggingLevel                ! 0 - write no debug files, 1 - write standard output .dbg-file, 2 - write standard output .dbg-file and complete avrSWAP-array .dbg2-file
     INTEGER(IntKi)                :: Echo                        ! 0 - no Echo, 1 - Echo input data to <RootName>.echo
     REAL(DbKi)                    :: DT_Out                      ! Output time step

diff --git a/ROSCO/src/ReadSetParameters.f90 b/ROSCO/src/ReadSetParameters.f90
@@ -332,6 +332,7 @@ SUBROUTINE ReadControlParameterFileSub(CntrPar, LocalVar, zmqVar, accINFILE, acc
         ENDIF
 
         !----------------------- Simulation Control --------------------------
+        CALL ParseInput(FileLines,'Identifier',   CntrPar%Identifier,       accINFILE(1), ErrVar, .TRUE., UnEc=UnEc)
         CALL ParseInput(FileLines,'LoggingLevel',   CntrPar%LoggingLevel,       accINFILE(1), ErrVar, .TRUE., UnEc=UnEc)
         CALL ParseInput(FileLines,'DT_Out',         CntrPar%DT_Out,             accINFILE(1), ErrVar, .TRUE., UnEc=UnEc)
         IF (ErrVar%aviFAIL < 0) RETURN

diff --git a/ROSCO/src/ZeroMQInterface.f90 b/ROSCO/src/ZeroMQInterface.f90
@@ -14,17 +14,18 @@ SUBROUTINE UpdateZeroMQ(LocalVar, CntrPar, zmqVar, ErrVar)
 
         character(256) :: zmq_address
         real(C_DOUBLE) :: setpoints(5)
-        real(C_DOUBLE) :: turbine_measurements(16)
+        real(C_DOUBLE) :: turbine_measurements(17)
         CHARACTER(*), PARAMETER                 :: RoutineName = 'UpdateZeroMQ'
 
+        !Identifier = CntrPar%Identifier
         ! C interface with ZeroMQ client
 #ifdef ZMQ_CLIENT
         interface
             subroutine zmq_client(zmq_address, measurements, setpoints) bind(C, name="zmq_client")
                 import :: C_CHAR, C_DOUBLE
                 implicit none
                 character(C_CHAR), intent(out) :: zmq_address(*)
-                real(C_DOUBLE) :: measurements(16)
+                real(C_DOUBLE) :: measurements(17)
                 real(C_DOUBLE) :: setpoints(5)
             end subroutine zmq_client
         end interface
@@ -49,6 +50,7 @@ end subroutine zmq_client
 			turbine_measurements(14) = LocalVar%FA_Acc
 			turbine_measurements(15) = LocalVar%NacIMU_FA_Acc
 			turbine_measurements(16) = LocalVar%Azimuth
+            turbine_measurements(17) = CntrPar%Identifier
 
 			write (zmq_address, "(A,A)") TRIM(CntrPar%ZMQ_CommAddress), C_NULL_CHAR
 #ifdef ZMQ_CLIENT

diff --git a/ROSCO/src/zmq_client.c b/ROSCO/src/zmq_client.c
@@ -22,11 +22,11 @@ void delete_blank_spaces_in_string(char *s)
 
 int zmq_client (
     char *zmq_address,
-    double measurements[16],
+    double measurements[17],
     double setpoints[5]
 )
 {
-	int num_measurements = 16;  // Number of setpoints and measurements, respectively, and float precision (character length)
+	int num_measurements = 17;  // Number of setpoints and measurements, respectively, and float precision (character length)
 	int char_buffer_size_single = 20; // Char buffer for a single measurement
     int char_buffer_size_array = (num_measurements * (char_buffer_size_single + 1));  // Char buffer for full messages to and from ROSCO
 	char string_to_ssc[char_buffer_size_array];

diff --git a/ROSCO_toolbox/control_interface.py b/ROSCO_toolbox/control_interface.py
@@ -436,6 +436,7 @@ def get_measurements(self):
             'FA_Acc': measurements[13],
             'NacIMU_FA_Acc': measurements[14],
             'Azimuth': measurements[15],
+            'Identifier': measurements[16],
         })
 
         if self.verbose: