commit 37d1d83b477ad95746f292927e630db8ad3b0acf Author: Joeri Exelmans Date: Wed Jun 4 16:43:27 2025 +0200 initial solution diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e50951e --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +robot_cdt/build/ +.metadata/ diff --git a/robot_cdt/.cproject b/robot_cdt/.cproject new file mode 100644 index 0000000..2a6ac94 --- /dev/null +++ b/robot_cdt/.cproject @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot_cdt/.project b/robot_cdt/.project new file mode 100644 index 0000000..b7a41e3 --- /dev/null +++ b/robot_cdt/.project @@ -0,0 +1,26 @@ + + + robot_cdt + + + + + + org.eclipse.xtext.ui.shared.xtextBuilder + + + + + org.eclipse.cdt.core.cBuilder + clean,full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.make.core.makeNature + org.eclipse.xtext.ui.shared.xtextNature + + diff --git a/robot_cdt/.settings/org.eclipse.core.resources.prefs b/robot_cdt/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..99f26c0 --- /dev/null +++ b/robot_cdt/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,2 @@ +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/robot_cdt/Makefile b/robot_cdt/Makefile new file mode 100644 index 0000000..75db5ef --- /dev/null +++ b/robot_cdt/Makefile @@ -0,0 +1,31 @@ +SRCS = \ + src/controller.c \ + src/pid.c \ + src/sc_rxc.c \ + src/sc_timer_service.c \ + src-gen/Statechart.c \ + +TARGET = main +CC = gcc +CFLAGS = -Wall -Wextra + + +OBJS = $(addprefix build/,$(SRCS:.c=.o)) + +build/%.o: %.c create_build_dirs + $(CC) $(CFLAGS) -c $< -o $@ + +$(TARGET): $(OBJS) + $(CC) $(CFLAGS) -o build/$@ $^ + +# all: $(TARGET) +all: $(OBJS) + +create_build_dirs: + mkdir -p build/src + mkdir -p build/src-gen + +clean: + rm -rf build + +.PHONY: all create_build_dirs clean \ No newline at end of file diff --git a/robot_cdt/Statechart.sgen b/robot_cdt/Statechart.sgen new file mode 100644 index 0000000..87096f8 --- /dev/null +++ b/robot_cdt/Statechart.sgen @@ -0,0 +1,23 @@ +GeneratorModel for create::c { + + const PROJECT : string = "robot_cdt" + const FOLDER : string = "src-gen" + + statechart Statechart { + + feature Outlet { + targetProject = PROJECT + targetFolder = FOLDER + libraryTargetFolder = "src" + } + + feature GeneratorOptions { + userAllocatedQueue = false + } + + feature GeneralFeatures { + timerService = true + timerServiceTimeType = "" + } + } +} \ No newline at end of file diff --git a/robot_cdt/Statechart.ysc b/robot_cdt/Statechart.ysc new file mode 100644 index 0000000..4a0bc63 --- /dev/null +++ b/robot_cdt/Statechart.ysc @@ -0,0 +1,339 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robot_cdt/src-gen/Statechart.c b/robot_cdt/src-gen/Statechart.c new file mode 100644 index 0000000..9dc338c --- /dev/null +++ b/robot_cdt/src-gen/Statechart.c @@ -0,0 +1,874 @@ +/* Generated by itemis CREATE code generator. */ + + +#include "../src/sc_types.h" + +#include "Statechart.h" +#include "Statechart_required.h" + +/*! \file +Implementation of the state machine 'Statechart' +*/ + +#ifndef SC_UNUSED +#define SC_UNUSED(P) (void)(P) +#endif + +/* prototypes of all internal functions */ +static void enact_main_region_on_r1_responding(Statechart* handle); +static void enact_main_region_on_r1_sensor_error(Statechart* handle); +static void enact_main_region_on_r2_pressed(Statechart* handle); +static void exact_main_region_on_r2_pressed(Statechart* handle); +static void enseq_main_region_on_default(Statechart* handle); +static void enseq_main_region_on_r1_responding_default(Statechart* handle); +static void enseq_main_region_on_r1_sensor_error_default(Statechart* handle); +static void enseq_main_region_on_r2_not_pressed_default(Statechart* handle); +static void enseq_main_region_on_r2_pressed_default(Statechart* handle); +static void enseq_main_region_off_default(Statechart* handle); +static void enseq_main_region_default(Statechart* handle); +static void enseq_main_region_on_r1_default(Statechart* handle); +static void enseq_main_region_on_r2_default(Statechart* handle); +static void exseq_main_region_on(Statechart* handle); +static void exseq_main_region_on_r1_responding(Statechart* handle); +static void exseq_main_region_on_r1_sensor_error(Statechart* handle); +static void exseq_main_region_on_r2_not_pressed(Statechart* handle); +static void exseq_main_region_on_r2_pressed(Statechart* handle); +static void exseq_main_region_off(Statechart* handle); +static void exseq_main_region(Statechart* handle); +static void exseq_main_region_on_r1(Statechart* handle); +static void exseq_main_region_on_r2(Statechart* handle); +static void react_main_region_on_r1__choice_0(Statechart* handle); +static void react_main_region_on_r1__entry_Default(Statechart* handle); +static void react_main_region_on_r2__entry_Default(Statechart* handle); +static void react_main_region__entry_Default(Statechart* handle); + +/*! The reactions of state responding. */ +static sc_integer main_region_on_r1_responding_react(Statechart* handle, const sc_integer transitioned_before); + +/*! The reactions of state sensor_error. */ +static sc_integer main_region_on_r1_sensor_error_react(Statechart* handle, const sc_integer transitioned_before); + +/*! The reactions of state not_pressed. */ +static sc_integer main_region_on_r2_not_pressed_react(Statechart* handle, const sc_integer transitioned_before); + +/*! The reactions of state pressed. */ +static sc_integer main_region_on_r2_pressed_react(Statechart* handle, const sc_integer transitioned_before); + +/*! The reactions of state off. */ +static sc_integer main_region_off_react(Statechart* handle, const sc_integer transitioned_before); + + +static void clear_in_events(Statechart* handle); + +static void micro_step(Statechart* handle); + +/*! Performs a 'run to completion' step. */ +static void run_cycle(Statechart* handle); + + + + +static void statechart_internal_set_speed(Statechart* handle, sc_real value) +; +static void statechart_internal_set_pid_vars(Statechart* handle, pid_vars_t value) +; + + +static void statechart_eventqueue_init(statechart_eventqueue * eq, statechart_event *buffer, sc_integer capacity); +static sc_integer statechart_eventqueue_size(statechart_eventqueue * eq); +static void statechart_event_init(statechart_event * ev, StatechartEventID name); +static statechart_event statechart_eventqueue_pop(statechart_eventqueue * eq); +static sc_boolean statechart_eventqueue_push(statechart_eventqueue * eq, statechart_event ev); +static void statechart_add_event_to_queue(statechart_eventqueue * eq, StatechartEventID name); +static sc_boolean statechart_dispatch_event(Statechart* handle, const statechart_event * event); +static statechart_event statechart_get_next_event(Statechart* handle); +static sc_boolean statechart_dispatch_next_event(Statechart* handle); +static StatechartEventID statechart_get_timed_event_name(Statechart* handle, sc_eventid evid); +static void statechart_event_value_init(statechart_event * ev, StatechartEventID name, void * value); +static void statechart_add_value_event_to_queue(statechart_eventqueue * eq, StatechartEventID name, void * value); + + +void statechart_init(Statechart* handle) +{ + sc_integer i; + + for (i = 0; i < STATECHART_MAX_ORTHOGONAL_STATES; ++i) + { + handle->stateConfVector[i] = Statechart_last_state; + } + + + clear_in_events(handle); + + sc_observable_sc_real_init(&handle->iface.setMotorR); + sc_observable_sc_real_init(&handle->iface.setMotorL); + + /* Default init sequence for statechart Statechart */ + statechart_internal_set_speed(handle, 0.0); + + handle->isExecuting = bool_false; + statechart_eventqueue_init(&handle->in_event_queue, handle->in_buffer, STATECHART_IN_EVENTQUEUE_BUFFERSIZE); +} + +void statechart_enter(Statechart* handle) +{ + /* Activates the state machine. */ + if (handle->isExecuting == bool_true) + { + return; + } + handle->isExecuting = bool_true; + /* Default enter sequence for statechart Statechart */ + enseq_main_region_default(handle); + handle->isExecuting = bool_false; +} + +void statechart_exit(Statechart* handle) +{ + /* Deactivates the state machine. */ + if (handle->isExecuting == bool_true) + { + return; + } + handle->isExecuting = bool_true; + /* Default exit sequence for statechart Statechart */ + exseq_main_region(handle); + handle->stateConfVector[0] = Statechart_last_state; + handle->stateConfVector[1] = Statechart_last_state; + handle->stateConfVectorPosition = 1; + handle->isExecuting = bool_false; +} + +/*! +Can be used by the client code to trigger a run to completion step without raising an event. +*/ +void statechart_trigger_without_event(Statechart* handle) { + run_cycle(handle); +} + + +sc_boolean statechart_is_active(const Statechart* handle) +{ + sc_boolean result = bool_false; + sc_integer i; + + for(i = 0; i < STATECHART_MAX_ORTHOGONAL_STATES; i++) + { + result = result || handle->stateConfVector[i] != Statechart_last_state; + } + + return result; +} + +/* + * Always returns 'false' since this state machine can never become final. + */ +sc_boolean statechart_is_final(const Statechart* handle) +{ + SC_UNUSED(handle); + return bool_false; +} + +void statechart_raise_time_event(Statechart* handle, sc_eventid evid) +{ + if ( ((sc_intptr_t)evid) >= ((sc_intptr_t)&(handle->timeEvents)) + && ((sc_intptr_t)evid) < ((sc_intptr_t)&(handle->timeEvents)) + (unsigned)sizeof(StatechartTimeEvents)) + { + statechart_add_event_to_queue(&(handle->in_event_queue), statechart_get_timed_event_name(handle, evid)); + run_cycle(handle); + } +} + +sc_boolean statechart_is_state_active(const Statechart* handle, StatechartStates state) +{ + sc_boolean result = bool_false; + switch (state) + { + case Statechart_main_region_on : + result = (sc_boolean) (handle->stateConfVector[SCVI_STATECHART_MAIN_REGION_ON] >= Statechart_main_region_on + && handle->stateConfVector[SCVI_STATECHART_MAIN_REGION_ON] <= Statechart_main_region_on_r2_pressed); + break; + case Statechart_main_region_on_r1_responding : + result = (sc_boolean) (handle->stateConfVector[SCVI_STATECHART_MAIN_REGION_ON_R1_RESPONDING] == Statechart_main_region_on_r1_responding + ); + break; + case Statechart_main_region_on_r1_sensor_error : + result = (sc_boolean) (handle->stateConfVector[SCVI_STATECHART_MAIN_REGION_ON_R1_SENSOR_ERROR] == Statechart_main_region_on_r1_sensor_error + ); + break; + case Statechart_main_region_on_r2_not_pressed : + result = (sc_boolean) (handle->stateConfVector[SCVI_STATECHART_MAIN_REGION_ON_R2_NOT_PRESSED] == Statechart_main_region_on_r2_not_pressed + ); + break; + case Statechart_main_region_on_r2_pressed : + result = (sc_boolean) (handle->stateConfVector[SCVI_STATECHART_MAIN_REGION_ON_R2_PRESSED] == Statechart_main_region_on_r2_pressed + ); + break; + case Statechart_main_region_off : + result = (sc_boolean) (handle->stateConfVector[SCVI_STATECHART_MAIN_REGION_OFF] == Statechart_main_region_off + ); + break; + default: + result = bool_false; + break; + } + return result; + } + +static void clear_in_events(Statechart* handle) +{ + handle->iface.sensor_raised = bool_false; + handle->iface.buttonPressed_raised = bool_false; + handle->iface.buttonReleased_raised = bool_false; + handle->timeEvents.statechart_main_region_on_r2_pressed_tev0_raised = bool_false; +} + +static void micro_step(Statechart* handle) +{ + sc_integer transitioned = -1; + handle->stateConfVectorPosition = 0; + switch(handle->stateConfVector[ 0 ]) + { + case Statechart_main_region_on_r1_responding : + { + transitioned = main_region_on_r1_responding_react(handle,transitioned); + break; + } + case Statechart_main_region_on_r1_sensor_error : + { + transitioned = main_region_on_r1_sensor_error_react(handle,transitioned); + break; + } + case Statechart_main_region_off : + { + transitioned = main_region_off_react(handle,transitioned); + break; + } + default: + /* do nothing */ + break; + } + if ((handle->stateConfVectorPosition) < (1)) + { + switch(handle->stateConfVector[ 1 ]) + { + case Statechart_main_region_on_r2_not_pressed : + { + main_region_on_r2_not_pressed_react(handle,transitioned); + break; + } + case Statechart_main_region_on_r2_pressed : + { + main_region_on_r2_pressed_react(handle,transitioned); + break; + } + default: + /* do nothing */ + break; + } + } +} + +static void run_cycle(Statechart* handle) +{ + /* Performs a 'run to completion' step. */ + if (handle->isExecuting == bool_true) + { + return; + } + handle->isExecuting = bool_true; + statechart_dispatch_next_event(handle); + do + { + micro_step(handle); + clear_in_events(handle); + } while (statechart_dispatch_next_event(handle) == bool_true); + handle->isExecuting = bool_false; +} + + +void statechart_raise_sensor(Statechart* handle, sc_real value) +{ + statechart_add_value_event_to_queue(&(handle->in_event_queue), Statechart_sensor, &value); + run_cycle(handle); +} + +void statechart_raise_buttonPressed(Statechart* handle) +{ + statechart_add_event_to_queue(&(handle->in_event_queue), Statechart_buttonPressed); + run_cycle(handle); +} + +void statechart_raise_buttonReleased(Statechart* handle) +{ + statechart_add_event_to_queue(&(handle->in_event_queue), Statechart_buttonReleased); + run_cycle(handle); +} + + +sc_observable_sc_real* statechart_get_setMotorR(Statechart* handle) +{ + return &handle->iface.setMotorR; +} +sc_observable_sc_real* statechart_get_setMotorL(Statechart* handle) +{ + return &handle->iface.setMotorL; +} + + +static void statechart_internal_set_speed(Statechart* handle, sc_real value) +{ + handle->internal.speed = value; +} +static void statechart_internal_set_pid_vars(Statechart* handle, pid_vars_t value) +{ + handle->internal.pid_vars = value; +} + + +/* implementations of all internal functions */ + +/* Entry action for state 'responding'. */ +static void enact_main_region_on_r1_responding(Statechart* handle) +{ + /* Entry action for state 'responding'. */ + statechart_internal_set_speed(handle, statechart_pid(handle,handle->iface.sensor_value, handle->internal.pid_vars)); + { + sc_real iface_setMotorL_value = handle->internal.speed; + sc_observable_sc_real_next(&handle->iface.setMotorL, iface_setMotorL_value); + }; + { + sc_real iface_setMotorR_value = handle->internal.speed; + sc_observable_sc_real_next(&handle->iface.setMotorR, iface_setMotorR_value); + }; +} + +/* Entry action for state 'sensor_error'. */ +static void enact_main_region_on_r1_sensor_error(Statechart* handle) +{ + /* Entry action for state 'sensor_error'. */ + { + sc_real iface_setMotorL_value = 1; + sc_observable_sc_real_next(&handle->iface.setMotorL, iface_setMotorL_value); + }; + { + sc_real iface_setMotorR_value = -(1); + sc_observable_sc_real_next(&handle->iface.setMotorR, iface_setMotorR_value); + }; +} + +/* Entry action for state 'pressed'. */ +static void enact_main_region_on_r2_pressed(Statechart* handle) +{ + /* Entry action for state 'pressed'. */ + statechart_set_timer(handle, (sc_eventid) &(handle->timeEvents.statechart_main_region_on_r2_pressed_tev0_raised) , (((sc_time) 2) * 1000), bool_false); +} + +/* Exit action for state 'pressed'. */ +static void exact_main_region_on_r2_pressed(Statechart* handle) +{ + /* Exit action for state 'pressed'. */ + statechart_unset_timer(handle, (sc_eventid) &(handle->timeEvents.statechart_main_region_on_r2_pressed_tev0_raised) ); +} + +/* 'default' enter sequence for state on */ +static void enseq_main_region_on_default(Statechart* handle) +{ + /* 'default' enter sequence for state on */ + enseq_main_region_on_r1_default(handle); + enseq_main_region_on_r2_default(handle); +} + +/* 'default' enter sequence for state responding */ +static void enseq_main_region_on_r1_responding_default(Statechart* handle) +{ + /* 'default' enter sequence for state responding */ + enact_main_region_on_r1_responding(handle); + handle->stateConfVector[0] = Statechart_main_region_on_r1_responding; + handle->stateConfVectorPosition = 0; +} + +/* 'default' enter sequence for state sensor_error */ +static void enseq_main_region_on_r1_sensor_error_default(Statechart* handle) +{ + /* 'default' enter sequence for state sensor_error */ + enact_main_region_on_r1_sensor_error(handle); + handle->stateConfVector[0] = Statechart_main_region_on_r1_sensor_error; + handle->stateConfVectorPosition = 0; +} + +/* 'default' enter sequence for state not_pressed */ +static void enseq_main_region_on_r2_not_pressed_default(Statechart* handle) +{ + /* 'default' enter sequence for state not_pressed */ + handle->stateConfVector[1] = Statechart_main_region_on_r2_not_pressed; + handle->stateConfVectorPosition = 1; +} + +/* 'default' enter sequence for state pressed */ +static void enseq_main_region_on_r2_pressed_default(Statechart* handle) +{ + /* 'default' enter sequence for state pressed */ + enact_main_region_on_r2_pressed(handle); + handle->stateConfVector[1] = Statechart_main_region_on_r2_pressed; + handle->stateConfVectorPosition = 1; +} + +/* 'default' enter sequence for state off */ +static void enseq_main_region_off_default(Statechart* handle) +{ + /* 'default' enter sequence for state off */ + handle->stateConfVector[0] = Statechart_main_region_off; + handle->stateConfVectorPosition = 0; +} + +/* 'default' enter sequence for region main region */ +static void enseq_main_region_default(Statechart* handle) +{ + /* 'default' enter sequence for region main region */ + react_main_region__entry_Default(handle); +} + +/* 'default' enter sequence for region r1 */ +static void enseq_main_region_on_r1_default(Statechart* handle) +{ + /* 'default' enter sequence for region r1 */ + react_main_region_on_r1__entry_Default(handle); +} + +/* 'default' enter sequence for region r2 */ +static void enseq_main_region_on_r2_default(Statechart* handle) +{ + /* 'default' enter sequence for region r2 */ + react_main_region_on_r2__entry_Default(handle); +} + +/* Default exit sequence for state on */ +static void exseq_main_region_on(Statechart* handle) +{ + /* Default exit sequence for state on */ + exseq_main_region_on_r1(handle); + exseq_main_region_on_r2(handle); + handle->stateConfVector[0] = Statechart_last_state; + handle->stateConfVector[1] = Statechart_last_state; + handle->stateConfVectorPosition = 1; +} + +/* Default exit sequence for state responding */ +static void exseq_main_region_on_r1_responding(Statechart* handle) +{ + /* Default exit sequence for state responding */ + handle->stateConfVector[0] = Statechart_main_region_on; + handle->stateConfVectorPosition = 0; +} + +/* Default exit sequence for state sensor_error */ +static void exseq_main_region_on_r1_sensor_error(Statechart* handle) +{ + /* Default exit sequence for state sensor_error */ + handle->stateConfVector[0] = Statechart_main_region_on; + handle->stateConfVectorPosition = 0; +} + +/* Default exit sequence for state not_pressed */ +static void exseq_main_region_on_r2_not_pressed(Statechart* handle) +{ + /* Default exit sequence for state not_pressed */ + handle->stateConfVector[1] = Statechart_main_region_on; + handle->stateConfVectorPosition = 1; +} + +/* Default exit sequence for state pressed */ +static void exseq_main_region_on_r2_pressed(Statechart* handle) +{ + /* Default exit sequence for state pressed */ + handle->stateConfVector[1] = Statechart_main_region_on; + handle->stateConfVectorPosition = 1; + exact_main_region_on_r2_pressed(handle); +} + +/* Default exit sequence for state off */ +static void exseq_main_region_off(Statechart* handle) +{ + /* Default exit sequence for state off */ + handle->stateConfVector[0] = Statechart_last_state; + handle->stateConfVectorPosition = 0; +} + +/* Default exit sequence for region main region */ +static void exseq_main_region(Statechart* handle) +{ + /* Default exit sequence for region main region */ + /* Handle exit of all possible states (of Statechart.main_region) at position 0... */ + switch(handle->stateConfVector[ 0 ]) + { + case Statechart_main_region_on_r1_responding : + { + exseq_main_region_on_r1_responding(handle); + break; + } + case Statechart_main_region_on_r1_sensor_error : + { + exseq_main_region_on_r1_sensor_error(handle); + break; + } + case Statechart_main_region_off : + { + exseq_main_region_off(handle); + break; + } + default: + /* do nothing */ + break; + } + /* Handle exit of all possible states (of Statechart.main_region) at position 1... */ + switch(handle->stateConfVector[ 1 ]) + { + case Statechart_main_region_on_r2_not_pressed : + { + exseq_main_region_on_r2_not_pressed(handle); + break; + } + case Statechart_main_region_on_r2_pressed : + { + exseq_main_region_on_r2_pressed(handle); + break; + } + default: + /* do nothing */ + break; + } +} + +/* Default exit sequence for region r1 */ +static void exseq_main_region_on_r1(Statechart* handle) +{ + /* Default exit sequence for region r1 */ + /* Handle exit of all possible states (of Statechart.main_region.on.r1) at position 0... */ + switch(handle->stateConfVector[ 0 ]) + { + case Statechart_main_region_on_r1_responding : + { + exseq_main_region_on_r1_responding(handle); + break; + } + case Statechart_main_region_on_r1_sensor_error : + { + exseq_main_region_on_r1_sensor_error(handle); + break; + } + default: + /* do nothing */ + break; + } +} + +/* Default exit sequence for region r2 */ +static void exseq_main_region_on_r2(Statechart* handle) +{ + /* Default exit sequence for region r2 */ + /* Handle exit of all possible states (of Statechart.main_region.on.r2) at position 1... */ + switch(handle->stateConfVector[ 1 ]) + { + case Statechart_main_region_on_r2_not_pressed : + { + exseq_main_region_on_r2_not_pressed(handle); + break; + } + case Statechart_main_region_on_r2_pressed : + { + exseq_main_region_on_r2_pressed(handle); + break; + } + default: + /* do nothing */ + break; + } +} + +/* The reactions of state null. */ +static void react_main_region_on_r1__choice_0(Statechart* handle) +{ + /* The reactions of state null. */ + if ((handle->iface.sensor_value) < (0)) + { + enseq_main_region_on_r1_sensor_error_default(handle); + } else + { + enseq_main_region_on_r1_responding_default(handle); + } +} + +/* Default react sequence for initial entry */ +static void react_main_region_on_r1__entry_Default(Statechart* handle) +{ + /* Default react sequence for initial entry */ + enseq_main_region_on_r1_responding_default(handle); +} + +/* Default react sequence for initial entry */ +static void react_main_region_on_r2__entry_Default(Statechart* handle) +{ + /* Default react sequence for initial entry */ + enseq_main_region_on_r2_not_pressed_default(handle); +} + +/* Default react sequence for initial entry */ +static void react_main_region__entry_Default(Statechart* handle) +{ + /* Default react sequence for initial entry */ + enseq_main_region_off_default(handle); +} + + +static sc_integer main_region_on_r1_responding_react(Statechart* handle, const sc_integer transitioned_before) +{ + /* The reactions of state responding. */ + sc_integer transitioned_after = transitioned_before; + if ((transitioned_after) < (0)) + { + if (handle->iface.sensor_raised == bool_true) + { + exseq_main_region_on_r1_responding(handle); + react_main_region_on_r1__choice_0(handle); + transitioned_after = 0; + } + } + return transitioned_after; +} + +static sc_integer main_region_on_r1_sensor_error_react(Statechart* handle, const sc_integer transitioned_before) +{ + /* The reactions of state sensor_error. */ + sc_integer transitioned_after = transitioned_before; + if ((transitioned_after) < (0)) + { + if (handle->iface.sensor_raised == bool_true) + { + exseq_main_region_on_r1_sensor_error(handle); + react_main_region_on_r1__choice_0(handle); + transitioned_after = 0; + } + } + return transitioned_after; +} + +static sc_integer main_region_on_r2_not_pressed_react(Statechart* handle, const sc_integer transitioned_before) +{ + /* The reactions of state not_pressed. */ + sc_integer transitioned_after = transitioned_before; + if ((transitioned_after) < (1)) + { + if (handle->iface.buttonPressed_raised == bool_true) + { + exseq_main_region_on_r2_not_pressed(handle); + enseq_main_region_on_r2_pressed_default(handle); + transitioned_after = 1; + } + } + /* If no transition was taken */ + if ((transitioned_after) == (transitioned_before)) + { + /* then execute local reactions. */ + transitioned_after = transitioned_before; + } + return transitioned_after; +} + +static sc_integer main_region_on_r2_pressed_react(Statechart* handle, const sc_integer transitioned_before) +{ + /* The reactions of state pressed. */ + sc_integer transitioned_after = transitioned_before; + if ((transitioned_after) < (1)) + { + if (handle->timeEvents.statechart_main_region_on_r2_pressed_tev0_raised == bool_true) + { + exseq_main_region_on(handle); + handle->timeEvents.statechart_main_region_on_r2_pressed_tev0_raised = bool_false; + enseq_main_region_off_default(handle); + transitioned_after = 1; + } else + { + if (handle->iface.buttonReleased_raised == bool_true) + { + exseq_main_region_on_r2_pressed(handle); + enseq_main_region_on_r2_not_pressed_default(handle); + transitioned_after = 1; + } + } + } + /* If no transition was taken */ + if ((transitioned_after) == (transitioned_before)) + { + /* then execute local reactions. */ + transitioned_after = transitioned_before; + } + return transitioned_after; +} + +static sc_integer main_region_off_react(Statechart* handle, const sc_integer transitioned_before) +{ + /* The reactions of state off. */ + sc_integer transitioned_after = transitioned_before; + if ((transitioned_after) < (0)) + { + if (handle->iface.buttonPressed_raised == bool_true) + { + exseq_main_region_off(handle); + enseq_main_region_on_default(handle); + transitioned_after = 0; + } + } + /* If no transition was taken */ + if ((transitioned_after) == (transitioned_before)) + { + /* then execute local reactions. */ + transitioned_after = transitioned_before; + } + return transitioned_after; +} + + + + +static void statechart_eventqueue_init(statechart_eventqueue * eq, statechart_event *buffer, sc_integer capacity) +{ + eq->events = buffer; + eq->capacity = capacity; + eq->push_index = 0; + eq->pop_index = 0; + eq->size = 0; +} + +static sc_integer statechart_eventqueue_size(statechart_eventqueue * eq) +{ + return eq->size; +} + +static statechart_event statechart_eventqueue_pop(statechart_eventqueue * eq) +{ + statechart_event event; + if(statechart_eventqueue_size(eq) <= 0) { + statechart_event_init(&event, Statechart_invalid_event); + } + else { + event = eq->events[eq->pop_index]; + + if(eq->pop_index < eq->capacity - 1) { + eq->pop_index++; + } + else { + eq->pop_index = 0; + } + eq->size--; + } + return event; +} +static sc_boolean statechart_eventqueue_push(statechart_eventqueue * eq, statechart_event ev) +{ + if(statechart_eventqueue_size(eq) == eq->capacity) { + return bool_false; + } + else { + eq->events[eq->push_index] = ev; + + if(eq->push_index < eq->capacity - 1) { + eq->push_index++; + } + else { + eq->push_index = 0; + } + eq->size++; + + return bool_true; + } +} +static void statechart_event_init(statechart_event * ev, StatechartEventID name) +{ + ev->name = name; + ev->has_value = bool_false; +} + +static void statechart_add_event_to_queue(statechart_eventqueue * eq, StatechartEventID name) +{ + statechart_event event; + statechart_event_init(&event, name); + statechart_eventqueue_push(eq, event); +} + +static sc_boolean statechart_dispatch_event(Statechart* handle, const statechart_event * event) { + switch(event->name) { + case Statechart_sensor: + { + handle->iface.sensor_raised = bool_true; + handle->iface.sensor_value = event->value.Statechart_sensor_value; + return bool_true; + } + case Statechart_buttonPressed: + { + handle->iface.buttonPressed_raised = bool_true; + return bool_true; + } + case Statechart_buttonReleased: + { + handle->iface.buttonReleased_raised = bool_true; + return bool_true; + } + case Statechart_Statechart_main_region_on_r2_pressed_time_event_0: + { + handle->timeEvents.statechart_main_region_on_r2_pressed_tev0_raised = bool_true; + return bool_true; + } + default: + return bool_false; + } +} + +static statechart_event statechart_get_next_event(Statechart* handle) +{ + statechart_event next_event; + statechart_event_init(&next_event, Statechart_invalid_event); + if(statechart_eventqueue_size(&(handle->in_event_queue)) > 0) { + next_event = statechart_eventqueue_pop(&(handle->in_event_queue)); + } + return next_event; +} + +static sc_boolean statechart_dispatch_next_event(Statechart* handle) +{ + statechart_event nextEvent; + nextEvent = statechart_get_next_event(handle); + return statechart_dispatch_event(handle, &nextEvent); +} + +static StatechartEventID statechart_get_timed_event_name(Statechart* handle, sc_eventid evid) +{ + if(evid == &handle->timeEvents.statechart_main_region_on_r2_pressed_tev0_raised) { + return Statechart_Statechart_main_region_on_r2_pressed_time_event_0; + } + return Statechart_invalid_event; +} + + +static void statechart_event_value_init(statechart_event * ev, StatechartEventID name, void * value) +{ + ev->name = name; + ev->has_value = bool_true; + + switch(name) + { + case Statechart_sensor: + ev->value.Statechart_sensor_value = *((sc_real*)value); + break; + default: + /* do nothing */ + break; + } +} + +static void statechart_add_value_event_to_queue(statechart_eventqueue * eq, StatechartEventID name, void * value) +{ + statechart_event event; + statechart_event_value_init(&event, name, value); + statechart_eventqueue_push(eq, event); +} diff --git a/robot_cdt/src-gen/Statechart.h b/robot_cdt/src-gen/Statechart.h new file mode 100644 index 0000000..477a930 --- /dev/null +++ b/robot_cdt/src-gen/Statechart.h @@ -0,0 +1,227 @@ +/* Generated by itemis CREATE code generator. */ + +#ifndef STATECHART_H_ +#define STATECHART_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/*! +* Forward declaration for the Statechart state machine. +*/ +typedef struct Statechart Statechart; + +/*! +* Forward declaration of the data structure for the StatechartIface interface scope. +*/ +typedef struct StatechartIface StatechartIface; + +/*! +* Forward declaration of the data structure for the StatechartInternal interface scope. +*/ +typedef struct StatechartInternal StatechartInternal; + +/*! +* Forward declaration of the data structure for the StatechartTimeEvents interface scope. +*/ +typedef struct StatechartTimeEvents StatechartTimeEvents; + +#ifdef __cplusplus +} +#endif + +#include "../src/data_types.h" +#include "../src/sc_types.h" +#include "../src/sc_rxc.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/*! \file +Header of the state machine 'Statechart'. +*/ + +#ifndef STATECHART_EVENTQUEUE_BUFFERSIZE +#define STATECHART_EVENTQUEUE_BUFFERSIZE 20 +#endif +#ifndef STATECHART_IN_EVENTQUEUE_BUFFERSIZE +#define STATECHART_IN_EVENTQUEUE_BUFFERSIZE STATECHART_EVENTQUEUE_BUFFERSIZE +#endif +#ifndef SC_INVALID_EVENT_VALUE +#define SC_INVALID_EVENT_VALUE 0 +#endif +/*! Define number of states in the state enum */ +#define STATECHART_STATE_COUNT 6 + +/*! Define dimension of the state configuration vector for orthogonal states. */ +#define STATECHART_MAX_ORTHOGONAL_STATES 2 +/*! Define maximum number of time events that can be active at once */ +#define STATECHART_MAX_PARALLEL_TIME_EVENTS 1 + +/*! Define indices of states in the StateConfVector */ +#define SCVI_STATECHART_MAIN_REGION_ON 0 +#define SCVI_STATECHART_MAIN_REGION_ON_R1_RESPONDING 0 +#define SCVI_STATECHART_MAIN_REGION_ON_R1_SENSOR_ERROR 0 +#define SCVI_STATECHART_MAIN_REGION_ON_R2_NOT_PRESSED 1 +#define SCVI_STATECHART_MAIN_REGION_ON_R2_PRESSED 1 +#define SCVI_STATECHART_MAIN_REGION_OFF 0 + +/* + * Union of all possible event value types. + */ +typedef union { + sc_real Statechart_sensor_value; +} statechart_event_value; + +/* + * Enum of event names in the statechart. + */ +typedef enum { + Statechart_invalid_event = SC_INVALID_EVENT_VALUE, + Statechart_sensor, + Statechart_buttonPressed, + Statechart_buttonReleased, + Statechart_Statechart_main_region_on_r2_pressed_time_event_0 +} StatechartEventID; + +/* + * Struct that represents a single event. + */ +typedef struct { + StatechartEventID name; + sc_boolean has_value; + statechart_event_value value; +} statechart_event; + +/* + * Queue that holds the raised events. + */ +typedef struct statechart_eventqueue_s { + statechart_event *events; + sc_integer capacity; + sc_integer pop_index; + sc_integer push_index; + sc_integer size; +} statechart_eventqueue; + +/*! Enumeration of all states */ +typedef enum +{ + Statechart_last_state, + Statechart_main_region_on, + Statechart_main_region_on_r1_responding, + Statechart_main_region_on_r1_sensor_error, + Statechart_main_region_on_r2_not_pressed, + Statechart_main_region_on_r2_pressed, + Statechart_main_region_off +} StatechartStates; + + + + +/*! Type declaration of the data structure for the StatechartIface interface scope. */ +struct StatechartIface +{ + sc_boolean sensor_raised; + sc_real sensor_value; + sc_boolean buttonPressed_raised; + sc_boolean buttonReleased_raised; + sc_observable_sc_real setMotorR; + sc_observable_sc_real setMotorL; +}; + + + +/*! Type declaration of the data structure for the StatechartInternal interface scope. */ +struct StatechartInternal +{ + sc_real speed; + pid_vars_t pid_vars; +}; + + + +/*! Type declaration of the data structure for the StatechartTimeEvents interface scope. */ +struct StatechartTimeEvents +{ + sc_boolean statechart_main_region_on_r2_pressed_tev0_raised; +}; + + + + + + +/*! + * Type declaration of the data structure for the Statechart state machine. + * This data structure has to be allocated by the client code. + */ +struct Statechart +{ + StatechartStates stateConfVector[STATECHART_MAX_ORTHOGONAL_STATES]; + StatechartIface iface; + StatechartInternal internal; + StatechartTimeEvents timeEvents; + sc_boolean isExecuting; + sc_integer stateConfVectorPosition; + statechart_eventqueue in_event_queue; + statechart_event in_buffer[STATECHART_IN_EVENTQUEUE_BUFFERSIZE]; +}; + + + +/*! Initializes the Statechart state machine data structures. Must be called before first usage.*/ +extern void statechart_init(Statechart* handle); + + +/*! Activates the state machine. */ +extern void statechart_enter(Statechart* handle); + +/*! Deactivates the state machine. */ +extern void statechart_exit(Statechart* handle); + +/*! +Can be used by the client code to trigger a run to completion step without raising an event. +*/ +extern void statechart_trigger_without_event(Statechart* handle); + + +/*! Raises a time event. */ +extern void statechart_raise_time_event(Statechart* handle, sc_eventid evid); + +/*! Raises the in event 'sensor' that is defined in the default interface scope. */ +extern void statechart_raise_sensor(Statechart* handle, sc_real value); +/*! Raises the in event 'buttonPressed' that is defined in the default interface scope. */ +extern void statechart_raise_buttonPressed(Statechart* handle); +/*! Raises the in event 'buttonReleased' that is defined in the default interface scope. */ +extern void statechart_raise_buttonReleased(Statechart* handle); +/*! Returns the observable for the out event 'setMotorR' that is defined in the default interface scope. */ +extern sc_observable_sc_real* statechart_get_setMotorR(Statechart* handle); + +/*! Returns the observable for the out event 'setMotorL' that is defined in the default interface scope. */ +extern sc_observable_sc_real* statechart_get_setMotorL(Statechart* handle); + + +/*! + * Checks whether the state machine is active (until 2.4.1 this method was used for states). + * A state machine is active if it was entered. It is inactive if it has not been entered at all or if it has been exited. + */ +extern sc_boolean statechart_is_active(const Statechart* handle); + +/*! + * Checks if all active states are final. + * If there are no active states then the state machine is considered being inactive. In this case this method returns false. + */ +extern sc_boolean statechart_is_final(const Statechart* handle); + +/*! Checks if the specified state is active (until 2.4.1 the used method for states was called isActive()). */ +extern sc_boolean statechart_is_state_active(const Statechart* handle, StatechartStates state); + + +#ifdef __cplusplus +} +#endif + +#endif /* STATECHART_H_ */ diff --git a/robot_cdt/src-gen/Statechart_required.h b/robot_cdt/src-gen/Statechart_required.h new file mode 100644 index 0000000..0fe22a6 --- /dev/null +++ b/robot_cdt/src-gen/Statechart_required.h @@ -0,0 +1,56 @@ +/* Generated by itemis CREATE code generator. */ + +#ifndef STATECHART_REQUIRED_H_ +#define STATECHART_REQUIRED_H_ + +#include "../src/sc_types.h" +#include "Statechart.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/*! \file +This header defines prototypes for all functions that are required by the state machine implementation. + +This is a state machine uses time events which require access to a timing service. Thus the function prototypes: +- statechart_set_timer and +- statechart_unset_timer +are defined. +This state machine makes use of operations declared in the state machines interface or internal scopes. Thus the function prototypes: +- statechart_pid +are defined. + +These functions will be called during a 'run to completion step' (runCycle) of the statechart. +There are some constraints that have to be considered for the implementation of these functions: +- make sure that the execution time is as short as possible. +*/ + +extern sc_real statechart_pid( Statechart* handle, const sc_real sensor, const pid_vars_t pid_vars); + + + +/*! +This function has to set up timers for the time events that are required by the state machine. +It will be called for each time event that is relevant for a state when a state will be entered. +\param evid An unique identifier of the event. +\time_ms The time in milliseconds +\periodic Indicates the the time event must be raised periodically until the timer is unset +*/ +extern void statechart_set_timer(Statechart* handle, const sc_eventid evid, const sc_time time_ms, const sc_boolean periodic); + +/*! +This function has to unset timers for the time events that are required by the state machine. +It will be called for each time event that is relevant for a state when a state will be left. +\param evid An unique identifier of the event. +*/ +extern void statechart_unset_timer(Statechart* handle, const sc_eventid evid); + + + +#ifdef __cplusplus +} +#endif + +#endif /* STATECHART_REQUIRED_H_ */ diff --git a/robot_cdt/src/controller.c b/robot_cdt/src/controller.c new file mode 100644 index 0000000..356081f --- /dev/null +++ b/robot_cdt/src/controller.c @@ -0,0 +1,67 @@ +#include +#include +#include +#include "../src/sc_timer_service.h" +#include "../src-gen/Statechart.h" +#include "../src-gen/Statechart_required.h" +#include "./data_types.h" +#include "./pid.h" + +#define TARGET_DIST 100 + +// YAKINDU forces us to use global variables so don't blame me +#define MAX_TIMERS 4 +static sc_timer_t timers[MAX_TIMERS]; +static sc_timer_service_t timer_service; + +// event handler callback functions +void on_motorL(Statechart* sc, sc_real value) { + fprintf(stderr, "got event motorL: %f\n", value); +} +void on_motorR(Statechart* sc, sc_real value) { + fprintf(stderr, "got event motorR: %f\n", value); +} + +// called once, on startup +void controller_init( + Statechart* sc, + sc_single_subscription_observer_sc_real* observer_motorL, + sc_single_subscription_observer_sc_real* observer_motorR +) { + sc_timer_service_init(&timer_service, timers, MAX_TIMERS, + (sc_raise_time_event_fp) &statechart_raise_time_event); + + statechart_init(sc); + statechart_enter(sc); + + // subscribe to output events: + + // motorL observer + sc_single_subscription_observer_sc_real_init(observer_motorL, sc, (sc_observer_next_sc_real_fp) on_motorL); + sc_single_subscription_observer_sc_real_subscribe(observer_motorL, statechart_get_setMotorL(sc)); + + // motorR observer + sc_single_subscription_observer_sc_real_init(observer_motorR, sc, (sc_observer_next_sc_real_fp) on_motorR); + sc_single_subscription_observer_sc_real_subscribe(observer_motorR, statechart_get_setMotorR(sc)); +} + +// to be called every 10 ms +void controller_poll() { + sc_time elapsed = 10; // ms + sc_timer_service_proceed(&timer_service, elapsed); + fprintf(stderr, "made 10ms step\n"); +} + +// finally, we need to implemented these functions (defined in "Statechart_required.h") +sc_real statechart_pid(Statechart* handle, const sc_real sensor, pid_vars_t pid_vars) { + sc_real error = TARGET_DIST - sensor; // is this correct? + return pid(pid_vars, error); +} +// here you see the reason we need global variables for the timer service: +// the signature of these functions is mandated by YAKINDU, and there is no 'timer service' parameter +void statechart_set_timer(Statechart *sc, const sc_eventid evid, const sc_integer time_ms, const sc_boolean periodic) { + sc_timer_set(&timer_service, sc, evid, time_ms, periodic); +} +void statechart_unset_timer(Statechart *sc, const sc_eventid evid) { + sc_timer_unset(&timer_service, evid); +} diff --git a/robot_cdt/src/data_types.h b/robot_cdt/src/data_types.h new file mode 100644 index 0000000..ba4093e --- /dev/null +++ b/robot_cdt/src/data_types.h @@ -0,0 +1,31 @@ +#ifndef DATA_TYPES_H +#define DATA_TYPES_H + +/**#include "nxt_motors.h" +#include "ecrobot_interface.h"**/ +#include +#include + +typedef struct cc_log { + uint32_t system_ticks; + int error; + float value; +} log_t; + +typedef struct { + /* PID controller parameters */ + float Kp; // 4 + float Ki; // 8 + float Kd; // 12 + + /* max output limits for the PID controller */ + float output_max; // 16 + float output_min; // 20 + + /* below are session variables for the PID controller */ + float _integral_sum; // 24 + float _prev_err; // 28 + float _dt; // 32 +} pid_vars_t; + +#endif diff --git a/robot_cdt/src/pid.c b/robot_cdt/src/pid.c new file mode 100644 index 0000000..d71ac98 --- /dev/null +++ b/robot_cdt/src/pid.c @@ -0,0 +1,23 @@ +#include "data_types.h" + +double pid(pid_vars_t vars, double current_err) { + /* current_error = setpoint - current_process_variable */ + + vars._integral_sum += current_err*(vars._dt); + + double output = (vars.Kp)*current_err \ + + (vars.Ki)*(vars._integral_sum) \ + + (vars.Kd)*((current_err - (vars._prev_err))\ + /(vars._dt)); + + vars._prev_err = current_err; + + /* limit output within output_min and output_max */ + if (output>(vars.output_max)) { + output = vars.output_max; + } + else if (output<(vars.output_min)) { + output = vars.output_min; + } + return output; +} diff --git a/robot_cdt/src/pid.h b/robot_cdt/src/pid.h new file mode 100644 index 0000000..33a31d6 --- /dev/null +++ b/robot_cdt/src/pid.h @@ -0,0 +1,8 @@ +#ifndef PID_H +#define PID_H + +#include "data_types.h" + +double pid(pid_vars_t vars, double current_err); + +#endif \ No newline at end of file diff --git a/robot_cdt/src/sc_rxc.c b/robot_cdt/src/sc_rxc.c new file mode 100644 index 0000000..f374f44 --- /dev/null +++ b/robot_cdt/src/sc_rxc.c @@ -0,0 +1,172 @@ +/** Generated by itemis CREATE code generator. */ + +#include "sc_rxc.h" + + +void sc_observer_init(sc_observer *self, sc_object_ref o, sc_observer_next_fp nf) +{ + self->object = o; + self->next = (sc_observer_next_fp) nf; +} + +void sc_observer_next(sc_observer *self) +{ + if (self != sc_null && self->next != sc_null) + { + self->next(self->object); + } +} + + +void sc_subscription_init(sc_subscription *self, sc_observer *o) +{ + self->observer = o; + self->next = sc_null; +} + + +void sc_observable_init(sc_observable *self) +{ + self->subscriptions = sc_null; +} + + +sc_boolean sc_observable_subscribe(sc_observable *self, sc_subscription *s) +{ + sc_subscription *currentSub; + if (s != sc_null && s->observer != sc_null && s->next == sc_null) { + currentSub = self->subscriptions; + s->next = (currentSub != sc_null) ? currentSub : s; + self->subscriptions = s; + return bool_true; + } + return bool_false; +} + +sc_boolean sc_observable_unsubscribe(sc_observable *self, sc_subscription *s) +{ + sc_subscription *sub; + + if (s != sc_null && self->subscriptions != sc_null) + { + if (self->subscriptions == s) { + self->subscriptions = (s->next != s) ? s->next : sc_null; + s->next = sc_null; + + return bool_true; + } + + sub = self->subscriptions; + while ( sub != sc_null ) + { + if ( sub->next != sub && sub->next == s) + { + sub->next = (s->next != s) ? s->next : sub; + return bool_true; + } + + sub = (sub->next != sub) ? sub->next : sc_null; + } + } + return bool_false; +} + + +void sc_observable_next(sc_observable *self) +{ + sc_subscription *sub = self->subscriptions; + while (sub != sc_null) + { + if (sub->observer != sc_null) + { + sc_observer_next(sub->observer); + } + sub = (sub->next != sub) ? sub->next : sc_null; + } +} + + +void sc_single_subscription_observer_init(sc_single_subscription_observer *self, sc_object_ref o, sc_observer_next_fp nf) +{ + sc_observer_init(&(self->observer), o, nf); + sc_subscription_init(&(self->subscription), &(self->observer)); +} + + +sc_boolean sc_single_subscription_observer_subscribe(sc_single_subscription_observer *self, sc_observable *o) { + return sc_observable_subscribe(o, &(self->subscription)); +} + +sc_boolean sc_single_subscription_observer_unsubscribe(sc_single_subscription_observer *self, sc_observable *o) { + return sc_observable_unsubscribe(o, &(self->subscription)); +} + + +/* ----------------------------------------------------------- + * declaration of reactive extensions for all Y-SCT default types + */ + +define_sc_reactive_extensions(sc_boolean) +define_sc_reactive_extensions(sc_integer) +define_sc_reactive_extensions(sc_real) + +/* declaration of declare_sc_reactive_extensions(sc_string) */ +void sc_observer_sc_string_init(sc_observer_sc_string *self, sc_object_ref o, sc_observer_next_sc_string_fp nf) +{ + self->object = o; + self->next = nf; +} + +void sc_observer_sc_string_next(sc_observer_sc_string *self, sc_string value) +{ + if (self != sc_null && self->next != sc_null) + { + self->next(self->object, value); + } +} + +void sc_observable_sc_string_init(sc_observable_sc_string *self) +{ + self->subscriptions = sc_null; +} + +sc_boolean sc_observable_sc_string_subscribe(sc_observable_sc_string *self, sc_subscription_sc_string *s) { + return sc_observable_subscribe((sc_observable*) self, (sc_subscription *) s); +} + +sc_boolean sc_observable_sc_string_unsubscribe(sc_observable_sc_string *self, sc_subscription_sc_string *s) { + return sc_observable_unsubscribe((sc_observable*) self, (sc_subscription *) s); +} + +void sc_observable_sc_string_next(sc_observable_sc_string *self, sc_string value) +{ + sc_subscription_sc_string *sub = self->subscriptions; + while (sub != sc_null) + { + if (sub->observer != sc_null) + { + sc_observer_sc_string_next(sub->observer, value); + } + sub = (sub->next != sub) ? sub->next : sc_null; + } +} + +void sc_single_subscription_observer_sc_string_init(sc_single_subscription_observer_sc_string *self, sc_object_ref o, sc_observer_next_sc_string_fp nf) +{ + sc_observer_sc_string_init(&(self->observer), o, nf); + sc_subscription_sc_string_init(&(self->subscription), &(self->observer)); +} + +sc_boolean sc_single_subscription_observer_sc_string_subscribe(sc_single_subscription_observer_sc_string *self, sc_observable_sc_string *o) { + return sc_observable_subscribe((sc_observable *) o, (sc_subscription *) &(self->subscription)); +} + +sc_boolean sc_single_subscription_observer_sc_string_unsubscribe(sc_single_subscription_observer_sc_string *self, sc_observable_sc_string *o) { + return sc_observable_unsubscribe((sc_observable *)o, (sc_subscription *) &(self->subscription)); +} + +void sc_subscription_sc_string_init(sc_subscription_sc_string *self, sc_observer_sc_string *o) +{ + self->observer = o; + self->next = sc_null; +} diff --git a/robot_cdt/src/sc_rxc.h b/robot_cdt/src/sc_rxc.h new file mode 100644 index 0000000..e02cd09 --- /dev/null +++ b/robot_cdt/src/sc_rxc.h @@ -0,0 +1,260 @@ +/** Generated by itemis CREATE code generator. */ + +#ifndef SC_RXC_H_ +#define SC_RXC_H_ + +#include "sc_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/*! A reference to generic, untyped object. */ +typedef void* sc_object_ref; + +typedef struct sc_observer sc_observer; +typedef struct sc_subscription sc_subscription; +typedef struct sc_observable sc_observable; +typedef struct sc_single_subscription_observer sc_single_subscription_observer; + + +/* ----------------------------------------------------------- + * observers + */ + +typedef void (*sc_observer_next_fp)(sc_object_ref); +struct sc_observer { + sc_object_ref object; + sc_observer_next_fp next; +}; + +extern void sc_observer_init(sc_observer *self, sc_object_ref o, sc_observer_next_fp nf); +extern void sc_observer_next(sc_observer *self); + + +#define declare_sc_observer(T) \ + typedef void (*sc_observer_next_##T##_fp)(sc_object_ref, T); \ + typedef struct sc_observer_##T sc_observer_##T; \ + struct sc_observer_##T { \ + sc_object_ref object; \ + sc_observer_next_##T##_fp next; \ + }; \ + extern void sc_observer_##T##_init(sc_observer_##T *self, sc_object_ref o, sc_observer_next_##T##_fp nf); \ + extern void sc_observer_##T##_next(sc_observer_##T *self, T value); + + +#define define_sc_observer(T) \ + void sc_observer_##T##_init(sc_observer_##T *self, sc_object_ref o, sc_observer_next_##T##_fp nf) \ + { \ + self->object = o; \ + self->next = nf; \ + } \ + \ + void sc_observer_##T##_next(sc_observer_##T *self, T value) \ + { \ + if (self != sc_null && self->next != sc_null) \ + { \ + self->next(self->object, value); \ + } \ + } \ + \ + + +/* ----------------------------------------------------------- + * subscriptions + */ + +struct sc_subscription { + sc_observer *observer; + sc_subscription* next; +}; +void sc_subscription_init(sc_subscription *self, sc_observer *o); + +#define declare_sc_subscription(T) \ + typedef struct sc_subscription_##T sc_subscription_##T; \ + struct sc_subscription_##T { \ + sc_observer_##T *observer; \ + sc_subscription_##T *next; \ + }; \ + extern void sc_subscription_##T##_init(sc_subscription_##T *self, sc_observer_##T *o); \ + + +#define define_sc_subscription(T) \ + void sc_subscription_##T##_init(sc_subscription_##T *self, sc_observer_##T *o) \ + { \ + self->observer = o; \ + self->next = sc_null; \ + } \ + + + +/* ----------------------------------------------------------- + * observables + */ + +struct sc_observable { + sc_uinteger observer_count; + sc_subscription *subscriptions; +}; + +extern void sc_observable_init(sc_observable *self); +extern sc_boolean sc_observable_subscribe(sc_observable *self, sc_subscription *s); +extern sc_boolean sc_observable_unsubscribe(sc_observable *self, sc_subscription *s); +extern void sc_observable_next(sc_observable *self); + +#define sc_observable(T) sc_observable_##T + +#define declare_sc_observable(T) \ + typedef struct sc_observable_##T sc_observable_##T; \ + struct sc_observable_##T { \ + sc_uinteger observer_count; \ + sc_subscription_##T *subscriptions; \ + }; \ + extern void sc_observable_##T##_init(sc_observable_##T *self); \ + extern sc_boolean sc_observable_##T##_subscribe(sc_observable_##T *self, sc_subscription_##T *s); \ + extern sc_boolean sc_observable_##T##_unsubscribe(sc_observable_##T *self, sc_subscription_##T *s); \ + extern void sc_observable_##T##_next(sc_observable_##T *self, T value); \ + + +#define define_sc_observable(T) \ + void sc_observable_##T##_init(sc_observable_##T *self) \ + { \ + self->subscriptions = sc_null; \ + } \ + \ + sc_boolean sc_observable_##T##_subscribe(sc_observable_##T *self, sc_subscription_##T *s) {\ + return sc_observable_subscribe((sc_observable*) self, (sc_subscription *) s); \ + } \ + \ + sc_boolean sc_observable_##T##_unsubscribe(sc_observable_##T *self, sc_subscription_##T *s) {\ + return sc_observable_unsubscribe((sc_observable*) self, (sc_subscription *) s); \ + } \ + \ + void sc_observable_##T##_next(sc_observable_##T *self, T value) \ + { \ + sc_subscription_##T *sub = self->subscriptions; \ + while (sub != sc_null) \ + { \ + if (sub->observer != sc_null) \ + { \ + sc_observer_##T##_next(sub->observer, value); \ + } \ + sub = (sub->next != sub) ? sub->next : sc_null; \ + } \ + }\ + + + +/* ----------------------------------------------------------- + * single subscription observer + */ + +struct sc_single_subscription_observer { + sc_observer observer; + sc_subscription subscription; +}; + +extern void sc_single_subscription_observer_init(sc_single_subscription_observer *self, sc_object_ref o, sc_observer_next_fp nf); +extern sc_boolean sc_single_subscription_observer_subscribe(sc_single_subscription_observer *self, sc_observable *o); +extern sc_boolean sc_single_subscription_observer_unsubscribe(sc_single_subscription_observer *self, sc_observable *o); + + +#define declare_sc_single_subscription_observer(T) \ + typedef struct sc_single_subscription_observer_##T sc_single_subscription_observer_##T; \ + struct sc_single_subscription_observer_##T { \ + sc_observer_##T observer; \ + sc_subscription_##T subscription; \ + }; \ + extern void sc_single_subscription_observer_##T##_init(sc_single_subscription_observer_##T *self, sc_object_ref o, sc_observer_next_##T##_fp nf); \ + extern sc_boolean sc_single_subscription_observer_##T##_subscribe(sc_single_subscription_observer_##T *self, sc_observable_##T *o); \ + extern sc_boolean sc_single_subscription_observer_##T##_unsubscribe(sc_single_subscription_observer_##T *self, sc_observable_##T *o); \ + + +#define define_sc_single_subscription_observer(T) \ + void sc_single_subscription_observer_##T##_init(sc_single_subscription_observer_##T *self, sc_object_ref o, sc_observer_next_##T##_fp nf) \ + { \ + sc_observer_##T##_init(&(self->observer), o, nf); \ + sc_subscription_##T##_init(&(self->subscription), &(self->observer)); \ + } \ + \ + sc_boolean sc_single_subscription_observer_##T##_subscribe(sc_single_subscription_observer_##T *self, sc_observable_##T *o) { \ + return sc_observable_subscribe((sc_observable *) o, (sc_subscription *) &(self->subscription)); \ + } \ + \ + sc_boolean sc_single_subscription_observer_##T##_unsubscribe(sc_single_subscription_observer_##T *self, sc_observable_##T *o) { \ + return sc_observable_unsubscribe((sc_observable *)o, (sc_subscription *) &(self->subscription)); \ + } \ + + + +/* ----------------------------------------------------------- + * macros to define reactive extensions for a specific type + */ +#define EMPTY + +#define declare_sc_reactive_extensions(T) \ + declare_sc_observer(T) \ + declare_sc_subscription(T) \ + declare_sc_observable(T) \ + declare_sc_single_subscription_observer(T) + + +#define define_sc_reactive_extensions(T) \ + define_sc_observer(T) \ + define_sc_subscription(T) \ + define_sc_observable(T) \ + define_sc_single_subscription_observer(T) + + +/* ----------------------------------------------------------- + * declaration of reactive extensions for all Y-SCT default types + */ + +declare_sc_reactive_extensions(sc_boolean) +declare_sc_reactive_extensions(sc_integer) +declare_sc_reactive_extensions(sc_real) + +/* declaration of declare_sc_reactive_extensions(sc_string */ +typedef void (*sc_observer_next_sc_string_fp)(sc_object_ref, sc_string); +typedef struct sc_observer_sc_string sc_observer_sc_string; +struct sc_observer_sc_string { + sc_object_ref object; + sc_observer_next_sc_string_fp next; +}; +extern void sc_observer_sc_string_init(sc_observer_sc_string *self, sc_object_ref o, sc_observer_next_sc_string_fp nf); +extern void sc_observer_sc_string_next(sc_observer_sc_string *self, sc_string value); + +typedef struct sc_subscription_sc_string sc_subscription_sc_string; +struct sc_subscription_sc_string { + sc_observer_sc_string *observer; + sc_subscription_sc_string *next; +}; +extern void sc_subscription_sc_string_init(sc_subscription_sc_string *self, sc_observer_sc_string *o); + +typedef struct sc_observable_sc_string sc_observable_sc_string; +struct sc_observable_sc_string { + sc_uinteger observer_count; + sc_subscription_sc_string *subscriptions; +}; +extern void sc_observable_sc_string_init(sc_observable_sc_string *self); +extern sc_boolean sc_observable_sc_string_subscribe(sc_observable_sc_string *self, sc_subscription_sc_string *s); +extern sc_boolean sc_observable_sc_string_unsubscribe(sc_observable_sc_string *self, sc_subscription_sc_string *s); +extern void sc_observable_sc_string_next(sc_observable_sc_string *self, sc_string value); + +typedef struct sc_single_subscription_observer_sc_string sc_single_subscription_observer_sc_string; +struct sc_single_subscription_observer_sc_string { + sc_observer_sc_string observer; + sc_subscription_sc_string subscription; +}; +extern void sc_single_subscription_observer_sc_string_init(sc_single_subscription_observer_sc_string *self, sc_object_ref o, sc_observer_next_sc_string_fp nf); +extern sc_boolean sc_single_subscription_observer_sc_string_subscribe(sc_single_subscription_observer_sc_string *self, sc_observable_sc_string *o); +extern sc_boolean sc_single_subscription_observer_sc_string_unsubscribe(sc_single_subscription_observer_sc_string *self, sc_observable_sc_string *o); + + + +#ifdef __cplusplus +} +#endif + + +#endif /* SC_RXC_H_ */ diff --git a/robot_cdt/src/sc_timer_service.c b/robot_cdt/src/sc_timer_service.c new file mode 100644 index 0000000..31cc7a6 --- /dev/null +++ b/robot_cdt/src/sc_timer_service.c @@ -0,0 +1,404 @@ +/* Generated by itemis CREATE code generator. */ + +#include "sc_timer_service.h" +#include + +void reset_task_data(task_data *data) +{ + switch (data->type) { + case TIME_EVENT_TASK: + data->get.time_ms = 0; + data->get.time_event.raise_event = sc_null; + data->get.time_event.pt_evid = 0; + data->get.time_event.periodic = bool_false; + break; + case RUNCYCLE_TASK: + data->get.time_ms = 0; + data->get.run_cycle = sc_null; + break; + default: + return; + } + data->type = EMPTY_TASK; + data->get.statemachine = NULL; +} + +void execute(task_data *data) +{ + switch (data->type) { + case TIME_EVENT_TASK: { + /* Fire the event. */ + sc_raise_time_event_fp raise_event = data->get.time_event.raise_event; + void *statemachine = data->get.statemachine; + if (raise_event != NULL && statemachine != NULL) { + raise_event(statemachine, data->get.time_event.pt_evid); + } + return; + } + case RUNCYCLE_TASK: { + sc_run_cycle_fp run_cycle = data->get.run_cycle; + void *statemachine = data->get.statemachine; + if (run_cycle != NULL) { + run_cycle(statemachine); + } + return; + } + default: + return; + } +} + +void update_elapsed_time_ms(sc_timer_t *task, sc_time elapsed_time_ms_) +{ + task->elapsed_time_ms += elapsed_time_ms_; +} + +sc_boolean is_periodic(sc_timer_t *task) +{ + task_type type; + type = task->data.type; + switch (type) { + case TIME_EVENT_TASK: + return task->data.get.time_event.periodic; + case RUNCYCLE_TASK: + return bool_true; + default: + return bool_false; + } +} + +sc_boolean is_runcycle_event(sc_timer_t *task) +{ + return task->data.type == RUNCYCLE_TASK; +} + +void reset_timer_task(sc_timer_t *task) +{ + reset_task_data(&task->data); + task->elapsed_time_ms = 0; +} + +/* + Compare tasks based on their execution order. + Return true if this task is always to be executed strictly before the other task if both are scheduled to run at the same time. + Default behavior: + - This task is to be scheduled strictly before the other task if its is_runcycle_event() method does not return true and the other task's is_runcycle_event() method returns true. + */ +sc_boolean less_than(sc_timer_t *task, sc_timer_t *other) +{ + return !is_runcycle_event(task) && is_runcycle_event(other); +} + +sc_boolean time_event_matcher(match_time_event *matcher, const sc_timer_t *other) +{ + return (matcher->statemachine == NULL || matcher->statemachine == other->data.get.statemachine) && matcher->pt_evid == other->data.get.time_event.pt_evid; +} + +sc_boolean run_cycle_matcher(match_run_cycle_of *matcher, const sc_timer_t *other) +{ + return matcher->statemachine == other->data.get.statemachine; +} + +void set_generic_timer(sc_timer_service_t *timer_service, const task_data data) +{ + sc_integer inserted_task_idx; + sc_timer_t *inserted_task; + + /* Do nothing if there are no free slots. */ + if (timer_service->next_free_task >= timer_service->length) { + return; + } + + /* Insert task at the front. */ + inserted_task_idx = timer_service->next_free_task; + inserted_task = &(timer_service->tasks[inserted_task_idx]); + inserted_task->data = data; + inserted_task->elapsed_time_ms = 0; + timer_service->next_free_task = inserted_task->next_task_idx; + inserted_task->next_task_idx = timer_service->next_active_task; + timer_service->next_active_task = inserted_task_idx; +} + +void unset_generic_timer(sc_timer_service_t *timer_service, timer_task_matcher *matcher) +{ + sc_integer last_position; + sc_integer next_position; + sc_timer_t *current_task; + sc_boolean match_result; + sc_integer current_position; + + last_position = timer_service->length; + next_position = timer_service->next_active_task; + + while (next_position < timer_service->length) { + + current_task = &timer_service->tasks[next_position]; + match_result = bool_false; + + if (matcher->is_match_time_event) { + match_result = time_event_matcher(&matcher->data.match_time_event_, current_task); + } else { + match_result = run_cycle_matcher(&matcher->data.match_run_cycle_of_, current_task); + } + + if (match_result) { + reset_timer_task(current_task); + if (last_position < timer_service->length) { + timer_service->tasks[last_position].next_task_idx = current_task->next_task_idx; + } else { + timer_service->next_active_task = current_task->next_task_idx; + } + + current_position = next_position; + next_position = current_task->next_task_idx; + current_task->next_task_idx = timer_service->next_free_task; + timer_service->next_free_task = current_position; + } else { + last_position = next_position; + next_position = current_task->next_task_idx; + } + } +} + +void sc_timer_service_init(sc_timer_service_t *timer_service, + sc_timer_t *tasks_, + sc_integer length_, + sc_raise_time_event_fp raise_event) +{ + sc_integer i; + + timer_service->length = length_; + timer_service->tasks = tasks_; + timer_service->next_active_task = length_; + timer_service->next_free_task = 0; + + for (i = 0; i < timer_service->length; i++) { + timer_service->tasks[i].next_task_idx = i + 1; + timer_service->tasks[i].elapsed_time_ms = 0; + timer_service->tasks[i].data.type = EMPTY_TASK; + } + + timer_service->raise_event = raise_event; +} + +/*! Start the timing for a time event.*/ +void sc_timer_set(sc_timer_service_t *timer_service, + void *handle, + const sc_eventid evid, + const sc_time time_ms, + const sc_boolean periodic) +{ + sc_raise_time_event_fp raise_event = timer_service->raise_event; + + sc_timer_set_for_raise_event(timer_service, raise_event, handle, evid, time_ms, periodic); +} + + +void sc_timer_set_for_raise_event(sc_timer_service_t *timer_service, + sc_raise_time_event_fp raise_event, + void *statemachine, + const sc_eventid evid, + const sc_time time_ms, + const sc_boolean periodic) +{ + task_data data; + data.type = TIME_EVENT_TASK; + data.get.time_ms = time_ms; + data.get.statemachine = statemachine; + data.get.run_cycle = NULL; + data.get.time_event.raise_event = raise_event; + data.get.time_event.pt_evid = evid; + data.get.time_event.periodic = periodic; + + set_generic_timer(timer_service, data); +} + +/*! Unset the given time event.*/ +void sc_timer_unset(sc_timer_service_t *timer_service, sc_eventid evid) +{ + void *statemachine; + statemachine = NULL; + sc_timer_unset_for_machine(timer_service, evid, statemachine); +} + +void sc_timer_unset_for_machine(sc_timer_service_t *timer_service, + sc_eventid evid, + void *statemachine) +{ + timer_task_matcher matcher; + matcher.is_match_time_event = bool_true; + matcher.data.match_time_event_.statemachine = statemachine; + matcher.data.match_time_event_.pt_evid = evid; + + unset_generic_timer(timer_service, &matcher); +} + +/*! Set a timer for running cycles of the given statemachine.*/ +void set_runcycle_timer_for(sc_timer_service_t *timer_service, + sc_run_cycle_fp run_cycle, + void *statemachine, + sc_time cycle_period) +{ + task_data data; + data.type = RUNCYCLE_TASK; + data.get.time_event.raise_event = NULL; + data.get.time_event.periodic = bool_false; + data.get.time_event.pt_evid = 0; + data.get.time_ms = cycle_period; + data.get.run_cycle = run_cycle; + data.get.statemachine = statemachine; + + set_generic_timer(timer_service, data); +} + +/*! Unset timers for running cycles of the given statemachine.*/ +void unset_runcycle_timer(sc_timer_service_t *timer_service, void *statemachine) +{ + timer_task_matcher matcher; + matcher.is_match_time_event = bool_false; + matcher.data.match_run_cycle_of_.statemachine = statemachine; + + unset_generic_timer(timer_service, &matcher); +} + +/*! + * This function must be called by the user. The elapsed time must be calculated every time, + * the function gets called. + */ +void sc_timer_service_proceed(sc_timer_service_t *timer_service, sc_time elapsed_ms) +{ + sc_time time_to_proceed; + sc_time proceeded_time; + sc_integer idx; + sc_boolean task_fired; + sc_integer before_best; + sc_integer best; + sc_time best_remaining_time; + sc_integer last_task; + sc_integer next_task; + sc_time remaining_time; + sc_time time_till_next_task_; + + if (timer_service->next_active_task >= timer_service->length) { + return; + } + + time_till_next_task_ = time_till_next_task(timer_service); + + time_to_proceed = time_till_next_task_ < elapsed_ms + ? time_till_next_task_ + : elapsed_ms; + proceeded_time = 0; + + while (time_to_proceed > 0) { + + idx = timer_service->next_active_task; + + while (idx < timer_service->length) { + sc_timer_t *current_task; + current_task = &timer_service->tasks[idx]; + update_elapsed_time_ms(current_task, time_to_proceed); + idx = current_task->next_task_idx; + } + + do { + task_fired = bool_false; + + + before_best = timer_service->length; + best = timer_service->next_active_task; + best_remaining_time = timer_service->tasks[best].data.get.time_ms + - timer_service->tasks[best].elapsed_time_ms; + last_task = best; + next_task = timer_service->tasks[best].next_task_idx; + while (next_task < timer_service->length) { + sc_timer_t current_task; + + + current_task = timer_service->tasks[next_task]; + remaining_time = current_task.data.get.time_ms + - current_task.elapsed_time_ms; + if (remaining_time < best_remaining_time + || (remaining_time == best_remaining_time + && !(less_than(&timer_service->tasks[best], ¤t_task)))) { + best = next_task; + before_best = last_task; + best_remaining_time = remaining_time; + } + last_task = next_task; + next_task = current_task.next_task_idx; + } + if (best_remaining_time <= 0) { + sc_timer_t *best_task; + task_data data; + + best_task = &timer_service->tasks[best]; + data = best_task->data; + if (is_periodic(best_task)) { + best_task->elapsed_time_ms = 0; + } else { + reset_timer_task(best_task); + if (before_best < timer_service->length) { + timer_service->tasks[before_best].next_task_idx = best_task->next_task_idx; + } else { + timer_service->next_active_task = best_task->next_task_idx; + } + best_task->next_task_idx = timer_service->next_free_task; + timer_service->next_free_task = best; + } + execute(&data); + task_fired = bool_true; + } + } while (task_fired && timer_service->next_active_task < timer_service->length); + proceeded_time += time_to_proceed; + time_till_next_task_ = time_till_next_task(timer_service); + time_to_proceed = (time_till_next_task_ < elapsed_ms - proceeded_time) + ? time_till_next_task_ + : (elapsed_ms - proceeded_time); + } +} + +/*! Cancel timer service. Use this to end possible timing threads and free + memory resources. + */ +void cancel(sc_timer_service_t *timer_service) +{ + sc_integer idx; + for (idx = 0; idx < timer_service->length; idx++) { + reset_timer_task(&timer_service->tasks[idx]); + timer_service->tasks[idx].next_task_idx = idx + 1; + } + timer_service->next_active_task = timer_service->length; + timer_service->next_free_task = 0; +} + +/*! Obtain the time (in ms) required to proceed to the next task. + */ +sc_time time_till_next_task(sc_timer_service_t *timer_service) +{ + sc_time time; + sc_integer task; + sc_time remaining_time; + + if (timer_service->next_active_task == timer_service->length) { + return 0; + } + + time = timer_service->tasks[timer_service->next_active_task].data.get.time_ms + - timer_service->tasks[timer_service->next_active_task].elapsed_time_ms; + task = timer_service->tasks[timer_service->next_active_task].next_task_idx; + while (task < timer_service->length) { + sc_timer_t *current_task; + + current_task = &timer_service->tasks[task]; + remaining_time = current_task->data.get.time_ms - current_task->elapsed_time_ms; + + if (remaining_time < time) { + time = remaining_time; + } + task = current_task->next_task_idx; + } + return time; +} + + diff --git a/robot_cdt/src/sc_timer_service.h b/robot_cdt/src/sc_timer_service.h new file mode 100644 index 0000000..18dff5e --- /dev/null +++ b/robot_cdt/src/sc_timer_service.h @@ -0,0 +1,143 @@ +/* Generated by itemis CREATE code generator. */ + +#ifndef SC_TIMER_SERVICE_H_ +#define SC_TIMER_SERVICE_H_ + +#include "sc_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/*! Interface definition of a timer service for itemis CREATE state machines. */ + +typedef void (*sc_raise_time_event_fp)(void *handle, sc_eventid evid); +typedef void (*sc_run_cycle_fp)(void *handle); + +typedef struct +{ + sc_raise_time_event_fp raise_event; + sc_eventid pt_evid; + sc_boolean periodic; +} time_based; + +typedef struct +{ + sc_time time_ms; + sc_run_cycle_fp run_cycle; + time_based time_event; + void *statemachine; +} non_empty; + +typedef enum { EMPTY_TASK, TIME_EVENT_TASK, RUNCYCLE_TASK } task_type; + +typedef struct +{ + task_type type; + non_empty get; +} task_data; + +/* A timer task. */ +typedef struct +{ + task_data data; + sc_time elapsed_time_ms; + sc_integer next_task_idx; +} sc_timer_t; + +typedef struct +{ + void *statemachine; + sc_eventid pt_evid; +} match_time_event; + +typedef struct +{ + void *statemachine; +} match_run_cycle_of; + +typedef union { + match_time_event match_time_event_; + match_run_cycle_of match_run_cycle_of_; +} timer_task_matcher_data; + +typedef struct +{ + timer_task_matcher_data data; + sc_boolean is_match_time_event; +} timer_task_matcher; + +typedef struct +{ + sc_integer length; + sc_timer_t *tasks; + sc_integer next_active_task; + sc_integer next_free_task; + sc_raise_time_event_fp raise_event; +} sc_timer_service_t; + +void reset_task_data(task_data *data); + +void execute(task_data *data); + +void update_elapsed_time_ms(sc_timer_t *task, sc_time elapsed_time_ms_); + +sc_boolean is_periodic(sc_timer_t *task); + +sc_boolean is_runcycle_event(sc_timer_t *task); + +void reset_timer_task(sc_timer_t *task); + +sc_boolean less_than(sc_timer_t *task, sc_timer_t *other); + +sc_boolean time_event_matcher(match_time_event *matcher, const sc_timer_t *other); + +sc_boolean run_cycle_matcher(match_run_cycle_of *matcher, const sc_timer_t *other); + +void set_generic_timer(sc_timer_service_t *timer_service, const task_data data); + +void unset_generic_timer(sc_timer_service_t *timer_service, timer_task_matcher *matcher); + +void sc_timer_service_init(sc_timer_service_t *timer_service, + sc_timer_t *tasks_, + sc_integer length_, + sc_raise_time_event_fp raise_event); + +void sc_timer_set(sc_timer_service_t *timer_service, + void *handle, + const sc_eventid evid, + const sc_time time_ms, + const sc_boolean periodic); + +void sc_timer_set_for_raise_event(sc_timer_service_t *timer_service, + sc_raise_time_event_fp raise_event, + void *statemachine, + const sc_eventid evid, + const sc_time time_ms, + const sc_boolean periodic); + +void sc_timer_unset(sc_timer_service_t *timer_service, sc_eventid evid); + +void sc_timer_unset_for_machine(sc_timer_service_t *timer_service, + sc_eventid evid, + void *statemachine); + +void set_runcycle_timer_for(sc_timer_service_t *timer_service, + sc_run_cycle_fp run_cycle, + void *statemachine, + sc_time cycle_period); + +void unset_runcycle_timer(sc_timer_service_t *timer_service, void *statemachine); + +void sc_timer_service_proceed(sc_timer_service_t *timer_service, sc_time elapsed_ms); + +void cancel(sc_timer_service_t *timer_service); + +sc_time time_till_next_task(sc_timer_service_t *timer_service); + +#ifdef __cplusplus +} +#endif + +#endif /* SC_TIMER_SERVICE_H_ */ + diff --git a/robot_cdt/src/sc_types.h b/robot_cdt/src/sc_types.h new file mode 100644 index 0000000..5139aed --- /dev/null +++ b/robot_cdt/src/sc_types.h @@ -0,0 +1,71 @@ +/** Generated by itemis CREATE code generator. */ + +#ifndef SC_TYPES_H_ +#define SC_TYPES_H_ + +#ifdef __STDC_VERSION__ + #if __STDC_VERSION__ >= 199901L + #define HAS_C99_BOOLS + #endif +#else + #ifdef __cplusplus + #define HAS_C99_BOOLS + #endif +#endif + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#ifdef HAS_C99_BOOLS +#include +#endif + +#define sc_string char* + +#ifdef HAS_C99_BOOLS +typedef bool sc_boolean; +#else +typedef uint8_t sc_boolean; +#endif + +typedef int_fast16_t sc_short; +typedef uint_fast16_t sc_ushort; +typedef int32_t sc_integer; +typedef uint32_t sc_uinteger; + +typedef int32_t sc_time; + +typedef double sc_real; + +typedef void* sc_eventid; + +typedef intptr_t sc_intptr_t; + +#ifdef __cplusplus +} +#endif + +#ifndef sc_null + #ifdef __cplusplus + #if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900) + #define sc_null nullptr + #else + #define sc_null 0 + #endif + #else + #define sc_null ((void *)0) + #endif +#endif + +#ifdef HAS_C99_BOOLS +#define bool_true true +#define bool_false false +#else +#define bool_true 1 +#define bool_false 0 +#endif + +#endif /* SC_TYPES_H_ */