initial solution

robot_cdt/src/controller.c

@@ -0,0 +1,67 @@
+#include <time.h>
+#include <stdlib.h>
+#include <stdio.h>
+#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);
+}

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 <stddef.h>
+#include <stdint.h>
+
+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

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;
+}

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

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;
+}

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_ */

robot_cdt/src/sc_timer_service.c

@@ -0,0 +1,404 @@
+/* Generated by itemis CREATE code generator. */
+
+#include "sc_timer_service.h"
+#include <stddef.h>
+
+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], &current_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;
+}
+
+

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_ */
+

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 <stdint.h>
+#ifdef HAS_C99_BOOLS
+#include <stdbool.h>
+#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_ */