HybridCosimulation/DSL_SemanticAdaptation/be.uantwerpen.ansymo.semanticadaptation/examples/controller_sa_canonical.BASE.sa

import PowerWindowModel

module Controller_SA

semantic adaptation mealy reactive controller_sa

for fmu controller

input ports armature_current, 
			passenger_up, 
			passenger_down, 
			passenger_stop, 
			driver_up, 
			driver_down, 
			driver_stop;

output ports	u,
				d;

param REL_TOL = 0.0001;
param ABS_TOL = 1e-8;
param CROSSING = 5;
param init_armature_current = CROSSING;
param init_up = 0;
param init_down = 0;

in var previous_arm_current := init_armature_current;
in var stored_arm_current := init_armature_current;
in var stored_displacement := init_displacement;
in var stored_speed := init_speed;
in var next_time_step := -1;
in var step_size;
in var aux_obj_detected := 0;

in rules {
	next_time_step < 0 -> { 
		next_time_step := get_next_time_step(controller) + t; 
	} --> { };
	
	true -> {
		stored_arm_current := armature_current
	} --> {
		obj_detected := aux_obj_detected; // Sets this input to the FMU 
	};
}

control rules {
	aux_obj_detected := false;
	step_size := H;
	if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
				and (not is_close(stored_arm_current, CROSSING, REL_TOL, ABS_TOL) and stored_arm_current > CROSSING)) { // crossing, but not within tolerance
		var negative_value := previous_arm_current - CROSSING;
		var positive_value := stored_arm_current - CROSSING;
		step_size := (H * (- negative_value)) / (positive_value - negative_value);
	} else {
		if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
					and is_close(stored_arm_current, CROSSING, REL_TOL, ABS_TOL )) { // crossing within tolerance found
			aux_obj_detected := true;
		}
	}
	
	if (obj_detected == true or t >= next_time_step) {
		var aux_h = do_step(controller, t-e, e);
		assert aux_h == e; 
		next_time_step := -1; 
	}
	if is_close(step_size, H, REL_TOL, ABS_TOL) {
		previous_arm_current := stored_arm_current;
	}
	return step_size;
}

out var stored_up := init_up;
out var stored_down := init_down;

out rules {
	controller.up -> {stored_up := 1} --> {u := stored_up};
	not controller.up -> {stored_up := 0} --> {u := stored_up};
	
	controller.down -> {stored_down := 1} --> {d := stored_down};
	not controller.down -> {stored_down := 0} --> {d := stored_down};
	
	controller.stop -> {stored_down := 0; stored_up :=0 } --> {u := stored_up ; d := stored_down};
	
}