81 lines
2.8 KiB
C
81 lines
2.8 KiB
C
#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;
|
|
|
|
// Observers are stateless (they just link to callbacks) so it's safe to use global variables here
|
|
static sc_single_subscription_observer_int32_t observer_motorL;
|
|
static sc_single_subscription_observer_int32_t observer_motorR;
|
|
|
|
static int32_t motor_left_speed;
|
|
static int32_t motor_right_speed;
|
|
|
|
// event handler callback functions
|
|
void on_motorL(Statechart* sc, int32_t value) {
|
|
fprintf(stderr, "got event motorL: %f\n", value);
|
|
motor_left_speed = value;
|
|
}
|
|
void on_motorR(Statechart* sc, int32_t value) {
|
|
fprintf(stderr, "got event motorR: %f\n", value);
|
|
motor_right_speed = value;
|
|
}
|
|
|
|
// called once, on startup
|
|
void controller_init(Statechart* sc) {
|
|
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_int32_t_init(&observer_motorL, sc, (sc_observer_next_int32_t_fp) on_motorL);
|
|
sc_single_subscription_observer_int32_t_subscribe(&observer_motorL, statechart_get_setMotorL(sc));
|
|
|
|
// motorR observer
|
|
sc_single_subscription_observer_int32_t_init(&observer_motorR, sc, (sc_observer_next_int32_t_fp) on_motorR);
|
|
sc_single_subscription_observer_int32_t_subscribe(&observer_motorR, statechart_get_setMotorR(sc));
|
|
}
|
|
|
|
// to be called every 10 ms
|
|
void controller_poll(int32_t* _motor_left_speed, int32_t* _motor_right_speed) {
|
|
sc_time elapsed = 10; // ms
|
|
sc_timer_service_proceed(&timer_service, elapsed);
|
|
fprintf(stderr, "made 10ms step\n");
|
|
|
|
(*_motor_left_speed) = motor_left_speed;
|
|
(*_motor_right_speed) = motor_right_speed;
|
|
}
|
|
|
|
// finally, we need to implemented these functions (defined in "Statechart_required.h")
|
|
int32_t statechart_pid(Statechart* handle, const int32_t sensor, pid_vars_t pid_vars) {
|
|
int32_t 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 int32_t 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);
|
|
}
|
|
|
|
|
|
void main() {
|
|
// find out how big the Statechart-struct is:
|
|
fprintf(stderr, "%d", sizeof(Statechart));
|
|
}
|