teaching/montenegro
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

return motor speed from poll function (dirty af)

Browse source
This commit is contained in:
Joeri Exelmans 2025-06-04 17:38:09 +02:00
parent 10d3de62da
commit e8f246cfa5
3 changed files with 30 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

4
robot_cdt/Makefile
View file

@ -18,8 +18,8 @@ build/%.o: %.c create_build_dirs
$(TARGET): $(OBJS) $(TARGET): $(OBJS)
$(CC) $(CFLAGS) -o build/$@ $^ $(CC) $(CFLAGS) -o build/$@ $^
# all: $(TARGET) all: $(TARGET)
all: $(OBJS) # all: $(OBJS)
create_build_dirs: create_build_dirs:
mkdir -p build/src mkdir -p build/src

2
robot_cdt/Statechart.ysc
View file

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<xmi:XMI xmi:version="2.0" xmlns:xmi="http://www.omg.org/XMI" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:notation="http://www.eclipse.org/gmf/runtime/1.0.2/notation" xmlns:sgraph="http://www.yakindu.org/sct/sgraph/2.0.0"> <xmi:XMI xmi:version="2.0" xmlns:xmi="http://www.omg.org/XMI" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:notation="http://www.eclipse.org/gmf/runtime/1.0.2/notation" xmlns:sgraph="http://www.yakindu.org/sct/sgraph/2.0.0">
<sgraph:Statechart xmi:id="_jCYoMD-lEfCA8aDVRFe8Eg" specification="@EventDriven&#xA;@SuperSteps(no)&#xA;&#xA;import: &quot;./src/data_types.h&quot;&#xA;&#xA;interface:&#xA;&#x9;in event sensor: real&#xA;&#xA;&#x9;in event buttonPressed&#xA;&#x9;in event buttonReleased&#xA;&#x9;&#xA;&#x9;out event setMotorR: real&#xA;&#x9;out event setMotorL: real&#xA;&#xA;&#x9;operation pid(sensor: real, pid_vars: pid_vars_t): real&#xA;&#xA;internal:&#xA;&#x9;var speed: real&#xA;&#x9;var pid_vars: pid_vars_t" name="Statechart" domainID="com.yakindu.domain.c"> <sgraph:Statechart xmi:id="_jCYoMD-lEfCA8aDVRFe8Eg" specification="@EventDriven&#xA;@SuperSteps(no)&#xA;&#xA;import: &quot;./src/data_types.h&quot;&#xA;&#xA;interface:&#xA;&#x9;in event sensor: int32_t&#xA;&#xA;&#x9;in event buttonPressed&#xA;&#x9;in event buttonReleased&#xA;&#x9;&#xA;&#x9;out event setMotorR: int32_t&#xA;&#x9;out event setMotorL: int32_t&#xA;&#xA;&#x9;operation pid(sensor: int32_t, pid_vars: pid_vars_t): int32_t&#xA;&#xA;internal:&#xA;&#x9;var speed: real&#xA;&#x9;var pid_vars: pid_vars_t" name="Statechart" domainID="com.yakindu.domain.c">
<regions xmi:id="_jCcSkz-lEfCA8aDVRFe8Eg" name="main region"> <regions xmi:id="_jCcSkz-lEfCA8aDVRFe8Eg" name="main region">
<vertices xsi:type="sgraph:State" xmi:id="_IsdxcD-yEfCA8aDVRFe8Eg" name="on" incomingTransitions="_ff2psD-yEfCA8aDVRFe8Eg"> <vertices xsi:type="sgraph:State" xmi:id="_IsdxcD-yEfCA8aDVRFe8Eg" name="on" incomingTransitions="_ff2psD-yEfCA8aDVRFe8Eg">
<regions xmi:id="_IseYgD-yEfCA8aDVRFe8Eg" name="r1"> <regions xmi:id="_IseYgD-yEfCA8aDVRFe8Eg" name="r1">

41
robot_cdt/src/controller.c
View file

@ -14,20 +14,25 @@
static sc_timer_t timers[MAX_TIMERS]; static sc_timer_t timers[MAX_TIMERS];
static sc_timer_service_t timer_service; 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_sc_integer observer_motorL;
static sc_single_subscription_observer_sc_integer observer_motorR;
static sc_integer motor_left_speed;
static sc_integer motor_right_speed;
// event handler callback functions // event handler callback functions
void on_motorL(Statechart* sc, sc_real value) { void on_motorL(Statechart* sc, sc_integer value) {
fprintf(stderr, "got event motorL: %f\n", value); fprintf(stderr, "got event motorL: %f\n", value);
motor_left_speed = value;
} }
void on_motorR(Statechart* sc, sc_real value) { void on_motorR(Statechart* sc, sc_integer value) {
fprintf(stderr, "got event motorR: %f\n", value); fprintf(stderr, "got event motorR: %f\n", value);
motor_right_speed = value;
} }
// called once, on startup // called once, on startup
void controller_init( void controller_init(Statechart* sc) {
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_timer_service_init(&timer_service, timers, MAX_TIMERS,
(sc_raise_time_event_fp) &statechart_raise_time_event); (sc_raise_time_event_fp) &statechart_raise_time_event);
@ -37,24 +42,27 @@ void controller_init(
// subscribe to output events: // subscribe to output events:
// motorL observer // 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_integer_init(&observer_motorL, sc, (sc_observer_next_sc_integer_fp) on_motorL);
sc_single_subscription_observer_sc_real_subscribe(observer_motorL, statechart_get_setMotorL(sc)); sc_single_subscription_observer_sc_integer_subscribe(&observer_motorL, statechart_get_setMotorL(sc));
// motorR observer // 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_integer_init(&observer_motorR, sc, (sc_observer_next_sc_integer_fp) on_motorR);
sc_single_subscription_observer_sc_real_subscribe(observer_motorR, statechart_get_setMotorR(sc)); sc_single_subscription_observer_sc_integer_subscribe(&observer_motorR, statechart_get_setMotorR(sc));
} }
// to be called every 10 ms // to be called every 10 ms
void controller_poll() { void controller_poll(sc_integer* _motor_left_speed, sc_integer* _motor_right_speed) {
sc_time elapsed = 10; // ms sc_time elapsed = 10; // ms
sc_timer_service_proceed(&timer_service, elapsed); sc_timer_service_proceed(&timer_service, elapsed);
fprintf(stderr, "made 10ms step\n"); 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") // 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_integer statechart_pid(Statechart* handle, const sc_integer sensor, pid_vars_t pid_vars) {
sc_real error = TARGET_DIST - sensor; // is this correct? sc_integer error = TARGET_DIST - sensor; // is this correct?
return pid(pid_vars, error); return pid(pid_vars, error);
} }
// here you see the reason we need global variables for the timer service: // here you see the reason we need global variables for the timer service:
@ -65,3 +73,8 @@ void statechart_set_timer(Statechart *sc, const sc_eventid evid, const sc_intege
void statechart_unset_timer(Statechart *sc, const sc_eventid evid) { void statechart_unset_timer(Statechart *sc, const sc_eventid evid) {
sc_timer_unset(&timer_service, evid); sc_timer_unset(&timer_service, evid);
} }
void main() {
fprintf(stderr, "%d", sizeof(Statechart));
}