WipBalance application

"Working"

LSM9DS1_MEMS/LSM9DS1_MEMS.hpp

@@ -106,7 +106,8 @@ class LSM9DS1
     [[nodiscard]]
     auto get_theta() const -> scalar_type
     {
-      return std::atan2(scalar_type(y), scalar_type(z));
+      // sign to match expectations on the bike
+      return -std::atan2(scalar_type(y), scalar_type(z));
     }
   };
 

balance/BUILD.bazel

@@ -0,0 +1,13 @@
+load("@rules_cc//cc:defs.bzl", "cc_binary")
+
+cc_binary(
+    name = "balance",
+    srcs = ["balance.cpp"],
+    deps = [
+        "//LSM9DS1_MEMS",
+        "//filter",
+        "//third_party/vesc_uart",
+        "//third_party/vesc_uart:extra",
+        "@fmt",
+    ],
+)

balance/balance.cpp

@@ -0,0 +1,111 @@
+#include "LSM9DS1_MEMS/LSM9DS1_MEMS.hpp"
+#include "filter/filter.hpp"
+#include "third_party/vesc_uart/VescUart.h"
+#include "third_party/vesc_uart/extra.hpp"
+
+#include <chrono>
+#include <fmt/core.h>
+#include <limits>
+#include <numbers>
+#include <thread>
+
+// Clang does not yet support floating point NTTP
+//
+template <class T>
+struct constant
+{
+  using value_type = T;
+
+  // NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members)
+  const value_type value;
+
+  consteval constant(value_type val) : value{val} {}
+
+  constexpr operator T() const { return value; }
+};
+
+auto PD_controller(float theta, float omega, float rpm) -> float
+{
+  constexpr auto P_angle = -1.0F * 1500.f;
+  constexpr auto D_angle = -1.0F * 400.f;
+
+  constexpr auto scale_wheel = 1.F * 0.35F;
+
+  constexpr auto D_wheel = 1.0F;
+
+  return scale_wheel * (P_angle * theta + D_angle * omega) + D_wheel * rpm;
+}
+
+// NOLINTBEGIN(readability-magic-numbers)
+constexpr auto sample_period = constant{0.0025F};  // s;
+constexpr auto dt = std::chrono::microseconds{2500};
+// // NOLINTEND(readability-magic-numbers)
+
+auto main(int argc, char** argv) -> int
+{
+
+  if (argc < 2) {
+    fmt::print("./i2c <device>");
+    return 1;
+  }
+
+  // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic)
+  const auto& device = argv[1];
+  auto sensor = LSM9DS1{device};
+
+  float theta_g{};
+  auto highpass_theta_g = filter::highpass<1, sample_period, float>{};
+  auto lowpss_theta_a = filter::lowpass<1, sample_period, float>{};
+  auto lowpss_omega_x = filter::lowpass<10, sample_period, float>{};
+
+  fmt::print("time;ay;az;wx;wm;theta_a;theta_g;theta_af;theta_gf;theta\n");
+
+  while (true) {
+    const auto now = std::chrono::steady_clock::now();
+
+    const auto wheel_rpm = float(::vesc::extra::read_rpm());
+
+    const auto gx = sensor.read_gx();
+    const auto omega_x_deg_s =
+        gx * 245.f / std::numeric_limits<decltype(gx)>::max();
+    const auto omega_x_rad_s =
+        omega_x_deg_s * std::numbers::pi_v<float> / 180.f;
+    const auto omega_xf = lowpss_omega_x(omega_x_rad_s).value;
+
+    theta_g += std::chrono::duration<float>{dt}.count() * omega_x_rad_s;
+    const auto theta_gf = highpass_theta_g(theta_g).value;
+
+    const auto accel = sensor.read_accel();
+    constexpr auto theta_a_calib = -0.048f;
+    const auto theta_a = accel.get_theta() + theta_a_calib;
+    const auto theta_af = lowpss_theta_a(theta_a).value;
+
+    const auto theta = theta_af + theta_gf;
+    const auto current_request = PD_controller(theta, omega_xf, wheel_rpm);
+    ::VescUartSetCurrent(current_request);
+
+    const auto deadline = now + dt;
+
+    fmt::print(
+        "{:+f};{};{};{:+f};{:+f};{:+f};{:+f};{:+f};{:+f};{:+f}\n",
+        std::chrono::duration<float>{now.time_since_epoch()}.count(),
+        accel.y,
+        accel.z,
+        omega_x_rad_s,
+        wheel_rpm,
+        theta_a,
+        theta_g,
+        theta_af,
+        theta_gf,
+        theta);
+    //  current_request,
+    //  1000.f *
+    //      std::chrono::duration<float>{
+    //          deadline - std::chrono::steady_clock::now()}
+    //          .count());
+
+    std::this_thread::sleep_until(deadline);
+  }
+
+  return 0;
+}