02/24/2024 Test Launch Version (BB Black) (#11)

* Adding a 90% completed, compilable but untested ADS

* Made basic changes to actuator & sensor. Also added motor class

* Removed unnecessary .cpp files

* Updated sensor & actuator classes, finished ads, added variable time step to kalman filter, set up all tests for future assertions

* Relocated 'main' to 'active-drag-system.cpp'. Added more info to README

* Removed main.cpp

* Added more details to README

* Changed some function parameters from pass-by-pointer to pass-by-reference. Also removed the std namespace

* Started writing the test cases

* Updated the .gitignore file

* Removed some files that should be gitignored

* Up to date with Jazz's pull request

* Test Launch Branch Created; PRU Servo Control with Test Program

* Added I2C device class and register IDs for MPL [INCOMPLETE SENSOR IMPLEMENTATION]

Needs actual data getting function implementation for both sensors and register IDs for BNO, will implement shortly.

* Partial implementation of MPL sensor

Added startup method, still needs fleshed out data getters and setters and finished I2C implementation. MOST LIKELY WILL HAVE COMPILATION ISSUES.

* *Hypothetically* complete MPL implementation

NEEDS HARDWARE TESTING

* IMU Header and init() method implementation

Needs like, all data handling still lol

* Hypothetically functional (Definitely won't compile)

* We ball?

---------

Co-authored-by: Jazz Jackson <[email protected]>
Co-authored-by: Cian Capacci <[email protected]>

1 files changed, 184 insertions, 0 deletions

diff --git a/include/math/vector.h b/include/math/vector.h
new file mode 100644
index 0000000..10345c3
--- /dev/null
+++ b/include/math/vector.h
@@ -0,0 +1,184 @@
+//  Inertial Measurement Unit Maths Library
+//
+//  Copyright 2013-2021 Sam Cowen <[email protected]>
+//  Bug fixes and cleanups by Gé Vissers ([email protected])
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a
+//  copy of this software and associated documentation files (the "Software"),
+//  to deal in the Software without restriction, including without limitation
+//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
+//  and/or sell copies of the Software, and to permit persons to whom the
+//  Software is furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+//  DEALINGS IN THE SOFTWARE.
+
+#ifndef IMUMATH_VECTOR_HPP
+#define IMUMATH_VECTOR_HPP
+
+#include <math.h>
+#include <stdint.h>
+#include <string.h>
+
+namespace imu {
+
+template <uint8_t N> class Vector {
+public:
+  Vector() { memset(p_vec, 0, sizeof(double) * N); }
+
+  Vector(double a) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+  }
+
+  Vector(double a, double b) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+  }
+
+  Vector(double a, double b, double c) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+    p_vec[2] = c;
+  }
+
+  Vector(double a, double b, double c, double d) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+    p_vec[2] = c;
+    p_vec[3] = d;
+  }
+
+  Vector(const Vector<N> &v) {
+    for (int x = 0; x < N; x++)
+      p_vec[x] = v.p_vec[x];
+  }
+
+  ~Vector() {}
+
+  uint8_t n() { return N; }
+
+  double magnitude() const {
+    double res = 0;
+    for (int i = 0; i < N; i++)
+      res += p_vec[i] * p_vec[i];
+
+    return sqrt(res);
+  }
+
+  void normalize() {
+    double mag = magnitude();
+    if (isnan(mag) || mag == 0.0)
+      return;
+
+    for (int i = 0; i < N; i++)
+      p_vec[i] /= mag;
+  }
+
+  double dot(const Vector &v) const {
+    double ret = 0;
+    for (int i = 0; i < N; i++)
+      ret += p_vec[i] * v.p_vec[i];
+
+    return ret;
+  }
+
+  // The cross product is only valid for vectors with 3 dimensions,
+  // with the exception of higher dimensional stuff that is beyond
+  // the intended scope of this library.
+  // Only a definition for N==3 is given below this class, using
+  // cross() with another value for N will result in a link error.
+  Vector cross(const Vector &v) const;
+
+  Vector scale(double scalar) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] * scalar;
+    return ret;
+  }
+
+  Vector invert() const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = -p_vec[i];
+    return ret;
+  }
+
+  Vector &operator=(const Vector &v) {
+    for (int x = 0; x < N; x++)
+      p_vec[x] = v.p_vec[x];
+    return *this;
+  }
+
+  double &operator[](int n) { return p_vec[n]; }
+
+  double operator[](int n) const { return p_vec[n]; }
+
+  double &operator()(int n) { return p_vec[n]; }
+
+  double operator()(int n) const { return p_vec[n]; }
+
+  Vector operator+(const Vector &v) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] + v.p_vec[i];
+    return ret;
+  }
+
+  Vector operator-(const Vector &v) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] - v.p_vec[i];
+    return ret;
+  }
+
+  Vector operator*(double scalar) const { return scale(scalar); }
+
+  Vector operator/(double scalar) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] / scalar;
+    return ret;
+  }
+
+  void toDegrees() {
+    for (int i = 0; i < N; i++)
+      p_vec[i] *= 57.2957795131; // 180/pi
+  }
+
+  void toRadians() {
+    for (int i = 0; i < N; i++)
+      p_vec[i] *= 0.01745329251; // pi/180
+  }
+
+  double &x() { return p_vec[0]; }
+  double &y() { return p_vec[1]; }
+  double &z() { return p_vec[2]; }
+  double x() const { return p_vec[0]; }
+  double y() const { return p_vec[1]; }
+  double z() const { return p_vec[2]; }
+
+private:
+  double p_vec[N];
+};
+
+template <> inline Vector<3> Vector<3>::cross(const Vector &v) const {
+  return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1],
+                p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2],
+                p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]);
+}
+
+} // namespace imu
+
+#endif
\ No newline at end of file