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, 40 insertions, 0 deletions

diff --git a/src/surfaceFitModel.cpp b/src/surfaceFitModel.cpp
new file mode 100644
index 0000000..d48da49
--- /dev/null
+++ b/src/surfaceFitModel.cpp
@@ -0,0 +1,40 @@
+#include "../include/surfaceFitModel.hpp"
+
+SurfaceFitModel::SurfaceFitModel() {
+
+    p = MatrixXd::Zero(X_DEGREE + 1, Y_DEGREE + 1);
+
+    p(0, 0) = -781536.384794701;
+    p(1, 0) = 8623.59011973048;
+    p(0, 1) = 643.65918253;
+    p(2, 0) = -34.3646691281487;
+    p(1, 1) = -5.46066535343611;
+    p(0, 2) = -0.177121900557321;
+    p(3, 0) = 0.0573287698655951;
+    p(2, 1) = 0.0150031142038895;
+    p(1, 2) = 0.00101871763126609;
+    p(0, 3) = 1.63862900553892e-05;
+    p(4, 0) = -3.21785828407871e-05;
+    p(3, 1) = -1.3161091180883e-05;
+    p(2, 2) = -1.42505256569339e-06;
+    p(1, 3) = -4.76209793830867e-08;
+}
+
+
+double SurfaceFitModel::getFit(double x, double y) {
+
+    return p(0, 0) + p(1, 0) * x + p(0, 1) * y + p(2, 0) * pow(x, 2) +
+	p(1, 1) * x * y + p(0, 2) * pow(y, 2) + p(3, 0) * pow(x, 3) +
+	p(2, 1) * pow(x, 2) * y + p(1, 2) * x * pow(y, 2) + p(0, 3) * pow(y, 3) +
+	p(4, 0) * pow(x, 4) + p(3, 1) * pow(x, 3) * y + p(2, 2) * pow(x, 2) * pow(y, 2) +
+	p(1, 3) * x * pow(y, 3); 
+}
+
+
+
+
+
+
+
+
+