Finish up the ARM MPC as far as I could get before abandoning
Turns out this is a bit of a dead end due to the solver employed. I'll
have to revisit it in the future.
Change-Id: Ib75a053395afa6f31dee3ba6c20a236c7c0b433f
diff --git a/y2018/control_loops/python/2d_plot.cc b/y2018/control_loops/python/2d_plot.cc
new file mode 100644
index 0000000..52b8b34
--- /dev/null
+++ b/y2018/control_loops/python/2d_plot.cc
@@ -0,0 +1,36 @@
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+#include "third_party/gflags/include/gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
+DEFINE_double(yrange, 1.0, "+- y max");
+
+double fx(double x, double yrange) {
+ return 2.0 * ((1.0 / (1.0 + ::std::exp(-x * 2.0 / yrange)) - 0.5)) *
+ yrange;
+}
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+
+ matplotlibcpp::figure();
+ ::std::vector<double> x;
+ ::std::vector<double> y;
+ ::std::vector<double> slope_y;
+
+ for (double i = -5.0; i < 5.0; i += 0.01) {
+ x.push_back(i);
+ y.push_back(fx(i, FLAGS_yrange));
+ slope_y.push_back(
+ (fx(i + 0.0001, FLAGS_yrange) - fx(i - 0.0001, FLAGS_yrange)) /
+ (2.0 * 0.0001));
+ }
+
+ matplotlibcpp::plot(x, y, {{"label", "saturated x"}});
+ matplotlibcpp::plot(x, slope_y, {{"label", "slope"}});
+ matplotlibcpp::legend();
+ matplotlibcpp::show();
+
+}