Browse Source

solved issues with loop sa and tested it

Cláudio Gomes 3 years ago
parent
commit
fe4e37afab

+ 8 - 4
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_Window_sa/FMI_Window_sa.c

@@ -59,6 +59,8 @@ int is_close(double a, double b, double REL_TOL, double ABS_TOL){
 
 fmi2Status fmi2SetDebugLogging(fmi2Component fc, fmi2Boolean loggingOn, size_t nCategories, const fmi2String categories[])
 {
+	FMUInstance* fi = (FMUInstance *)fc;
+	fi->fmu[0].setDebugLogging(fi->c_fmu[0],loggingOn,nCategories,categories);
 	return fmi2OK;
 }
 
@@ -112,9 +114,13 @@ fmi2Status fmi2SetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t n
 
 fmi2Status fmi2GetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2Real value[])
 {
-
 	FMUInstance* comp = (FMUInstance *)fc;
 
+	printf("%s: getReal with vr: \n", comp->instanceName);
+	for (int i = 0; i < nvr; ++i) {
+		printf("vr[%d] = %d \n", i, vr[i]);
+	}
+
 	int isEmpty = 1;
 	for (int i=0; i<_NR_OF_OUT_CONDITIONS;i++){
 		if(comp->out_conditions_executed[i] !=0){
@@ -128,7 +134,6 @@ fmi2Status fmi2GetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t n
 		comp->out_conditions_executed[0] = 1;
 	}
 
-
 	for(int i=0; i<_NR_OF_OUT_CONDITIONS;i++){
 		if(comp->out_conditions_executed[i]){
 			comp->r[_out_tau] =  comp->stored_window_reaction_torque;
@@ -346,10 +351,9 @@ static fmi2Status DoInnerStep(fmi2Component fc, int index, fmi2Real currentCommP
 	if (1){
 		fmi2ValueReference vr_fromWindow[2] = {1,2};
 		fmi2Real out_values[2];
-		fi->fmu[index].getReal(fi->c_fmu[index],vr_fromWindow, 3, &out_values[0]);
+		fi->fmu[index].getReal(fi->c_fmu[index],vr_fromWindow, 2, &out_values[0]);
 		fi->stored_window_reaction_torque = out_values[0];
 		fi->stored_window_height = out_values[1];
-
 	}
 	return status;
 }

File diff suppressed because it is too large
+ 4 - 3
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_loop_sa/.cproject


+ 44 - 29
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_loop_sa/FMI_loop_sa.c

@@ -9,7 +9,6 @@ Template for a  FMU
 #define MODEL_GUID "{41f87101-edf2-4eef-90f3-42db56d4565f}"
 #define FMI2_FUNCTION_PREFIX LOOP_SA
 
-
 #include <stdio.h>
 #include "string.h"
 #include "fmi2Functions.h"
@@ -196,7 +195,6 @@ fmi2Component fmi2Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2Str
 		fi->s = functions->allocateMemory(NUMBER_OF_STRINGS,  sizeof(fmi2String));
 	} // variables in predefined arrays (performance issue) --> makes multiple instances of fmu impossible
 
-
 	fi->instanceName = functions->allocateMemory(1 + strlen(instanceName), sizeof(char));
 	fi->GUID = functions->allocateMemory(1 + strlen(fmuGUID), sizeof(char));
 
@@ -208,20 +206,29 @@ fmi2Component fmi2Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2Str
 	fi->state = fmuInstantiated;
 	/* Load the inner FMUs:*/
 
+	printf("%s loading internal dlls...\n",instanceName);
+
 	loadDll("libFMI_Window_sa.dll", &(fi->fmu[_window_sa]), "WINDOW_SA");
 	fi->fmuResourceLocation[_window_sa] = "libFMI_Window_sa";
 	loadDll("Obstacle.dll", &(fi->fmu[_obstacle]),"");
 	fi->fmuResourceLocation[_obstacle] = "Obstacle.dll";
 
 	fi->fmu_guid[_window_sa]= functions->allocateMemory(1 + strlen("1"), sizeof(char));
-	fi->fmu_guid[_obstacle] = functions->allocateMemory(1 + strlen("{547938fa-ae1c-44b9-971b-64c1c3344c33}"), sizeof(char));
+	fi->fmu_guid[_obstacle] = functions->allocateMemory(1 + strlen("{f5edf8de-102d-41c8-a439-ded057d2dee2}"), sizeof(char));
 	strcpy(fi->fmu_guid[_window_sa], "1");
-	strcpy(fi->fmu_guid[_obstacle], "{8de5bd74-8d30-4a72-9170-0e4bf874b6a8}");
+	strcpy(fi->fmu_guid[_obstacle], "{f5edf8de-102d-41c8-a439-ded057d2dee2}");
 
 	/*Instantiate inner components*/
+	printf("%s instantiating internal fmus...\n",instanceName);
+
+	fmi2Boolean childLoggingOn = fmi2True;
 
-	fi->c_fmu[_window_sa] = fi->fmu[_window_sa].instantiate("window_sa", fmi2CoSimulation, fi->fmu_guid[_window_sa], fi->fmuResourceLocation[_window_sa] , fi->functions, visible, 0);
-	fi->c_fmu[_obstacle] = fi->fmu[_obstacle].instantiate("obstacle", fmi2CoSimulation, fi->fmu_guid[_obstacle], fi->fmuResourceLocation[_obstacle] , fi->functions, visible, 0);
+	fi->c_fmu[_window_sa] = fi->fmu[_window_sa].instantiate("window_sa", fmi2CoSimulation, fi->fmu_guid[_window_sa], fi->fmuResourceLocation[_window_sa] , fi->functions, visible, childLoggingOn);
+	fi->c_fmu[_obstacle] = fi->fmu[_obstacle].instantiate("obstacle", fmi2CoSimulation, fi->fmu_guid[_obstacle], fi->fmuResourceLocation[_obstacle] , fi->functions, visible, childLoggingOn);
+
+	fmi2String categories[] = {"logAll"};
+	fi->fmu[_window_sa].setDebugLogging(fi->c_fmu[_window_sa], childLoggingOn, 1, categories);
+	fi->fmu[_obstacle].setDebugLogging(fi->c_fmu[_obstacle], childLoggingOn, 1, categories);
 
 	return fi;
 }
@@ -229,12 +236,11 @@ fmi2Component fmi2Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2Str
 fmi2Status fmi2SetupExperiment(fmi2Component fc, fmi2Boolean toleranceDefined, fmi2Real tolerance,
 		fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime) {
 
-
 	FMUInstance* fi = (FMUInstance*) fc;
 	printf("%s in fmiSetupExperiment\n",fi->instanceName);
 	if (fi->state != fmuInstantiated)
 	{
-		printf("fmu: %s was not instatiated before calling fmiSetupExperiment\n", fi->instanceName);
+		printf("fmu: %s was not instantiated before calling fmiSetupExperiment\n", fi->instanceName);
 		return fmi2Error;
 	}
 	fi->currentTime = startTime;
@@ -310,21 +316,26 @@ static fmi2Status DoInnerStep(fmi2Component fc, int index, fmi2Real currentCommP
 
 	fmi2Real dt =currentCommPoint - fi->time_last_fmu[index];
 	fmi2Real h = commStepSize + dt;
-	int repeat = 0;
+	fmi2Boolean converged = 0;
 
-	fmi2Component obstacle_temp, window_sa_temp;
+	//fmi2Component obstacle_temp, window_sa_temp;
 	fmi2ValueReference vr_to_window_sa[3]={2,3,4};
 	fmi2ValueReference vr_disp[1] = {2};
 	fmi2ValueReference vr_F_obstacle[1] = {0};
 	fmi2ValueReference vr_from_window[2] = {0,1};
 	fmi2Real toWindowSA[3];
 	fmi2Real fromWindow[2];
-	for (int iter= 0; iter< MAXITER; iter++) {
-		fi->fmu[_obstacle].getFMUstate(fi->c_fmu[_obstacle], &obstacle_temp);
-		fi->fmu[_window_sa].getFMUstate(fi->c_fmu[_window_sa], &window_sa_temp);
+
+	fi->fmu[_obstacle].doStep(fi->c_fmu[_obstacle], currentCommPoint, h, fmi2False);
+	fi->fmu[_window_sa].doStep(fi->c_fmu[_window_sa], currentCommPoint, h, fmi2False);
+
+	int iter;
+	for (iter = 0; iter<MAXITER; iter++) {
+		// Since these FMUs are stateless, no need to do this.
+		//fi->fmu[_obstacle].getFMUstate(fi->c_fmu[_obstacle], &obstacle_temp);
+		//fi->fmu[_window_sa].getFMUstate(fi->c_fmu[_window_sa], &window_sa_temp);
 
 		fi->fmu[_obstacle].setReal(fi->c_fmu[_obstacle], vr_disp,1, &fi->prev_disp);
-		fi->fmu[_obstacle].doStep(fi->c_fmu[_obstacle], currentCommPoint, h, fmi2False);
 		fi->fmu[_obstacle].getReal(fi->c_fmu[_obstacle],vr_F_obstacle,1,&toWindowSA[0]);
 		printf("from obstacle = %f\n", toWindowSA[0]);
 
@@ -332,30 +343,34 @@ static fmi2Status DoInnerStep(fmi2Component fc, int index, fmi2Real currentCommP
 		printf("to window displacement = %f\n", fi->r[_in_displacement]);
 		toWindowSA[2] = fi->r[_in_speed];
 		printf("to window speed = %f\n", fi->r[_in_speed]);
+		fflush(stdout);
 		fi->fmu[_window_sa].setReal(fi->c_fmu[_window_sa], vr_to_window_sa, 3, &toWindowSA[0]);
-		fi->fmu[_window_sa].doStep(fi->c_fmu[_window_sa], currentCommPoint, h, fmi2False);
 		fi->fmu[_window_sa].getReal(fi->c_fmu[_window_sa], vr_from_window, 2, &fromWindow[0]);
 
-		repeat = is_close(fi->prev_disp, fromWindow[1], REL_TOL, ABS_TOL);
+		converged = is_close(fi->prev_disp, fromWindow[1], REL_TOL, ABS_TOL);
 		fi->prev_disp = fromWindow[1];\
 
-		if (repeat) {
-			fi->fmu[_obstacle].setFMUstate(fi->c_fmu[_obstacle], obstacle_temp);
-			fi->fmu[_window_sa].setFMUstate(fi->c_fmu[_window_sa], window_sa_temp);
-			fi->fmu[_obstacle].doStep(fi->c_fmu[_obstacle], currentCommPoint, h, fmi2True);
-			fi->fmu[_window_sa].doStep(fi->c_fmu[_window_sa], currentCommPoint, h, fmi2True);
-//			fi->fmu[_obstacle].freeInstance(obstacle_temp);
-//			fi->fmu[_window_sa].freeInstance(window_sa_temp);
+		if (converged) {
+			printf("Step converged!\n");
+			fflush(stdout);
 			break;
 		} else {
-			//rollback(obstacle);
-			fi->fmu[_obstacle].setFMUstate(fi->c_fmu[_obstacle], obstacle_temp);
-			fi->fmu[_window_sa].setFMUstate(fi->c_fmu[_window_sa], window_sa_temp);
-//			fi->fmu[_obstacle].freeInstance(obstacle_temp);
-//			fi->fmu[_window_sa].freeInstance(window_sa_temp);
-			//rollback(window_sa);
+			printf("Rolling back and repeating step...\n");
+			fflush(stdout);
+			// Since these FMUs are stateless, no need to do this.
+			//fi->fmu[_obstacle].setFMUstate(fi->c_fmu[_obstacle], obstacle_temp);
+			//fi->fmu[_window_sa].setFMUstate(fi->c_fmu[_window_sa], window_sa_temp);
 		}
+
+	}
+
+	if (iter == MAXITER){
+		printf("Loop_sa did not converge within %d iterations!\n", MAXITER);
+		return fmi2Fatal;
+		fflush(stdout);
 	}
+
+
 	if(1){
 		fi->r[_out_tau] = fromWindow[0];
 	}

+ 8 - 0
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_loop_sa/build.ps1

@@ -3,3 +3,11 @@ rm "Debug\*"
 mingw32-make clean makefile
 mingw32-make lib makefile
 
+Echo "Exporting dependencies..."
+
+Push-Location ..\FMI_Window_sa\
+	& .\build.ps1
+Pop-Location
+
+copy ..\FMI_Window_sa\Debug\* .\Debug\
+copy ..\ThirdParty\FMI_Obstacle\binaries\win32\*.dll Debug\

+ 85 - 0
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_loop_sa/test.c

@@ -0,0 +1,85 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include "fmi2.h"
+#include "sim_support.h"
+
+#define START_TIME 0.0
+#define STOP_TIME 6.0
+#define STEP_SIZE 0.01
+
+FMU fmu_loop_sa;
+
+int main(void) {
+
+	puts("Loading Dlls\n");
+	/* loading */
+    loadDll("libFMI_loop_sa.dll", &fmu_loop_sa, "LOOP_SA");
+
+    puts("instantiating fmus\n");
+    fmi2Component c_loop_sa;
+
+    char *fmuResourceLocation_window_sa = "libFMI_loop_sa.dll";
+
+    fmi2CallbackFunctions callbacks_loop_sa = {fmuLogger, calloc, free, NULL, &fmu_loop_sa};
+
+    c_loop_sa = fmu_loop_sa.instantiate("loop_sa",fmi2CoSimulation, "1", fmuResourceLocation_window_sa, &callbacks_loop_sa, fmi2False, fmi2False );
+
+    fmi2Boolean toleranceDefined = fmi2True;  // true if model description define tolerance
+    fmi2Real tolerance = 1e-1;                    // used in setting up the experiment
+
+    puts("FMU components instantiated, setting up experiments\n");
+    fmi2Status fmi2Flag;
+
+    fmi2Flag = fmu_loop_sa.setupExperiment(c_loop_sa, toleranceDefined, tolerance, START_TIME, fmi2True, STOP_TIME);
+    if (fmi2Flag == fmi2Error){
+    	return -1;
+    }
+
+    puts("Experiment setup, entering init mode\n");
+
+    fmi2Flag = fmu_loop_sa.enterInitializationMode(c_loop_sa);
+    if (fmi2Flag == fmi2Error){
+    	return -1;
+    }
+
+    puts("Experiment setup, exiting init mode\n");
+
+    fmi2Flag = fmu_loop_sa.exitInitializationMode(c_loop_sa);
+    if (fmi2Flag == fmi2Error){
+    	return -1;
+    }
+
+    fmi2ValueReference vr_out_loop_sa[1] = {0};
+    fmi2Real r_out_loop_sa[1];
+    fmi2ValueReference vr_toLoop[2] = {1,2}; // speed, displacement
+	fmi2Real toLoop[2];
+
+    double currentTime = START_TIME;
+
+    while(currentTime <= STOP_TIME){
+    	printf("\n----master new loop, ct:%f, h:%f\n",currentTime,STEP_SIZE);
+
+    	toLoop[0] = 1.0f;
+    	toLoop[1] = currentTime;
+
+    	if (fmu_loop_sa.setReal(c_loop_sa, vr_toLoop, 2, &toLoop[0]) != fmi2OK){
+    		return 1;
+		}
+    	fmi2Flag = fmu_loop_sa.doStep(c_loop_sa, currentTime, STEP_SIZE, fmi2True);
+    	fflush(stdout);
+    	if (fmu_loop_sa.getReal(c_loop_sa, vr_out_loop_sa, 1, &r_out_loop_sa[0]) != fmi2OK){
+    		return 1;
+		}
+
+    	printf("%f,%f\n",
+						currentTime,
+						r_out_loop_sa[0]
+						);
+
+    	currentTime += STEP_SIZE;
+
+    	fflush(stdout);
+
+    }
+	return EXIT_SUCCESS;
+}

BIN
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/ThirdParty/FMI_Obstacle.fmu