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