diff --git a/cours/inverted-pendulum/Makefile b/cours/inverted-pendulum/Makefile new file mode 100644 index 0000000..60b5ef5 --- /dev/null +++ b/cours/inverted-pendulum/Makefile @@ -0,0 +1,62 @@ +HEPTC=heptc +PYGMENTS=python -m pygments -x -l ../../../notes/heptagon.py:HeptagonLexer + +HEPT_NAME=pendulum + +HEPT_SOURCES=$(HEPT_NAME).ept +HEPT_GENERATED=\ + $(HEPT_NAME)_types.h \ + $(HEPT_NAME).h \ + $(HEPT_NAME)_types.c \ + $(HEPT_NAME).c +C_SOURCES=mathext.c + +SWIG_SOURCE=$(HEPT_NAME).i +SWIG_GENERATED=$(HEPT_NAME).py $(HEPT_NAME)_wrap.c + +PY_SUFFIX=$(shell python -c \ + "import sysconfig as s; print(s.get_config_var('EXT_SUFFIX'))") +TARGET?=_$(HEPT_NAME)$(PY_SUFFIX) + +HTML=pendulum.html + +.PHONY: all clean run runplot repl + +all: $(TARGET) + +clean: + rm -f *.epci *.log *.mls *.obc *.epci *.epo + rm -f $(SWIG_GENERATED) $(HEPT_GENERATED) + rm -rf $(HEPT_NAME)_c build __pycache__ *.so + rm -f $(HTML) + +repl: all + python -i -c 'import $(HEPT_NAME)' + +run: all + ./main.py + +runplot: all + ./main.py --plot + +%.html: %.ept + $(PYGMENTS) -O full -o $@ $^ + +$(TARGET): $(SWIG_GENERATED) $(HEPT_GENERATED) $(C_SOURCES) + python setup.py build_ext --inplace + @echo $@ + +$(SWIG_GENERATED): $(SWIG_SOURCE) $(HEPT_GENERATED) $(C_SOURCES) + swig -python $< + +%_types.h %_types.c %.c %.h %.epci : %.ept + $(HEPTC) -c -target c $< + cp $(foreach ext,c h,$(basename $<)_c/*.$(ext)) . + +%.epci: %.epi + $(HEPTC) $< + +$(HEPT_SOURCES): mathext.epci debug.epci + +%.html: %.ept + $(PYGMENTS) -O full -o $@ $^ diff --git a/cours/inverted-pendulum/README.md b/cours/inverted-pendulum/README.md new file mode 100644 index 0000000..8fd88a7 --- /dev/null +++ b/cours/inverted-pendulum/README.md @@ -0,0 +1,43 @@ +# Pendule inversé et contrôleur PID + +## Présentation + +Ce dossier contient un exemple classique de système physique qu'on peut chercher +à contrôler, le pendule inversé. + +## Dépendances + +Le projet utilise du code Python 3, et le générateur d'interfaces SWIG pour +exposer le code C généré par Heptagon à Python. Pour le lancer, vous devez : + +- avoir Heptagon, à installer via OPAM, + +- avoir SWIG, à installer via le gestionnaire de paquet de votre système + d'exploitation (`apt-get` sous GNU/Linux Debian ou Ubuntu, `pacman` sous Arch + Linux, `brew` sous macOS, etc.), + +- avoir la bibliothèque Python `matplotlib`, à installer `pip install --user + matplotlib`. + +## Utilisation + +Vous pouvez lancer le programme en exécutant `make run`. + +Pour lancer le programme avec l'affichage des courbes, exécutez `make runplot`. +Attention : afficher les courbes engendre un ralentissement assez conséquent. + +L'interface utilisateur fonctione de la façon suivante : + +- **Q** permet de quitter, + +- **P** permet de mettre en pause, + +- **R** permet de réinitialiser la simulation, + +- **M** permet de changer de mode (manuel, contrôleur PID, contrôleur BangBang), + +- **←** et **→** permettent de déplacer le mobile, + +- **clic gauche** permet de déplacer le mobile à la souris. + +Les deux dernières commandes ne fonctionnent qu'en mode manuel. diff --git a/cours/inverted-pendulum/cutils.c b/cours/inverted-pendulum/cutils.c new file mode 100644 index 0000000..d4ab5ca --- /dev/null +++ b/cours/inverted-pendulum/cutils.c @@ -0,0 +1,88 @@ +/* This file is part of SyncContest. + Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */ + +#include "cutils.h" + +#include +#include +#include +#include +#include + +log_verbosity_level level = LOG_INFO; +FILE *f = NULL; +char *filename = NULL; + +void log_set_verbosity_level(log_verbosity_level l) { + level = l; +} + +void log_message_v(log_verbosity_level msg_level, const char *fmt, va_list va) { + va_list vb; + FILE *out = msg_level == LOG_INFO ? stdout : stderr; + + if (msg_level > level) + return; + + va_copy(vb, va); + if (f != NULL) { + vfprintf(f, fmt, va); + } + + vfprintf(out, fmt, vb); + fflush(out); +} + +void log_message(log_verbosity_level msg_level, const char *fmt, ...) { + va_list va; + va_start(va, fmt); + log_message_v(msg_level, fmt, va); + va_end(va); +} + +void log_fatal(const char *fmt, ...) { + va_list va; + va_start(va, fmt); + log_message_v(LOG_FATAL, fmt, va); + va_end(va); + exit(EXIT_FAILURE); +} + +void log_info(const char *fmt, ...) { + va_list va; + va_start(va, fmt); + log_message_v(LOG_INFO, fmt, va); + va_end(va); +} + +void log_debug(const char *fmt, ...) { + va_list va; + va_start(va, fmt); + log_message_v(LOG_DEBUG, fmt, va); + va_end(va); +} + +void log_init(const char *fn) { + if (!fn) { + log_info("[log] initializing, not saving to file\n"); + return; + } + + filename = strdup(fn); + assert (filename); + f = fopen(filename, "w"); + if (!f) + log_fatal("[log] could not open log file %s (fopen)", filename); + + log_info("[log] logging to %s\n", filename); +} + +void log_shutdown() { + if (f) { + log_info("[log] shutting down, closing %s\n", filename); + fclose(f); + free(filename); + } else { + log_info("[log] shutting down\n"); + } +} diff --git a/cours/inverted-pendulum/cutils.h b/cours/inverted-pendulum/cutils.h new file mode 100644 index 0000000..2876046 --- /dev/null +++ b/cours/inverted-pendulum/cutils.h @@ -0,0 +1,30 @@ +/* This file is part of SyncContest. + Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */ + +#ifndef CUTILS_H +#define CUTILS_H + +#include + +typedef enum { + LOG_FATAL = 0, + LOG_INFO = 1, + LOG_DEBUG = 2, +} log_verbosity_level; + +/* Calling `log_init(fn)` initializes the logging subsystem, asking it to save + log messages to the file `fn`. This pointer may be NULL, in which case the + messages are not saved. */ +void log_init(const char *filename); +void log_shutdown(); + +void log_set_verbosity_level(log_verbosity_level level); + +void log_message_v(log_verbosity_level level, const char *fmt, va_list); +void log_message(log_verbosity_level level, const char *fmt, ...); + +void log_fatal(const char *fmt, ...); +void log_info(const char *fmt, ...); +void log_debug(const char *fmt, ...); + +#endif /* CUTILS_H */ diff --git a/cours/inverted-pendulum/debug.c b/cours/inverted-pendulum/debug.c new file mode 100644 index 0000000..6fc6f31 --- /dev/null +++ b/cours/inverted-pendulum/debug.c @@ -0,0 +1,39 @@ +#include "debug.h" + +#include "cutils.h" + +void Debug__dbg_step(char *msg, Debug__dbg_out *o) { + log_info("%s\n", msg); +} + +void Debug__dbg_bool_step(char *msg, bool x, Debug__dbg_bool_out *o) { + log_info("%s %d\n", msg, x); +} + +void Debug__dbg_int_step(char *msg, int x, Debug__dbg_int_out *o) { + log_info("%s %d\n", msg, x); +} + +void Debug__dbg_float_step(char *msg, float x, Debug__dbg_float_out *o) { + log_info("%s %f\n", msg, x); +} + +void Debug__d_init_step(Debug__d_init_out *o) { + /* Empty by design */ +} + +void Debug__d_string_step(Debug__world _w, char *s, Debug__d_string_out *o) { + log_info("%s", s); +} + +void Debug__d_bool_step(Debug__world _w, bool b, Debug__d_bool_out *o) { + log_info("%d", b); +} + +void Debug__d_int_step(Debug__world _w, int i, Debug__d_int_out *o) { + log_info("%d", i); +} + +void Debug__d_float_step(Debug__world _w, float f, Debug__d_float_out *o) { + log_info("%f", f); +} diff --git a/cours/inverted-pendulum/debug.epi b/cours/inverted-pendulum/debug.epi new file mode 100644 index 0000000..56e5014 --- /dev/null +++ b/cours/inverted-pendulum/debug.epi @@ -0,0 +1,11 @@ +external fun dbg(msg : string) returns () +external fun dbg_bool(msg : string; x : bool) returns () +external fun dbg_int(msg : string; x : int) returns () +external fun dbg_float(msg : string; x : float) returns () + +type world +external fun d_init() returns (n : world) +external fun d_string(w : world; string) returns (n : world) +external fun d_bool(w : world; bool) returns (n : world) +external fun d_int(w : world; int) returns (n : world) +external fun d_float(w : world; float) returns (n : world) diff --git a/cours/inverted-pendulum/debug.h b/cours/inverted-pendulum/debug.h new file mode 100644 index 0000000..9a85ac1 --- /dev/null +++ b/cours/inverted-pendulum/debug.h @@ -0,0 +1,23 @@ +#ifndef DEBUG_H +#define DEBUG_H + +#include "stdbool.h" +#include "assert.h" +#include "pervasives.h" + +#include "hept_ffi.h" + +DECLARE_HEPT_FUN(Debug, dbg, (char *),); +DECLARE_HEPT_FUN(Debug, dbg_bool, (char *, bool),); +DECLARE_HEPT_FUN(Debug, dbg_int, (char *, int),); +DECLARE_HEPT_FUN(Debug, dbg_float, (char *, float),); + +typedef struct { } Debug__world; + +DECLARE_HEPT_FUN_NULLARY(Debug, d_init, Debug__world n); +DECLARE_HEPT_FUN(Debug, d_string, (Debug__world, char *), Debug__world n); +DECLARE_HEPT_FUN(Debug, d_bool, (Debug__world, bool), Debug__world n); +DECLARE_HEPT_FUN(Debug, d_int, (Debug__world, int), Debug__world n); +DECLARE_HEPT_FUN(Debug, d_float, (Debug__world, float), Debug__world n); + +#endif /* DEBUG_H */ diff --git a/cours/inverted-pendulum/debug_types.h b/cours/inverted-pendulum/debug_types.h new file mode 100644 index 0000000..d12b30d --- /dev/null +++ b/cours/inverted-pendulum/debug_types.h @@ -0,0 +1,4 @@ +#ifndef DEBUG_TYPES_H +#define DEBUG_TYPES_H + +#endif /* DEBUG_TYPES_H */ diff --git a/cours/inverted-pendulum/hept_ffi.h b/cours/inverted-pendulum/hept_ffi.h new file mode 100644 index 0000000..2d9b81b --- /dev/null +++ b/cours/inverted-pendulum/hept_ffi.h @@ -0,0 +1,52 @@ +#ifndef HEPT_FFI_H +#define HEPT_FFI_H + +#define UNPAREN(...) __VA_ARGS__ + +#define DECLARE_HEPT_FUN(module, name, inputs, outputs) \ + typedef struct { outputs; } module ## __ ## name ## _out; \ + void module ## __ ## name ##_step(UNPAREN inputs, \ + module ## __ ## name ## _out *) + +#define DECLARE_HEPT_FUN_NULLARY(module, name, outputs) \ + typedef struct { outputs; } module ## __ ## name ## _out; \ + void module ## __ ## name ##_step(module ## __ ## name ## _out *) + +#define DEFINE_HEPT_FUN(module, name, inputs) \ + void module ## __ ## name ##_step(UNPAREN inputs, \ + module ## __ ## name ## _out *out) + +#define DEFINE_HEPT_FUN_NULLARY(module, name, inputs) \ + void module ## __ ## name ##_step(module ## __ ## name ## _out *out) + +#define DECLARE_HEPT_NODE(module, name, inputs, outputs, state) \ + typedef struct { outputs; } module ## __ ## name ## _out; \ + typedef struct { state; } module ## __ ## name ## _mem; \ + void module ## __ ## name ##_step(UNPAREN inputs, \ + module ## __ ## name ## _out *, \ + module ## __ ## name ## _mem *); \ + void module ## __ ## name ##_reset(module ## __ ## name ## _mem *) + +#define DECLARE_HEPT_NODE_NULLARY(module, name, outputs, state) \ + typedef struct { outputs; } module ## __ ## name ## _out; \ + typedef struct { state; } module ## __ ## name ## _mem; \ + void module ## __ ## name ##_step(module ## __ ## name ## _out *, \ + module ## __ ## name ## _mem *); \ + void module ## __ ## name ##_reset(module ## __ ## name ## _mem *) + +#define DEFINE_HEPT_NODE_RESET(module, name) \ + void module ## __ ## name ##_reset(module ## __ ## name ## _mem *mem) + +#define DEFINE_HEPT_NODE_STEP(module, name, inputs) \ + void module ## __ ## name ##_step(UNPAREN inputs, \ + module ## __ ## name ## _out *out, \ + module ## __ ## name ## _mem *mem) + +#define DEFINE_HEPT_NODE_NULLARY_STEP(module, name, inputs) \ + void module ## __ ## name ##_step(module ## __ ## name ## _out *out, \ + module ## __ ## name ## _mem *mem) + +/* FIXME remove when Heptagon's pervasives.h has been fixed. */ +typedef char * string; + +#endif /* HEPT_FFI */ diff --git a/cours/inverted-pendulum/main.py b/cours/inverted-pendulum/main.py new file mode 100755 index 0000000..2a3e608 --- /dev/null +++ b/cours/inverted-pendulum/main.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 + +import pendulum +import math, time, argparse, sys + +from tkinter import * +from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg +from matplotlib.figure import Figure +import matplotlib.animation as animation + +parser = argparse.ArgumentParser( + usage = "%(prog)s [OPTION]", + description = "Simulate an inverted pendulum" +) +parser.add_argument('-p', '--plot', dest='plot', action='store_true') +parser.set_defaults(plot=False) +args = parser.parse_args() + +c = pendulum.Cart() +input_x0 = pendulum.max_w / 2 +mouse_diff = False +mode_diff = False +pause = False +last_time = {0: time.time()} + +theta_samples = [0] +error_samples = [0] + +root = Tk() +if sys.platform == 'linux': + root.attributes('-type', 'dialog') +root.title("Inverted pendulum") + +w = Canvas(root, width=pendulum.max_w, height=pendulum.max_h) + +f = Frame(root) +f.pack(side=TOP, fill=X) + +vmode = StringVar() +lmode = Label(f, textvariable=vmode) +lmode.pack(side=LEFT) +vinfo = StringVar() +linfo = Label(f, textvariable=vinfo) +linfo.pack(side=RIGHT) + +def create_line(x1, y1, x2, y2, **kwargs): + w.create_line(x1, pendulum.max_h - y1, x2, pendulum.max_h - y2, **kwargs) + +def create_oval(x1, y1, x2, y2, **kwargs): + w.create_oval(x1, pendulum.max_h - y1, x2, pendulum.max_h - y2, **kwargs) + +def push_sample(data, sample): + data.append(sample) + +def repaint(): + global input_x0, out, pause, error_samples + joint_color = "#4760B2" + mass_color = "#EF1010" + radius = 4 + if not pause: + out = c.step(input_x0, mouse_diff, mode_diff) + push_sample(theta_samples, math.fmod(out.p.theta, 2 * math.pi)) + push_sample(error_samples, out.p.error) + w.delete("all") + create_line(0, out.p.y0, pendulum.max_w, out.p.y0, width=5); + create_line(out.p.x0, out.p.y0, out.p.x, out.p.y, width=3) + create_oval(out.p.x - radius, out.p.y - radius, + out.p.x + radius, out.p.y + radius, fill=mass_color) + create_oval(out.p.x0 - 2 * radius, out.p.y0 - 2 * radius, + out.p.x0 + 2 * radius, out.p.y0 + 2 * radius, fill=joint_color) + input_x0 = out.p.x0 + vmode.set(f"Current mode: {out.p.mode_name}" + + (" (paused)" if pause else "")) + new_time = time.time() + vinfo.set("x: {:07.2f}, theta: {:06.2f}, error: {:06.2f}, fps: {:06.2f}" \ + .format(out.p.x, + out.p.theta, + out.p.error, + 1. / (new_time - last_time[0]))) + last_time.update({0:new_time}) + + w.after(int(1000 * pendulum.dt), repaint) + +def mouse_change_callback(event): + global mouse_diff, input_x0 + input_x0 = event.x + mouse_diff = not mouse_diff + +def key_pressed(event): + global input_x0, mouse_diff, mode_diff, pause + x_step = 3 + if event.keysym == 'q': + quit() + elif event.keysym == 'r': + c.reset() + input_x0 = pendulum.max_w / 2 + theta_samples.clear() + error_samples.clear() + elif event.keysym == 'm': + mode_diff = not mode_diff + elif event.keysym == 'p': + pause = not pause + elif event.keysym == 'Right' and not pause: + input_x0 = input_x0 + x_step + mouse_diff = not mouse_diff + elif event.keysym == 'Left' and not pause: + input_x0 = input_x0 - x_step + mouse_diff = not mouse_diff + +w.pack(expand = YES, fill = BOTH) +root.bind("", mouse_change_callback) +root.bind("", mouse_change_callback) +root.bind("", key_pressed) + +fig = Figure(figsize=(6, 4.5), dpi=100) +ax = fig.add_subplot(111) +text = ax.text(0.97,0.97, "", transform=ax.transAxes, ha="left", va="top") +line1, = ax.plot([]) +line1.set_label("theta") +line2, = ax.plot([]) +line2.set_label("error") +ax.legend(loc = 'upper left') + +def animate(i): + line1.set_data(range(len(theta_samples)), theta_samples) + line2.set_data(range(len(error_samples)), error_samples) + ax.set_xlim([0, len(theta_samples)]) + ax.set_ylim([min(min(theta_samples), min(error_samples)), + max(max(theta_samples), max(error_samples)) + 0.0001]) + + return line1, text + +if args.plot: + canfig = FigureCanvasTkAgg(fig, master=root) + canfig.draw() + canfig.get_tk_widget().pack(side=BOTTOM, fill=X) + ani = animation.FuncAnimation(fig, + animate, + interval=int(1000 * pendulum.dt)) + +root.focus_set() +repaint() +mainloop() diff --git a/cours/inverted-pendulum/mathext.c b/cours/inverted-pendulum/mathext.c new file mode 100644 index 0000000..9cc1169 --- /dev/null +++ b/cours/inverted-pendulum/mathext.c @@ -0,0 +1,55 @@ +#include +#include +#include + +/* Avoid Heptagon's math.h. I don't think there's a single place where to find + math.h on Apple-platforms. */ +#ifndef __APPLE__ +#include +#endif + +#include "mathext.h" + +void Mathext__float_step(int x, Mathext__float_out *o) { + o->o = (float)x; +} + +void Mathext__int_step(float x, Mathext__int_out *o) { + o->o = (int)x; +} + +void Mathext__floor_step(float x, Mathext__floor_out *o) { + o->o = floorf(x); +} + +void Mathext__sin_step(float x, Mathext__sin_out *o) { + o->o = sinf(x); +} + +void Mathext__cos_step(float x, Mathext__cos_out *o) { + o->o = cosf(x); +} + +void Mathext__atan2_step(float y, float x, Mathext__atan2_out *o) { + o->o = atan2f(y, x); +} + +void Mathext__pow_step(float x, float y, Mathext__pow_out *o) { + o->o = powf(x, y); +} + +void Mathext__hypot_step(float x, float y, Mathext__hypot_out *o) { + o->o = hypotf(x, y); +} + +void Mathext__sqrt_step(float x2, Mathext__sqrt_out *o) { + o->o = sqrtf(x2); +} + +void Mathext__fmod_step(float x, float y, Mathext__fmod_out *o) { + o->o = fmodf(x, y); +} + +void Mathext__piano_freq_of_key_step(int n, Mathext__piano_freq_of_key_out *o) { + o->f = (float)(pow(2, (float)(n - 49) / (float)12) * 440.); +} diff --git a/cours/inverted-pendulum/mathext.epi b/cours/inverted-pendulum/mathext.epi new file mode 100644 index 0000000..5fe95cd --- /dev/null +++ b/cours/inverted-pendulum/mathext.epi @@ -0,0 +1,16 @@ +external fun float(x : int) returns (o : float) +external fun int(x : float) returns (o : int) +external fun floor(x : float) returns (o : float) + +external fun sin(x : float) returns (o : float) +external fun cos(x : float) returns (o : float) +external fun atan2(y : float; x : float) returns (o : float) +external fun hypot(x : float; y : float) returns (o : float) +external fun sqrt(x2 : float) returns (o : float) +external fun pow(x : float; y : float) returns (o : float) + +external fun fmod(x : float; y : float) returns (o : float) + +external fun piano_freq_of_key(k : int) returns (f : float) + +const pi : float = 3.14115 diff --git a/cours/inverted-pendulum/mathext.h b/cours/inverted-pendulum/mathext.h new file mode 100644 index 0000000..a2e8624 --- /dev/null +++ b/cours/inverted-pendulum/mathext.h @@ -0,0 +1,27 @@ +#ifndef MATHEXT_H +#define MATHEXT_H + +#include "stdbool.h" +#include "assert.h" +#include "pervasives.h" + +#include "hept_ffi.h" + +DECLARE_HEPT_FUN(Mathext, float, (int), float o); +DECLARE_HEPT_FUN(Mathext, int, (float), int o); +DECLARE_HEPT_FUN(Mathext, floor, (float), float o); + +DECLARE_HEPT_FUN(Mathext, sin, (float), float o); +DECLARE_HEPT_FUN(Mathext, cos, (float), float o); +DECLARE_HEPT_FUN(Mathext, atan2, (float, float), float o); +DECLARE_HEPT_FUN(Mathext, hypot, (float, float), float o); +DECLARE_HEPT_FUN(Mathext, sqrt, (float), float o); +DECLARE_HEPT_FUN(Mathext, pow, (float, float), float o); + +DECLARE_HEPT_FUN(Mathext, fmod, (float, float), float o); + +DECLARE_HEPT_FUN(Mathext, piano_freq_of_key, (int), float f); + +static const float Mathext__pi = 3.14115; + +#endif /* MATHEXT_H */ diff --git a/cours/inverted-pendulum/mathext_types.h b/cours/inverted-pendulum/mathext_types.h new file mode 100644 index 0000000..a046195 --- /dev/null +++ b/cours/inverted-pendulum/mathext_types.h @@ -0,0 +1,7 @@ +#ifndef MATHEXT_TYPES_H +#define MATHEXT_TYPES_H + +/* FIXME remove once Heptagon properly defines its string type. */ +typedef char *string; + +#endif /* MATHEXT_TYPES_H */ diff --git a/cours/inverted-pendulum/node_wrapping.py b/cours/inverted-pendulum/node_wrapping.py new file mode 100644 index 0000000..5e9181f --- /dev/null +++ b/cours/inverted-pendulum/node_wrapping.py @@ -0,0 +1,38 @@ + +from sys import modules +from inspect import getmembers, isfunction, isclass + +# This should probably be done using MetaClasses. + +def create_node_wrapper_class(name): + table = dict([ (n, o) for n, o in getmembers(modules[__name__]) \ + if isfunction(o) or isclass(o) ]) + out_class = table[name + "_out"] + mem_class = table[name + "_mem"] + step_fun = table[name + "_step"] + reset_fun = table[name + "_reset"] + + def wrapper_init(self): + self.mem = mem_class() + reset_fun(self.mem) + + def wrapper_reset(self): + reset_fun(self.mem) + + def wrapper_step(self, *args): + out = out_class() + step_fun(*args, out, self.mem) + return out + + return type(name.capitalize(), (), { + '__init__': wrapper_init, + 'step': wrapper_step, + 'reset': wrapper_reset, + }) + +# For now, we only create wrappers for nodes that have memory. + +for node_mem in [ n for n, o in getmembers(modules[__name__]) \ + if isclass(o) and n.endswith("_mem") ]: + node = node_mem.replace("_mem", "") + globals()[node.capitalize()] = create_node_wrapper_class(node) diff --git a/cours/inverted-pendulum/pendulum.ept b/cours/inverted-pendulum/pendulum.ept new file mode 100644 index 0000000..438367f --- /dev/null +++ b/cours/inverted-pendulum/pendulum.ept @@ -0,0 +1,223 @@ +(******************************************************************************) +(* PENDULE INVERSÉ EN HEPTAGON *) +(******************************************************************************) + +(* Le but de ce fichier est d'illustrer l'automatique par un exemple classique, + le pendule inversé. Il s'agit d'un mobile sur lequel est fixé une tringle + rigide à l'extrémité de laquelle se trouve une sphère d'une certaine + masse. Le but est de déplacer le mobile de sorte à maintenir la sphère à la + verticale. + + https://en.wikipedia.org/wiki/Inverted_pendulum + + Le présent fichier est conçu pour s'interfacer avec l'interface graphique en + Python contenue dans `main.py`. *) + +(* On commence par quelques fonctions et noeuds utilitaires. *) + +fun max(x, y : float) returns (z : float) +let + z = if x <. y then y else x; +tel + +fun min(x, y : float) returns (z : float) +let + z = if x <. y then x else y; +tel + +fun clamp(x, x_min, x_max : float) returns (y : float) +let + y = min(x_max, max(x_min, x)); +tel + +fun abs(x : float) returns (y : float) +let + y = if x >. 0.0 then x else -. x; +tel + +fun ignore_below(x, eps : float) returns (y : float) +let + y = if abs(x) <. eps then 0.0 else x; +tel + +node maintain(ini : float; c : bool; x : float on c) returns (y : float) +let + y = merge c x ((ini fby y) whenot c); +tel + +node edge(b : bool) returns (c : bool) +let + c = b -> (b <> pre b); +tel + +(* Le pendule inversé, en tant que dispositif physique, voit sa dynamique + gouvernée par des équations différentielles. Pour le simuler, nous avons + besoin de résoudre celles-ci, ou du moins d'approcher leur solution par des + moyens numériques. *) + +(* La résolution d'équations différentielles par des moyens numériques constitue + un vaste pan de l'analyse numérique. Nous allons nous contenter de solutions + très simples, dites à pas fixe, où chaque instant synchrone sera supposé + correspondre à une certaine durée de temps physique `dt`. *) + +const dt : float = 0.01 + +(* L'intégration s'effectue par la méthode des rectangles, à la manière de la + définition de l'intégrale de Riemann. Plus `dt` est petit, plus les valeurs + calculées vont être précises. *) + +node integ(x_ini, dx : float) returns (x : float) +let + x = x_ini -> (dt *. dx +. pre x); +tel + +(* La dérivée s'effectue en suivant la formule classique + + x'(t) = lim_{dt → 0} (x(t + dt) - x(t)) / dt + + sauf qu'ici on s'arrête à un `dt` fixé, qu'on espère assez petit. *) + +node deriv(x : float) returns (dx : float) +let + dx = 0.0 -> ((x -. pre x) /. dt); +tel + +(* Pour définir la physique du pendule, on a besoin de certaines constantes : la + constante gravitationnelle sur Terre, la longueur de la tige, et la masse. *) + +const g : float = 9.81 + +const l : float = 100.0 + +const m : float = 10.0 + +(* Une partie de la simulation dépend du référentiel choisi, un rectangle de + `max_w` unités de côté et `max_h` unités de haut. *) + +const max_w : int = 1200 + +const max_h : int = 300 + +(* L'équation différentielle ci-dessous gouverne la physique du pendule inversé. + Ici ℓ est la longueur du pendule, m sa masse, g la gravité à la surface de la + Terre, et θ l'angle par rapport à la verticale. + + ℓ · d²θ/d²t - m · g · sin(θ) = - d²x₀/d²t · cos(θ) + + On peut exprimer d²θ/d²t en fonction de x₀. + + d²θ/dt² = (m · g · sin(θ) + d²x₀/dt² · cos(θ)) / ℓ + + Ensuite, on intègre. + + θ = ∫² (m · g · sin(θ) + d²x₀/dt² · cos(θ)) / ℓ · dt² + + On peut maintenant approcher la solution de l'intégrale et les dérivées + numériquement, à l'aide des noeuds integ() et deriv() définis précédemment, + pour obtenir la valeur de θ en fonction de celle de x₀. + + Attention : comme θ apparaît à gauche et à droite du signe égal, on doit + introduire un délai (`fby`) pour éviter une erreur de causalité. *) + +node pendulum(d2x0 : float) returns (theta : float) +var thetap, d2theta : float; +let + thetap = 0.0 fby theta; + d2theta = (m *. g *. Mathext.sin(thetap) -. d2x0 *. Mathext.cos(thetap)) /. l; + theta = integ(0.0, integ(0.0, d2theta)); +tel + +(* Notre but est maintenant d'écrire des contrôleurs, c'est à dire des noeuds + qui vont chercher à maintenir l'angle θ à 0 en déplaçant le mobile vers la + gauche quand il est positif, et vers la droite quand il est négatif. *) + +(* En pratique, on va écrire nos contrôleurs de manière générique, comme des + noeuds cherchant à calculer une _consigne_ `y` de sorte à ramener leur entrée + d'erreur `e` à 0. *) + +(* Il faut commencer par calculer l'erreur à fournir aux contrôleurs, en + fonction de θ. Pour ce faire, on ramène les angles dans l'intervalle [π, 2π] + dans l'intervalle [-π, 0]. *) + +fun error(theta : float) returns (err : float) +var tm : float; +let + tm = Mathext.fmod(theta, 2.0 *. Mathext.pi); + err = if tm <. Mathext.pi then tm else tm -. 2.0 *. Mathext.pi; +tel + +(* On peut maintenant écrire nos contrôleurs. *) + +(* Le premier contrôleur, le contrôleur bang-bang, est le plus simple. Si + l'erreur est positive, on envoie `act`, si elle est négative on envoie + `-. act`. *) + +node bangbang(e, act : float) returns (y : float) +var eps : float; +let + eps = 0.001; + y = if e >. eps then act + else if e <. -. eps then -. act + else 0.0; +tel + +(* Le contrôleur PID agit de façon proportionnelle à l'erreur, son intégrale et + sa dérivée. Il est paramétré par trois _gains_, c'est à dire les facteurs + qu'on applique respectivement à l'erreur, l'intégrale et la dérivée. *) + +node pid(e : float; kp, ki, kd : float) returns (y : float) +let + y = kp *. e +. ki *. integ(0.0, e) +. kd *. deriv(e); +tel + +(* Le noeud ci-dessous réunit tous les composants du fichier, le pendule et son + mobile, ainsi que la détection d'erreur, et envoie les données nécessaires à + l'interface graphique. Ces données sont représentées par le type `pend`. *) + +type pend = { x0 : float; y0 : float; x : float; y : float; + mode_name : string; theta : float; error : float } + +node cart(mouse_x0 : float; mouse_diff, mode_diff : bool) returns (p : pend) +var x_ini, y_ini, x0user, x0, x, y, theta, error : float; + last d2x0 : float = 0.0; + mouse_click, mode_change : bool; + mode_name : string; +let + mouse_click = edge(mouse_diff); + mode_change = edge(mode_diff); + x_ini = Mathext.float(max_w) /. 2.0; + y_ini = Mathext.float(max_h) /. 2.0; + x0user = maintain(x_ini, mouse_click, mouse_x0 when mouse_click); + + x0 = integ(x_ini, integ(0.0, d2x0)); + theta = pendulum(d2x0); + error = 0.0 fby error(theta); + + x = x0 +. l *. Mathext.sin(theta); + y = y_ini +. l *. Mathext.cos(theta); + + p = { x0 = x0; y0 = y_ini; x = x; y = y; + mode_name = mode_name; + theta = theta; error = error }; + + automaton + state User + do mode_name = "user"; + d2x0 = x0user -. (x_ini fby x0); + unless mode_change then PID + + state PID + var kP, kI, kD : float; + do mode_name = "PID control"; + d2x0 = 10.0 *. pid(error, kP, kI, kD); + kP = 40.0; + kI = 41.0; + kD = 70.0; + unless mode_change then BangBang + + state BangBang + do mode_name = "BangBang control"; + d2x0 = bangbang(error, 50.0) + unless mode_change then User + end; +tel diff --git a/cours/inverted-pendulum/pendulum.i b/cours/inverted-pendulum/pendulum.i new file mode 100644 index 0000000..a19438e --- /dev/null +++ b/cours/inverted-pendulum/pendulum.i @@ -0,0 +1,16 @@ +%module pendulum + +%{ +#define SWIG_FILE_WITH_INIT +#include "pendulum.h" +#include "/usr/include/math.h" // hack to avoid Heptagon's math.h +%} + +// Important: makes SWIG aware of our dumb "string" type. +%include "mathext_types.h" + +%rename("%(strip:[Pendulum__])s") ""; +%include "pendulum_types.h" +%include "pendulum.h" + +%pythoncode "node_wrapping.py" diff --git a/cours/inverted-pendulum/setup.py b/cours/inverted-pendulum/setup.py new file mode 100644 index 0000000..630cf63 --- /dev/null +++ b/cours/inverted-pendulum/setup.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +""" +setup.py file for Pendulum example +""" + +from distutils.core import setup, Extension +from subprocess import check_output + +heptagon_headers = \ + check_output("heptc -where", shell=True) \ + .decode("utf-8") \ + .replace("\n", "/c") + +pendulum_module = Extension('_pendulum', + include_dirs = [heptagon_headers, + '.', + 'pendulum_c'], + sources=['mathext.c', + 'debug.c', + 'cutils.c', + 'pendulum_wrap.c', + 'pendulum_c/pendulum_types.c', + 'pendulum_c/pendulum.c'], + ) + +setup (name = 'pendulum', + version = '0.1', + author = "Adrien Guatto", + description = "Wrapper for Heptagon module", + ext_modules = [pendulum_module], + py_modules = ["pendulum"], + ) diff --git a/cours/journal.org b/cours/journal.org index 57184d3..6020c65 100644 --- a/cours/journal.org +++ b/cours/journal.org @@ -25,6 +25,11 @@ * Cours 4 <2025-10-20> On traite des automates et tableaux. * Cours 5 <2025-10-27> - On traite des tableaux et de la génération d'échantillons audios. + On traite des tableaux et de la génération d'échantillons audios, disponbile dans son [dossier](audio/). * Cours 6 <2025-11-04> On traite du fonctionnement du projet. +* Cours 7 <2025-11-10> + On traite de l'exemple du pendule inversé, disponible dans son [dossier](inverted-pendulum/). + + On commence à expliquer la compilation d'Heptagon, avec l'explication de + MiniLS vers Obc jusqu'au traitement des horloges *exclu*. diff --git a/notes/notes-de-cours-progsync.pdf b/notes/notes-de-cours-progsync.pdf index 0d6e096..e6f8df1 100644 Binary files a/notes/notes-de-cours-progsync.pdf and b/notes/notes-de-cours-progsync.pdf differ