Browse Source

fixed issues with controller and tests working

Cláudio Gomes 3 years ago
parent
commit
df8c129b86

+ 7 - 9
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_controller/PW_Controller.c

@@ -198,6 +198,7 @@ fmi2Status fmi2ExitInitializationMode(fmi2Component fc)
 	if (fi->state != fmuInitMode)
 	{
 		printf("fmu: %s did not enter Initialization Mode before calling fmiExitInitializationMode\n", fi->instanceName);
+		fflush(stdout);
 		return fmi2Error;
 	}
 	// TODO
@@ -205,6 +206,7 @@ fmi2Status fmi2ExitInitializationMode(fmi2Component fc)
 	//initialize();
 	powerwindow_enter(fi->Handle);
     fi->state = fmuInitialized;
+    printf("fmu: %s finish fmiExitInitializationMode\n", fi->instanceName);
     return initStatus;
 }
 
@@ -232,16 +234,12 @@ fmi2Status fmi2DoStep(fmi2Component fc , fmi2Real currentCommPoint, fmi2Real com
     	}
     }else{powerwindow_timeradvance(fi->thePWTimer, (currentCommPoint+commStepSize)*1000);}
 
-
-
-
     if (fi->b[_in_driver_up]){
     	powerwindowIfaceInput_raise_driver_up(fi->Handle);
     	printf("raise up driver\n");
     }
 
-
-	if(fi->b[ _in_driver_down]){
+	if(fi->b[_in_driver_down]){
 		powerwindowIfaceInput_raise_driver_down(fi->Handle);
 		printf("raise down driver\n");
 	}
@@ -250,7 +248,7 @@ fmi2Status fmi2DoStep(fmi2Component fc , fmi2Real currentCommPoint, fmi2Real com
 		powerwindowIfaceInput_raise_stop(fi->Handle);
 		printf("raise up stop\n");
 	}
-	if(fi->b[ _in_driver_down_stop]){
+	if(fi->b[_in_driver_down_stop]){
 		powerwindowIfaceInput_raise_stop(fi->Handle);
 		printf("raise driver down stop\n");
 	}
@@ -272,13 +270,13 @@ fmi2Status fmi2DoStep(fmi2Component fc , fmi2Real currentCommPoint, fmi2Real com
 	}
 	if(fi->b[_in_obj_detected]){
 		powerwindowIfaceInput_raise_obj_detected(fi->Handle);
+		printf("raise object detected\n");
 	}
 
 	powerwindow_runCycle(fi->Handle);
 
-
-	fi->b[_motor_up] = powerwindowIfaceOutput_get_down(fi->Handle);
-	fi->b[_motor_down] = powerwindowIfaceOutput_get_up(fi->Handle);
+	fi->b[_motor_up] = powerwindowIfaceOutput_get_up(fi->Handle);
+	fi->b[_motor_down] = powerwindowIfaceOutput_get_down(fi->Handle);
 	/*
 	 * Check timers and set
 	 */

+ 2 - 0
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_controller/build.ps1

@@ -2,3 +2,5 @@ rm "Debug\*"
 
 mingw32-make clean makefile
 mingw32-make lib makefile
+
+pause

+ 40 - 31
SemanticAdaptationForFMI/Experiments/power_window_case_study/fmus/FMI_controller/test.c

@@ -12,15 +12,15 @@
 #include "fmi2.h"
 #include "sim_support.h"
 
-#define _out_driver_up 0
-#define _out_driver_up_stop 1
-#define _out_driver_down 2
-#define _out_driver_down_stop 3
-#define _out_passenger_up 4
-#define _out_passenger_up_stop 5
-#define _out_passenger_down 6
-#define _out_passenger_down_stop 7
-#define _out_object 8
+#define _out_object 0
+#define _out_driver_up 1
+#define _out_driver_up_stop 2
+#define _out_driver_down 3
+#define _out_driver_down_stop 4
+#define _out_passenger_up 5
+#define _out_passenger_up_stop 6
+#define _out_passenger_down 7
+#define _out_passenger_down_stop 8
 
 #define START_TIME 0.0
 #define STOP_TIME 9.0
@@ -58,16 +58,25 @@ int main(void) {
     	return -1;
     }
 
-    fmi2Flag = fmu_control.exitInitializationMode(c_control);
+    puts("Experiment setup, exiting init mode\n");
+        fmi2Flag = fmu_control.exitInitializationMode(c_control);
     if (fmi2Flag == fmi2Error){
     	return -1;
     }
 
-    fmi2ValueReference vr_in_control_sa_from_env[8]={0,1,2,3,4,5,6,7};
+    fmi2ValueReference vr_in_control_sa_from_env[9]={
+    		_out_object,
+			_out_driver_up,
+			_out_driver_up_stop,
+			_out_driver_down,
+			_out_driver_down_stop,
+			_out_passenger_up,
+			_out_passenger_up_stop,
+			_out_passenger_down,
+			_out_passenger_down_stop
+    };
     fmi2Boolean b_out_env[9];
-    fmi2ValueReference vr_in_control_sa_from_power[1] = {0};
-    fmi2Real in_control_sa_from_power[1] = {0.0f};
-    fmi2ValueReference vr_out_control_sa[2]={9,10};
+    fmi2ValueReference vr_out_control_sa[2]={9,10}; // Up, Down
     fmi2Boolean b_out_control_sa[2];
 
     double currentTime = START_TIME;
@@ -76,6 +85,7 @@ int main(void) {
     	printf("\n----master new loop, ct:%f, h:%f\n",currentTime,STEP_SIZE);
 
     	if(currentTime < 0.01){
+			b_out_env[_out_object] = 0;
 			b_out_env[_out_driver_up] = 0;
 			b_out_env[_out_driver_up_stop] = 0;
 			b_out_env[_out_driver_down] = 0;
@@ -84,8 +94,8 @@ int main(void) {
 			b_out_env[_out_passenger_up_stop] = 0;
 			b_out_env[_out_passenger_down] = 0;
 			b_out_env[_out_passenger_down_stop] = 0;
-			b_out_env[_out_object] = 0;
 		}else if(currentTime < 4.59){
+			b_out_env[_out_object] = 0;
 			b_out_env[_out_driver_up] = 1;
 			b_out_env[_out_driver_up_stop] = 0;
 			b_out_env[_out_driver_down] = 0;
@@ -94,18 +104,18 @@ int main(void) {
 			b_out_env[_out_passenger_up_stop] = 0;
 			b_out_env[_out_passenger_down] = 0;
 			b_out_env[_out_passenger_down_stop] = 0;
-			b_out_env[_out_object] = 0;
 		}else if(currentTime < 5){
+			b_out_env[_out_object] = 1;
 			b_out_env[_out_driver_up] = 0;
-			b_out_env[_out_driver_up_stop] = 1;
+			b_out_env[_out_driver_up_stop] = 0;
 			b_out_env[_out_driver_down] = 0;
 			b_out_env[_out_driver_down_stop] = 0;
 			b_out_env[_out_passenger_up] = 0;
 			b_out_env[_out_passenger_up_stop] = 0;
 			b_out_env[_out_passenger_down] = 0;
 			b_out_env[_out_passenger_down_stop] = 0;
-			b_out_env[_out_object] = 0;
 		}else if(currentTime < 5.01){
+			b_out_env[_out_object] = 0;
 			b_out_env[_out_driver_up] = 0;
 			b_out_env[_out_driver_up_stop] = 0;
 			b_out_env[_out_driver_down] = 0;
@@ -114,9 +124,9 @@ int main(void) {
 			b_out_env[_out_passenger_up_stop] = 0;
 			b_out_env[_out_passenger_down] = 0;
 			b_out_env[_out_passenger_down_stop] = 0;
-			b_out_env[_out_object] = 0;
 		}else if(currentTime < 6.7){
-			b_out_env[_out_driver_up] = 0;
+			b_out_env[_out_object] = 0;
+			b_out_env[_out_driver_up] = 1;
 			b_out_env[_out_driver_up_stop] = 0;
 			b_out_env[_out_driver_down] = 0;
 			b_out_env[_out_driver_down_stop] = 0;
@@ -124,18 +134,18 @@ int main(void) {
 			b_out_env[_out_passenger_up_stop] = 0;
 			b_out_env[_out_passenger_down] = 0;
 			b_out_env[_out_passenger_down_stop] = 0;
-			b_out_env[_out_object] = 0;
 		}else if(currentTime < 7.1){
+			b_out_env[_out_object] = 0;
 			b_out_env[_out_driver_up] = 0;
-			b_out_env[_out_driver_up_stop] = 0;
-			b_out_env[_out_driver_down] = 1;
+			b_out_env[_out_driver_up_stop] = 1;
+			b_out_env[_out_driver_down] = 0;
 			b_out_env[_out_driver_down_stop] = 0;
 			b_out_env[_out_passenger_up] = 0;
 			b_out_env[_out_passenger_up_stop] = 0;
 			b_out_env[_out_passenger_down] = 0;
 			b_out_env[_out_passenger_down_stop] = 0;
+		}else {
 			b_out_env[_out_object] = 0;
-		}else if(currentTime < 8.5){
 			b_out_env[_out_driver_up] = 0;
 			b_out_env[_out_driver_up_stop] = 0;
 			b_out_env[_out_driver_down] = 0;
@@ -144,23 +154,22 @@ int main(void) {
 			b_out_env[_out_passenger_up_stop] = 0;
 			b_out_env[_out_passenger_down] = 0;
 			b_out_env[_out_passenger_down_stop] = 0;
-			b_out_env[_out_object] = 0;
 		}
 
     	/* do step*/
     	fmi2Flag = fmu_control.setBoolean(c_control,vr_in_control_sa_from_env,8,&b_out_env[0]);
     	if(fmi2Flag != fmi2OK){
-    		return 1;
-    	}
-    	fmi2Flag = fmu_control.setReal(c_control,vr_in_control_sa_from_power,1, &in_control_sa_from_power[0]);
-    	if(fmi2Flag != fmi2OK){
-    		return 1;
+    		printf("Failed setBoolean\n");
+    		fflush(stdout);
+			return 1;
     	}
     	fflush(stdout);
     	fmi2Flag = fmu_control.doStep(c_control, currentTime, STEP_SIZE, fmi2True);
 
     	if (fmu_control.getBoolean(c_control, vr_out_control_sa, 2, &b_out_control_sa[0]) != fmi2OK){
-    		return 1;
+    		printf("Failed getBoolean\n");
+			fflush(stdout);
+			return 1;
 		}
 
     	if(fmi2Flag == fmi2Discard){