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));
     }
   };
 
@@ -173,7 +174,7 @@ class LSM9DS1
       static constexpr std::uint8_t Z_disable = 0b0 << 5;
       static constexpr std::uint8_t Y_disable = 0b0 << 4;
       static constexpr std::uint8_t X_enable = 0b1 << 3;
-      static constexpr std::uint8_t LatchedInterrupt_disable = 0b0 << 1;
+      static constexpr std::uint8_t LatchedInterrupt_disable = 0b1 << 1;
 
       i2c_write(
           CTRL_REG4,
@@ -198,18 +199,18 @@ class LSM9DS1
 
     // CTRL_REG6_XL
     {
-      static constexpr std::uint8_t ODR_50Hz = 0b010 << 5;
-      static constexpr std::uint8_t FS_4g = 0b10 << 3;
-      static constexpr std::uint8_t BW_sel = 0b1 << 2;
-      static constexpr std::uint8_t BW_50Hz = 0b11;
+      static constexpr std::uint8_t ODR_50Hz = 0b110 << 5;
+      static constexpr std::uint8_t FS_4g = 0b00 << 3;
+      static constexpr std::uint8_t BW_sel = 0b0 << 2;
+      static constexpr std::uint8_t BW_50Hz = 0b00;
       i2c_write(CTRL_REG6_XL, ODR_50Hz | FS_4g | BW_sel | BW_50Hz);
     }
 
     // CTRL_REG7_XL
     {
-      static constexpr std::uint8_t HR_enable = 0b1 << 7;
+      static constexpr std::uint8_t HR_enable = 0b0 << 7;
       static constexpr std::uint8_t LPF_ODR_div_50 = 0b00 << 5;
-      static constexpr std::uint8_t LPF_enable = 0b1 << 2;
+      static constexpr std::uint8_t LPF_enable = 0b0 << 2;
       static constexpr std::uint8_t HP_int_disable = 0b0;
       i2c_write(
           CTRL_REG7_XL,

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,120 @@
+#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 <iostream>
+#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 = 0.F * 1.0F;
+
+  return scale_wheel * (P_angle * theta + D_angle * omega) + D_wheel * rpm;
+}
+
+// NOLINTBEGIN(readability-magic-numbers)
+constexpr auto sample_period = constant{0.01F};  // s;
+constexpr auto dt = std::chrono::milliseconds{10};
+// // 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()) / 45.F;
+
+    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::cerr
+        << "Sleep time: "
+        << 1000.f *
+               std::chrono::duration<float>{
+                   deadline - std::chrono::steady_clock::now()}
+                   .count()
+        << std::endl;
+
+    std::this_thread::sleep_until(deadline);
+  }
+
+  return 0;
+}