diff --git a/LSM9DS1_MEMS/LSM9DS1_MEMS.hpp b/LSM9DS1_MEMS/LSM9DS1_MEMS.hpp index 9221c86..cc90361 100644 --- a/LSM9DS1_MEMS/LSM9DS1_MEMS.hpp +++ b/LSM9DS1_MEMS/LSM9DS1_MEMS.hpp @@ -106,7 +106,7 @@ class LSM9DS1 [[nodiscard]] auto get_theta() const -> scalar_type { - return std::atan2(scalar_type(y), scalar_type(z)); + return std::atan2(scalar_type(z), scalar_type(y)); } }; @@ -173,11 +173,11 @@ 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_enable = 0b1 << 1; i2c_write( CTRL_REG4, - Z_disable | Y_disable | X_enable | LatchedInterrupt_disable); + Z_disable | Y_disable | X_enable | LatchedInterrupt_enable); } // NOLINTEND(readability-magic-numbers) @@ -198,22 +198,22 @@ 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; - i2c_write(CTRL_REG6_XL, ODR_50Hz | FS_4g | BW_sel | BW_50Hz); + static constexpr std::uint8_t ODR_952Hz = 0b110 << 5; + static constexpr std::uint8_t FS_2g = 0b00 << 3; + static constexpr std::uint8_t BW_odr = 0b0 << 2; + static constexpr std::uint8_t BW_408Hz = 0b00; + i2c_write(CTRL_REG6_XL, ODR_952Hz | FS_2g | BW_odr | BW_408Hz); } // CTRL_REG7_XL { - static constexpr std::uint8_t HR_enable = 0b1 << 7; + static constexpr std::uint8_t HR_disable = 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_disable = 0b0 << 2; static constexpr std::uint8_t HP_int_disable = 0b0; i2c_write( CTRL_REG7_XL, - HR_enable | LPF_ODR_div_50 | LPF_enable | HP_int_disable); + HR_disable | LPF_ODR_div_50 | LPF_disable | HP_int_disable); } // CTRL_REG8 diff --git a/balance/BUILD.bazel b/balance/BUILD.bazel new file mode 100644 index 0000000..b9f04cc --- /dev/null +++ b/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", + ], +) diff --git a/balance/balance.cpp b/balance/balance.cpp new file mode 100644 index 0000000..91ff0f2 --- /dev/null +++ b/balance/balance.cpp @@ -0,0 +1,127 @@ +#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 +#include +#include +#include +#include +#include + +// Clang does not yet support floating point NTTP +// +template +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 wheel_rotation_speed) + -> float +{ + constexpr auto P_angle = -1.5F * 1500.f; + constexpr auto D_angle = -1.F * 400.f; + + constexpr auto scale_wheel = +1.F * 0.35F; + + constexpr auto D_wheel = +1.F * 1.9F; + + constexpr auto scale_all = 1.0F; + + return scale_all * + (scale_wheel * (P_angle * theta + D_angle * omega) + + D_wheel * wheel_rotation_speed); +} + +// 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 "); + 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<8, sample_period, float>{}; + auto lowpss_wheel_rot = filter::lowpass<5, sample_period, float>{}; + + fmt::print("time;ay;az;wx;wm;theta_a;theta_g;theta_af;theta_gf;theta;I\n"); + + while (true) { + const auto now = std::chrono::steady_clock::now(); + + const auto wheel_rpm = -float(::vesc::extra::read_rpm()) / 45.F; + const auto wheel_rotation_speed = + wheel_rpm / 60.F * 2.F * std::numbers::pi_v; + const auto wheel_rotation_speed_f = + lowpss_wheel_rot(wheel_rotation_speed).value; + + const auto gx = sensor.read_gx(); + const auto omega_x_deg_s = + gx * 245.f / std::numeric_limits::max(); + const auto omega_x_rad_s = + omega_x_deg_s * std::numbers::pi_v / 180.f; + const auto omega_xf = lowpss_omega_x(omega_x_rad_s).value; + + theta_g += std::chrono::duration{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.971F / 180.F * std::numbers::pi_v; + 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_rotation_speed_f); + ::VescUartSetCurrent(current_request); + + const auto deadline = now + dt; + + fmt::print( + "{:+f};{};{};{:+f};{:+f};{:+f};{:+f};{:+f};{:+f};{:+f};{:+f}\n", + std::chrono::duration{now.time_since_epoch()}.count(), + accel.y, + accel.z, + omega_x_rad_s, + wheel_rotation_speed_f, + theta_a, + theta_g, + theta_af, + theta_gf, + theta, + current_request); + + std::cerr + << "Sleep time: " + << 1000.f * + std::chrono::duration{ + deadline - std::chrono::steady_clock::now()} + .count() + << std::endl; + + std::this_thread::sleep_until(deadline); + } + + return 0; +}