Merge pull request #20 from lf-lang/date23

Furuta pendulum example

C/README.md

@@ -2,4 +2,5 @@
 * [Patterns](src/patterns/README.md): Common communication patterns.
 * [Rosace](src/rosace/README.md): Aircraft controller illustrating periodic systems with multiple periods.
 * [Simulation](src/simulation/README.md): Using Lingua Franca for simulation.
-* [MQTT](src/MQTT/README.md): Interacting with MQTT-based services.
+* [MQTT](src/MQTT/README.md): Interacting with MQTT-based services.
+* [Furuta Pendulum](src/modal_models/FurutaPendulum/README.md): A controller implementation and simulation of a [Furuta pendulum](https://en.wikipedia.org/wiki/Furuta_pendulum) illustrating a modal reactor.

C/src/modal_models/FurutaPendulum/.gitignore

@@ -0,0 +1,2 @@
+pendulum.csv
+pendulum.pdf

C/src/modal_models/FurutaPendulum/FurutaPendulum.lf

@@ -0,0 +1,44 @@
+/**
+ * A simulation of a Furuta pendulum with a modal controller
+ * based on the Ptolemy II model constructed by Johan Eker
+ * and described in this paper:
+ * 
+ * J. Liu, J. Eker, J. W. Janneck, and E. A. Lee,
+ * “Realistic simulations of embedded control systems,”
+ * IFAC Proceedings Volumes, vol. 35, no. 1, pp. 391–396, 2002.
+ * 
+ * This program specifies a build script that only code generates
+ * and compiles the program, as usual, but also executes the
+ * program and processes its output to generate and open a
+ * plot. 
+ * You have to have installed gnuplot and have it in your
+ * PATH for this script to work as expected (and also cmake).
+ * 
+ * @author Edward A. Lee
+ * @author Alexander Schulz-Rosengarten
+ */
+target C {
+    timeout: 3 secs,
+    fast: true,
+    flags: "-lm",
+    build: "./build_run_plot.sh FurutaPendulum"
+}
+import PendulumController from "PendulumController.lf";
+import PendulumSimulation from "PendulumSimulation.lf";
+import Print from "Print.lf";
+
+main reactor {
+    s = new PendulumSimulation();
+    c = new PendulumController();
+    p = new Print();
+
+    s.phi, s.d_phi -> c.phi, c.d_phi;
+    s.theta, s.d_theta -> c.theta, c.d_theta;
+    c.control -> s.u;
+
+    c.control -> p.control;
+    c.modeID -> p.modeID;
+    c.energy -> p.energy;
+    s.theta -> p.theta;
+    s.phi -> p.phi;
+}

C/src/modal_models/FurutaPendulum/FurutaPendulumDisturbance.lf

@@ -0,0 +1,38 @@
+/**
+ * This variant of the Furuta pendulum example introduces
+ * a disturbance to bring the pendulum out of balance.
+ * 
+ * @author Edward A. Lee
+ * @author Alexander Schulz-Rosengarten
+ */
+target C {
+    timeout: 5 secs,
+    fast: true,
+    flags: "-lm",
+    build: "./build_run_plot.sh FurutaPendulumDisturbance"
+}
+import PendulumController from "PendulumController.lf";
+import PendulumSimulation from "PendulumSimulation.lf";
+import Print from "Print.lf";
+
+main reactor {
+    s = new PendulumSimulation();
+    c = new PendulumController();
+    p = new Print();
+
+    timer disturb(3 sec);
+
+    reaction(disturb) -> s.d {=
+        lf_set(s.d, 0.5);
+    =}
+
+    s.phi, s.d_phi -> c.phi, c.d_phi;
+    s.theta, s.d_theta -> c.theta, c.d_theta;
+    c.control -> s.u;
+
+    c.control -> p.control;
+    c.modeID -> p.modeID;
+    c.energy -> p.energy;
+    s.theta -> p.theta;
+    s.phi -> p.phi;
+}

C/src/experimental/FurutaPendulum/PendulumController.lf → C/src/modal_models/FurutaPendulum/PendulumController.lf

@@ -2,7 +2,7 @@
  * A modal controller for a Furuta pendulum,
  * based on the Ptolemy II model constructed by Johan Eker
  * and described in this paper:
- * 
+ *
  * J. Liu, J. Eker, J. W. Janneck, and E. A. Lee,
  * “Realistic simulations of embedded control systems,”
  * IFAC Proceedings Volumes, vol. 35, no. 1, pp. 391–396, 2002.
@@ -13,6 +13,7 @@ target C;
 preamble {=
     #include <math.h>
     #define PI 3.14159265
+    #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
     double sign(double x) {
         return (x > 0.0) - (x < 0.0);
     }
@@ -52,8 +53,11 @@ reactor PendulumController(
     initial mode SwingUp {
         reaction(theta, d_theta) d_phi -> control, modeID, energy {=
             double th = restrictAngle(theta->value);
-            double E = 0.5 * d_theta->value * d_theta->value / (self->w0 * self->w0)
-                    + cos(th) - 1.0;
+            double E = 0.5 
+                * d_theta->value 
+                * d_theta->value 
+                / (self->w0 * self->w0)
+                + cos(th) - 1.0;
             double c = sign(d_theta->value * cos(th));
             double out = sign(E) * MIN(fabs(self->k * E), self->n) * c;
             lf_set(control, out);
@@ -77,8 +81,11 @@ reactor PendulumController(
                 + d_phi->value * self->ci4
             ));
             lf_set(modeID, 0);
-            double E = 0.5 * d_theta->value * d_theta->value / (self->w0 * self->w0)
-                    + cos(th) - 1.0;
+            double E = 0.5 
+                * d_theta->value
+                * d_theta->value
+                / (self->w0 * self->w0)
+                + cos(th) - 1.0;
             lf_set(energy, E);
         =}
         reaction(phi, d_phi) -> Stabilize {=
@@ -98,8 +105,11 @@ reactor PendulumController(
                 + (phi->value - self->phi0) * self->si3
                 + d_phi->value * self->si4
             ));
-            double E = 0.5 * d_theta->value * d_theta->value / (self->w0 * self->w0)
-                    + cos(th) - 1.0;
+            double E = 0.5 
+                * d_theta->value 
+                * d_theta->value 
+                / (self->w0 * self->w0)
+                + cos(th) - 1.0;
             lf_set(energy, E);
             lf_set(modeID, 1);
         =}

C/src/experimental/FurutaPendulum/PendulumSimulation.lf → C/src/modal_models/FurutaPendulum/PendulumSimulation.lf

@@ -3,7 +3,7 @@ target C;
  * A simple forward-Euler simulation of a Furuta pendulum,
  * based on the Ptolemy II model constructed by Johan Eker
  * and described in this paper:
- * 
+ *
  * J. Liu, J. Eker, J. W. Janneck, and E. A. Lee,
  * “Realistic simulations of embedded control systems,”
  * IFAC Proceedings Volumes, vol. 35, no. 1, pp. 391–396, 2002.
@@ -70,7 +70,11 @@ reactor PendulumSimulation(
                     + pow(self->alpha * sin(self->x[0]), 2.0)
                     - pow(self->gamma * cos(self->x[0]), 2.0)
                 ) * (
-                    (self->alpha * self->beta + pow(self->alpha * sin(self->x[0]), 2.0))
+                    (
+                        self->alpha
+                        * self->beta
+                        + pow(self->alpha * sin(self->x[0]), 2.0)
+                    )
                     * pow(self->x[3], 2.0)
                     * sin(self->x[0])
                     * cos(self->x[0])
@@ -92,7 +96,11 @@ reactor PendulumSimulation(
                     * self->g
                     * self->latest_u
                 + 
-                    (self->alpha * self->beta + pow(self->alpha * sin(self->x[0]), 2.0))
+                    (
+                        self->alpha 
+                        * self->beta 
+                        + pow(self->alpha * sin(self->x[0]), 2.0)
+                    )
                     * self->epsilon / self->alpha * sin(self->x[0])
                 );
             double x2_dot = self->x[3];

C/src/modal_models/FurutaPendulum/Print.lf

@@ -0,0 +1,45 @@
+/**
+ * A utility reactor to print the pendulum state into a csv file.
+ */
+target C;
+
+preamble {=
+    #include <math.h>
+    #define PI 3.14159265
+=}
+
+reactor Print(filename:string("pendulum.csv")) {
+    input control:double;
+    input modeID:double;
+    input energy:double;
+    input theta:double;
+    input phi:double;
+
+    state file:FILE*({=NULL=});
+
+    reaction(startup) {=
+        self->file = fopen(self->filename, "w");
+        if(self->file == NULL) {
+            lf_print_error_and_exit("Failed to open file: %s", self->filename);
+        } else {
+            fprintf(self->file, "Time,Control,Mode,Energy,Theta,Phi\n");
+        }
+    =}
+
+    reaction(control, modeID, energy, theta, phi) {=
+        double t = lf_time_logical_elapsed() / 1.0e9;
+        fprintf(self->file,
+            "%f,%f,%f,%f,%f,%f\n",
+            t,
+            control->value,
+            modeID->value,
+            energy->value,
+            fmod(theta->value, PI),
+            fmod(phi->value, PI)
+        );
+    =}
+
+    reaction(shutdown) {=
+        fclose(self->file);
+    =}
+}