controller tested and working

.gitignore

@@ -47,3 +47,4 @@ xtend-gen/
 /SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_power_sa/log.txt
 /SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_loop_sa/log.txt
 /SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_controller/log.txt
+/SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_controller_sa/log.txt

SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_controller_sa/FMI_GM.c

@@ -6,7 +6,7 @@ Template for a  FMU
  */
 
 #define MODEL_IDENTIFIER GM
-#define MODEL_GUID "{41f87101-edf2-4eef-90f3-42db56d4565f}"
+#define MODEL_GUID "1"
 #define FMI2_FUNCTION_PREFIX PW_CONTROLLER_SA_
 
 #define MAX 10000
@@ -32,7 +32,6 @@ Template for a  FMU
 
 #define _in_armature_current 0
 
-
 #define _in_driver_up 0
 #define _in_driver_up_stop 1
 #define _in_driver_down 2
@@ -105,6 +104,8 @@ fmi2Status fmi2SetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t n
 	in_condition[1] = 1;
 
 	if(in_condition[0]){
+		printf("%s: Calculating max allowed step size... \n",comp->instanceName);
+
 		fmi2Component fmu0_temp;
 		comp->fmu[0].getFMUstate(comp->c_fmu[0],&fmu0_temp);
 		fmi2Status status;
@@ -116,11 +117,14 @@ fmi2Status fmi2SetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t n
 		}else{
 			comp->next_time_step = -1;
 		}
+		printf("%s: Step size max = %f... \n",comp->instanceName, comp->next_time_step);
 		comp->fmu[0].setFMUstate(comp->c_fmu[0],fmu0_temp);
-		comp->fmu[0].freeInstance(fmu0_temp);
+		comp->fmu[0].freeInstance(fmu0_temp); // TODO: should be fmi2FreeFMUState
 	}
 	if(in_condition[1]){
 		comp->stored_arm_current = comp->r[_in_armature_current];
+		printf("%s: comp->stored_arm_current = %f... \n",comp->instanceName, comp->stored_arm_current);
+		printf("%s: comp->previous_arm_current = %f... \n",comp->instanceName, comp->previous_arm_current);
 		/* If mealy do update_in and recursive call */
 	}
 
@@ -225,7 +229,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));
 
@@ -237,7 +240,7 @@ fmi2Component fmi2Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2Str
 	fi->state = fmuInstantiated;
 	/* Load the inner FMUs:*/
 
-	loadDll("libpw_controller.dll", &(fi->fmu[0]), "PW_Controller");
+	loadDll("libFMI_controller.dll", &(fi->fmu[0]), "PW_Controller");
 	fi->fmuResourceLocation[0] = "libpw_controller";
 
 	/*Instantiate inner components*/
@@ -255,7 +258,7 @@ fmi2Status fmi2SetupExperiment(fmi2Component fc, fmi2Boolean toleranceDefined, f
 	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;
@@ -338,56 +341,82 @@ fmi2Status fmi2ExitInitializationMode(fmi2Component fc)
 fmi2Status fmi2DoStep(fmi2Component fc , fmi2Real currentCommPoint, fmi2Real commStepSize, fmi2Boolean noPrevFMUState)
 {
 	FMUInstance* fi = (FMUInstance *)fc;
-	fmi2Status simStatus = fmi2OK;
+	fmi2Status externalSimStatus = fmi2OK;
+	fmi2Status internalSimStatus = fmi2OK;
 	memset(fi->out_conditions_executed,0,sizeof(fmi2Boolean)*_NR_OF_OUT_CONDITIONS);
 	fmi2Boolean b_temp_out[2];
 
 	printf("%s in fmiDoStep(), ct:%f, h:%f\n",fi->instanceName,currentCommPoint,commStepSize);
 	fi->aux_obj_detected = 0;
 	fi->step_size = commStepSize;
-	if(!(! is_close(fi->previous_arm_current, fi->CROSSING , fi->REL_TOLERANCE, fi->ABS_TOLERANCE) && fi->previous_arm_current < fi->CROSSING)
-			&& (!is_close(fi->stored_arm_current,fi->CROSSING,fi->REL_TOLERANCE,fi->ABS_TOLERANCE) && fi->stored_arm_current > fi->CROSSING)){
+	if( (
+			(!is_close(fi->previous_arm_current, fi->CROSSING , fi->REL_TOLERANCE, fi->ABS_TOLERANCE))
+			&& fi->previous_arm_current < fi->CROSSING
+		)
+		&&
+		(
+			(!is_close(fi->stored_arm_current,fi->CROSSING,fi->REL_TOLERANCE,fi->ABS_TOLERANCE))
+			&& fi->stored_arm_current > fi->CROSSING
+		)
+	   ){
 		double negative_value = fi->previous_arm_current - fi->CROSSING;
 		double positive_value = fi->stored_arm_current - fi->CROSSING;
 		fi->step_size = (commStepSize * (-negative_value)) / (positive_value - negative_value);
-		simStatus = fmi2Discard;
+		fi->currentTime = currentCommPoint + fi->step_size;
+		externalSimStatus = fmi2Discard;
+		printf("%s: crossed too far, rejecting step size %f and proposing %f... \n",fi->instanceName, commStepSize, fi->step_size);
+		printf("%s: fi->previous_arm_current = %f \n",fi->instanceName, fi->previous_arm_current);
+		printf("%s: fi->stored_arm_current = %f \n",fi->instanceName, fi->stored_arm_current);
 	}else{
-		if(!(! is_close(fi->previous_arm_current,fi->CROSSING,fi->REL_TOLERANCE,fi->ABS_TOLERANCE) && fi->previous_arm_current < fi->CROSSING)
-				&& is_close(fi->stored_arm_current,fi->CROSSING, fi->REL_TOLERANCE, fi->ABS_TOLERANCE)){
+		if((
+				(!is_close(fi->previous_arm_current, fi->CROSSING , fi->REL_TOLERANCE, fi->ABS_TOLERANCE))
+				&& fi->previous_arm_current < fi->CROSSING
+			)
+			&&
+			(
+				is_close(fi->stored_arm_current,fi->CROSSING,fi->REL_TOLERANCE,fi->ABS_TOLERANCE)
+			)
+		  ){
 			fi->aux_obj_detected = 1;
+			printf("%s: crossed just right... \n",fi->instanceName);
 		}
-		fi->step_size = commStepSize;
-	}
-
-	if(fi->aux_obj_detected || (fi->next_time_step != -1 && currentCommPoint > fi->next_time_step ) ||1){ //this is not correct. It also has to work when new values are given to all of the bools
-		fmi2ValueReference vr_in_star[8] = {1,2,3,4,5,6,7,8};
-		fi->fmu[0].setBoolean(fi->c_fmu[0], vr_in_star,8, &(fi->b[0]));
-		fmi2ValueReference vr_aux_obj_detected[1] = {0};
-		fi->fmu[0].setBoolean(fi->c_fmu[0], vr_aux_obj_detected,1, &(fi->aux_obj_detected));
-		simStatus = fi->fmu[0].doStep(fi->c_fmu[0], currentCommPoint, commStepSize, fmi2True);
-		fmi2ValueReference vr_out_star[2]={9,10};
-		fi->fmu[0].getBoolean(fi->c_fmu[0],vr_out_star,2,&b_temp_out[0]);
-		fi->get_next_step = 1;
-	}else{
-		fi->get_next_step = 0;
-	}
-	if(is_close(fi->step_size, commStepSize, fi->REL_TOLERANCE,fi->ABS_TOLERANCE)){
-		fi->previous_arm_current = fi->stored_arm_current;
 	}
 
-	if(simStatus == fmi2Discard){
-		fmi2Real theNextTime = currentCommPoint + fi->step_size;
-		for(int i=0; i<1; i++){
-			fmi2Real theFMUtime;
-			fi->fmu[i].getRealStatus(fi->c_fmu[i], fmi2LastSuccessfulTime, &theFMUtime);
-			if(theFMUtime<theNextTime){
-				theNextTime = theFMUtime;
+	if (externalSimStatus == fmi2OK){ // only do the internal step if the current step is OK
+		if (((fi->next_time_step != -1 && currentCommPoint > fi->next_time_step ) || 1)){
+			fmi2ValueReference vr_in_star[8] = {1,2,3,4,5,6,7,8};
+			fi->fmu[0].setBoolean(fi->c_fmu[0], vr_in_star,8, &(fi->b[0]));
+			fmi2ValueReference vr_aux_obj_detected[1] = {0};
+			fi->fmu[0].setBoolean(fi->c_fmu[0], vr_aux_obj_detected,1, &(fi->aux_obj_detected));
+			internalSimStatus = fi->fmu[0].doStep(fi->c_fmu[0], currentCommPoint, commStepSize, fmi2True);
+			fmi2ValueReference vr_out_star[2]={9,10};
+			fi->fmu[0].getBoolean(fi->c_fmu[0],vr_out_star,2,&b_temp_out[0]);
+			fi->get_next_step = 1;
+		} else {
+			fi->get_next_step = 0;
+		}
+
+		if(internalSimStatus == fmi2Discard){
+			externalSimStatus = fmi2Discard;
+			fmi2Real theNextTime = currentCommPoint + fi->step_size;
+			for(int i=0; i<1; i++){
+				fmi2Real theFMUtime;
+				fi->fmu[i].getRealStatus(fi->c_fmu[i], fmi2LastSuccessfulTime, &theFMUtime);
+				if(theFMUtime<theNextTime){
+					theNextTime = theFMUtime;
+				}
 			}
+			fi->currentTime = theNextTime;
 		}
-		fi->currentTime = theNextTime;
-	}else if(simStatus == fmi2OK){
+	}
+
+
+
+	if(externalSimStatus == fmi2OK){
+		printf("%s: Step accepted, committing arm current... \n",fi->instanceName);
 		fi->currentTime = currentCommPoint + commStepSize;
 		fi->time_last_fmu[0] = currentCommPoint + commStepSize;
+		fi->previous_arm_current = fi->stored_arm_current;
 	}
 
 	/* do out functions*/
@@ -405,7 +434,7 @@ fmi2Status fmi2DoStep(fmi2Component fc , fmi2Real currentCommPoint, fmi2Real com
 	}
 
 	memset(fi->in_condition_executed, 0, sizeof(fmi2Boolean)*_NR_OF_IN_CONDITIONS);
-	return simStatus;
+	return externalSimStatus;
 }
 
 fmi2Status fmi2Terminate(fmi2Component fc)

SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_controller_sa/build.ps1

@@ -3,3 +3,6 @@ rm "Debug\*"
 mingw32-make clean makefile
 mingw32-make lib makefile
 
+copy ..\FMI_controller\Debug\* .\Debug\
+
+pause

SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_controller_sa/test.c

@@ -0,0 +1,170 @@
+/*
+ ============================================================================
+ Name        : pw_case.c
+ Author      : Joachim Denil
+ Version     :
+ Copyright   : Your copyright notice
+ ============================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "fmi2.h"
+#include "sim_support.h"
+
+#define START_TIME 0.0
+#define STOP_TIME 9.0
+#define STEP_SIZE 0.1
+
+#define _in_armature_current 0
+
+#define _in_driver_up 0
+#define _in_driver_up_stop 1
+#define _in_driver_down 2
+#define _in_driver_down_stop 3
+#define _in_passenger_up 4
+#define _in_passenger_up_stop 5
+#define _in_passenger_down 6
+#define _in_passenger_down_stop 7
+
+#define _out_u 9
+#define _out_d 10
+
+FMU fmu_control_sa;
+
+int main(void) {
+
+	puts("FMI_controller_sa: Loading Dlls\n");
+	/* loading */
+    loadDll("libFMI_controller_sa.dll", &fmu_control_sa, "PW_CONTROLLER_SA_");
+
+    puts("instantiating fmus\n");
+    fmi2Component  c_control_sa;
+
+    char *fmuResourceLocation_control_sa = "libFMI_controller_sa.dll";
+
+    fmi2CallbackFunctions callbacks_control_sa = {fmuLogger, calloc, free, NULL, &fmu_control_sa};
+
+    c_control_sa = fmu_control_sa.instantiate("control_sa", fmi2CoSimulation, "1", fmuResourceLocation_control_sa, &callbacks_control_sa, fmi2False, fmi2False);
+
+    fmi2Boolean toleranceDefined = fmi2False;  // true if model description define tolerance
+    fmi2Real tolerance = 0;                    // used in setting up the experiment
+
+    puts("FMU components instantiated, setting up experiments\n");
+    fmi2Status fmi2Flag;
+    fmi2Flag = fmu_control_sa.setupExperiment(c_control_sa, toleranceDefined, tolerance, START_TIME, fmi2True, STOP_TIME);
+    if (fmi2Flag == fmi2Error){
+    	return -1;
+    }
+
+    puts("Experiment setup, entering init mode\n");
+    fmi2Flag = fmu_control_sa.enterInitializationMode(c_control_sa);
+    if (fmi2Flag == fmi2Error){
+    	return -1;
+    }
+
+    puts("Experiment setup, exiting init mode\n");
+    fmi2Flag = fmu_control_sa.exitInitializationMode(c_control_sa);
+    if (fmi2Flag == fmi2Error){
+    	return -1;
+    }
+
+    fmi2ValueReference vr_in_control_sa_from_env[8]={0,1,2,3,4,5,6,7};
+    fmi2Boolean b_out_env[9];
+    fmi2ValueReference vr_out_control_sa[2]={9,10};
+    fmi2Boolean b_out_control_sa[2];
+    fmi2ValueReference vr_in_control_sa_from_window[1] = {0};
+    fmi2Real r_out_power[3];
+
+    double currentTime = START_TIME;
+	double next_step_size = STEP_SIZE;
+
+    while(currentTime <= STOP_TIME){
+    	printf("\n----master new loop, ct:%f, h:%f\n",currentTime,next_step_size);
+
+		fmi2Component ctemp_control_sa;
+
+		fmu_control_sa.getFMUstate(c_control_sa, &ctemp_control_sa);
+
+    	if(currentTime < 0.01){
+    		r_out_power[_in_armature_current] = 0.0f;
+			b_out_env[_in_driver_up] = 0;
+			b_out_env[_in_driver_up_stop] = 0;
+			b_out_env[_in_driver_down] = 0;
+			b_out_env[_in_driver_down_stop] = 0;
+			b_out_env[_in_passenger_up] = 0;
+			b_out_env[_in_passenger_up_stop] = 0;
+			b_out_env[_in_passenger_down] = 0;
+			b_out_env[_in_passenger_down_stop] = 0;
+		}else if(currentTime < 4){
+    		r_out_power[_in_armature_current] = 1.44*(currentTime + next_step_size); // crossing occurs at 3.47...
+			b_out_env[_in_driver_up] = 1;
+			b_out_env[_in_driver_up_stop] = 0;
+			b_out_env[_in_driver_down] = 0;
+			b_out_env[_in_driver_down_stop] = 0;
+			b_out_env[_in_passenger_up] = 0;
+			b_out_env[_in_passenger_up_stop] = 0;
+			b_out_env[_in_passenger_down] = 0;
+			b_out_env[_in_passenger_down_stop] = 0;
+		} else {
+			r_out_power[_in_armature_current] = 1.44*3.47 - (currentTime + next_step_size - 3.47);
+			b_out_env[_in_driver_up] = 1;
+			b_out_env[_in_driver_up_stop] = 0;
+			b_out_env[_in_driver_down] = 0;
+			b_out_env[_in_driver_down_stop] = 0;
+			b_out_env[_in_passenger_up] = 0;
+			b_out_env[_in_passenger_up_stop] = 0;
+			b_out_env[_in_passenger_down] = 0;
+			b_out_env[_in_passenger_down_stop] = 0;
+		}
+
+    	fmi2Flag = fmu_control_sa.setBoolean(c_control_sa,vr_in_control_sa_from_env,8,&b_out_env[0]);
+    	if(fmi2Flag != fmi2OK){
+    		printf("Failed setBoolean\n");
+			fflush(stdout);
+    		return 1;
+    	}
+    	fmi2Flag = fmu_control_sa.setReal(c_control_sa,vr_in_control_sa_from_window,1, &r_out_power[0]);
+    	if(fmi2Flag != fmi2OK){
+    		printf("Failed setReal\n");
+    		fflush(stdout);
+    	    return 1;
+    	}
+    	fmi2Flag = fmu_control_sa.doStep(c_control_sa, currentTime, next_step_size, fmi2True);
+
+    	if (fmu_control_sa.getBoolean(c_control_sa, vr_out_control_sa, 2, &b_out_control_sa[0]) != fmi2OK){
+    		printf("Failed getBoolean\n");
+    		fflush(stdout);
+			return 1;
+		}
+
+		printf("%f,%d,%d\n",
+				currentTime,
+				b_out_control_sa[0],
+				b_out_control_sa[1]);
+
+    	if (fmi2Flag == fmi2Discard){
+    		printf("Discard occurred!\n");
+
+			fmi2Real newtime;
+
+			fmu_control_sa.getRealStatus(c_control_sa, fmi2LastSuccessfulTime, &newtime);
+			fmi2Real the_FMU_new_step = newtime - currentTime;
+			if(the_FMU_new_step < next_step_size){
+				printf("Control_sa simulated partially with a step size of %f!\n", the_FMU_new_step);
+				next_step_size = the_FMU_new_step;
+
+				printf("Rolling back Control_sa...\n");
+				fmu_control_sa.setFMUstate(c_control_sa, ctemp_control_sa);
+			}
+		} else {
+			printf("Step accepted, advancing time and adjusting next step size...\n");
+			currentTime += next_step_size;
+			next_step_size = STEP_SIZE;
+		}
+
+    	fflush(stdout);
+
+    }
+	return EXIT_SUCCESS;
+}

SemanticAdaptationForFMI/Experiments/power_window_case_study/pw_case.c

@@ -131,8 +131,6 @@ int main(void) {
     double currentTime = START_TIME;
     const fmi2StatusKind lst = fmi2LastSuccessfulTime;
 
-
-
     while(currentTime <= STOP_TIME){
     	printf("\n----master new loop, ct:%f, h:%f\n",currentTime,STEP_SIZE);
     	double next_step_size = STEP_SIZE;