Merge pull request #19 from adamb314/NoMotorSurgery

No motor surgery
adamb314 · Mar 19, 2024 · a1b8ac7 · a1b8ac7
2 parents 93f176c + 0a3b61d
commit a1b8ac7
Show file tree

Hide file tree

Showing 150 changed files with 53,730 additions and 36 deletions.
diff --git a/ArduinoSketch/config/defaultDS3225.h b/ArduinoSketch/config/defaultDS3225.h
@@ -0,0 +1,127 @@
+#ifndef CONFIG_HOLDER_H
+#define CONFIG_HOLDER_H
+
+#include "../defaultConfigHolder.h"
+
+class SetupConfigHolder : public DefaultConfigHolder
+{
+public:
+    static std::unique_ptr<OpticalEncoderHandler> createMainEncoderHandler()
+    {
+        constexpr static std::array<uint16_t, 2048> aVec = {0};
+        constexpr static std::array<uint16_t, 2048> bVec = {0};
+
+        return std::make_unique<OpticalEncoderHandler>(aVec, bVec, A3, A4, 4096.0f * 10.0f / 1 * 11.0f / 62 * 14.0f / 48 * 13.0f / 45 * 1.0f / 42);
+    }
+
+    static std::unique_ptr<EncoderHandlerInterface> createOutputEncoderHandler()
+    {
+        constexpr static std::array<int16_t, 1025> compVec = {0};
+        return std::make_unique<ResistiveEncoderHandler>(A1, 4096.0f * 220.0f / 360, compVec);
+    }
+
+    static std::unique_ptr<CurrentController> createCurrentController()
+    {
+        constexpr float pwmToStallCurrent{1.0f};
+        constexpr float backEmfCurrent{0.0f};
+
+        auto pwmHighFrqCompFun = [](uint16_t in)
+        {
+            return in;
+        };
+        auto pwm = std::make_unique<HBridgeHighResPin11And12Pwm>(true, true, pwmHighFrqCompFun, 24000);
+        return std::make_unique<CurrentControlModel>(pwmToStallCurrent, backEmfCurrent, std::move(pwm));
+    }
+
+    class DefaultControlParametersWithPositionCompensation : public SetupConfigHolder::DefaultControlParameters
+    {
+        public:
+        static std::array<int16_t, 512> getPosDepForceCompVec()
+        {
+            constexpr static std::array<int16_t, 512> vec = {0};
+            return vec;
+        }
+
+        static std::array<int16_t, 512> getPosDepFrictionCompVec()
+        {
+            constexpr static std::array<int16_t, 512> vec = {0};
+            return vec;
+        }
+    };
+
+    class ControlParameters : public DefaultControlParametersWithPositionCompensation
+    {
+      public:
+        //kalman filter observer vector
+        static Eigen::Matrix<float, 3, 7> getKVector()
+        {
+            Eigen::Matrix<float, 3, 7> K;
+            K << -1.9619302890043613e-23f, 6.763936428834821e-19f, -1.0211439280426281e-14f, 8.966380809069127e-11f, -5.026553398907519e-07f, 0.0017546745669688208f, -0.01965617492779689f,
+                2.5494778082386736e-19f, -7.113271986018928e-15f, 7.773198181849748e-11f, -4.0726853722124173e-07f, 0.0008984664748649702f, 0.3759747733999441f, -61.62201356939168f,
+                5.093177792677117e-19f, -1.8966263561104195e-14f, 2.783998596483012e-10f, -1.9943485906824697e-06f, 0.006503382629253611f, -2.7471624009629547f, 339.77668409510306f;
+
+            return K;
+        }
+
+        //system model A matrix
+        static Eigen::Matrix3f getAMatrix()
+        {
+            Eigen::Matrix3f A;
+            A << 1.0f, 0.0005936462787358029f, 1.887871435352648e-05f,
+                0.0f, 0.9788959531583871f, 0.06270613007078364f,
+                0.0f, 0.0f, 1.0f;
+
+            return A;
+        }
+
+        //system model invers A matrix
+        static Eigen::Matrix3f getAInvMatrix()
+        {
+            Eigen::Matrix3f AInv;
+            AInv << 1.0f, -0.0006064447164383669f, 1.9149086916197254e-05f,
+                0.0f, 1.021559029612413f, -0.0640580133858595f,
+                0.0f, 0.0f, 1.0f;
+
+            return AInv;
+        }
+
+        //system model B matrix
+        static Eigen::Vector3f getBVector()
+        {
+            Eigen::Vector3f B;
+            B << 1.887871435352648e-05f,
+                0.06270613007078364f,
+                0.0f;
+
+            return B;
+        }
+
+        static bool internalFeedForwardEnabled()
+        {
+            return true;
+        }
+
+        //system model friction comp value
+        static float getFrictionComp()
+        {
+            return 58.36825891129791f;
+        }
+    };
+};
+
+class ConfigHolder
+{
+public:
+    static std::unique_ptr<Communication> getCommunicationHandler()
+    {
+        Serial.begin(115200);
+        Serial1.begin(115200);
+        auto com = std::make_unique<Communication>(SerialComOptimizer(&Serial1, &Serial));
+        com->addCommunicationNode(
+                std::make_unique<DCServoCommunicationHandler>(1, createDCServo<SetupConfigHolder>()));
+
+        return com;
+    }
+};
+
+#endif
diff --git a/ArduinoSketch/config/defaultMG90S.h b/ArduinoSketch/config/defaultMG90S.h
@@ -0,0 +1,127 @@
+#ifndef CONFIG_HOLDER_H
+#define CONFIG_HOLDER_H
+
+#include "../defaultConfigHolder.h"
+
+class SetupConfigHolder : public DefaultConfigHolder
+{
+public:
+    static std::unique_ptr<OpticalEncoderHandler> createMainEncoderHandler()
+    {
+        constexpr static std::array<uint16_t, 2048> aVec = {0};
+        constexpr static std::array<uint16_t, 2048> bVec = {0};
+
+        return std::make_unique<OpticalEncoderHandler>(aVec, bVec, A3, A4, 4096.0f * 10.0f / 48.0 * 10.0f / 38.0 * 10.0f / 38.0 * 10.0f / 38.0);
+    }
+
+    static std::unique_ptr<EncoderHandlerInterface> createOutputEncoderHandler()
+    {
+        constexpr static std::array<int16_t, 1025> compVec = {0};
+        return std::make_unique<ResistiveEncoderHandler>(A1, 4096.0f * 200.0f / 360, compVec);
+    }
+
+    static std::unique_ptr<CurrentController> createCurrentController()
+    {
+        constexpr float pwmToStallCurrent{1.0f};
+        constexpr float backEmfCurrent{0.0f};
+
+        auto pwmHighFrqCompFun = [](uint16_t in)
+        {
+            return in;
+        };
+        auto pwm = std::make_unique<HBridgeHighResPin11And12Pwm>(true, true, pwmHighFrqCompFun, 24000);
+        return std::make_unique<CurrentControlModel>(pwmToStallCurrent, backEmfCurrent, std::move(pwm));
+    }
+
+    class DefaultControlParametersWithPositionCompensation : public SetupConfigHolder::DefaultControlParameters
+    {
+        public:
+        static std::array<int16_t, 512> getPosDepForceCompVec()
+        {
+            constexpr static std::array<int16_t, 512> vec = {0};
+            return vec;
+        }
+
+        static std::array<int16_t, 512> getPosDepFrictionCompVec()
+        {
+            constexpr static std::array<int16_t, 512> vec = {0};
+            return vec;
+        }
+    };
+
+    class ControlParameters : public DefaultControlParametersWithPositionCompensation
+    {
+      public:
+        //kalman filter observer vector
+        static Eigen::Matrix<float, 3, 7> getKVector()
+        {
+            Eigen::Matrix<float, 3, 7> K;
+            K << -1.9622364197173024e-23f, 6.764613340485041e-19f, -1.0211998919971856e-14f, 8.96659593729972e-11f, -5.026592018080586e-07f, 0.001754677360416462f, -0.006887646928972721f,
+                2.5291289597823245e-19f, -7.054107394054792e-15f, 7.703046508627754e-11f, -4.0285856466740963e-07f, 0.0008823680645336847f, 0.4108714140087351f, -61.801326156486226f,
+                1.3052175124930118e-19f, -4.860751271226963e-15f, 7.135234239869617e-11f, -5.111528899101177e-07f, 0.0016668424014543767f, -0.704100787886532f, 87.08405835980246f;
+
+            return K;
+        }
+
+        //system model A matrix
+        static Eigen::Matrix3f getAMatrix()
+        {
+            Eigen::Matrix3f A;
+            A << 1.0f, 0.0005974960091957293f, 7.349786814334303e-05f,
+                0.0f, 0.9916649912249555f, 0.2446516076963969f,
+                0.0f, 0.0f, 1.0f;
+
+            return A;
+        }
+
+        //system model invers A matrix
+        static Eigen::Matrix3f getAInvMatrix()
+        {
+            Eigen::Matrix3f AInv;
+            AInv << 1.0f, -0.0006025180020297697f, 7.390912971926106e-05f,
+                0.0f, 1.0084050650661254f, -0.2467079203776173f,
+                0.0f, 0.0f, 1.0f;
+
+            return AInv;
+        }
+
+        //system model B matrix
+        static Eigen::Vector3f getBVector()
+        {
+            Eigen::Vector3f B;
+            B << 7.349786814334303e-05f,
+                0.2446516076963969f,
+                0.0f;
+
+            return B;
+        }
+
+        static bool internalFeedForwardEnabled()
+        {
+            return true;
+        }
+
+        //system model friction comp value
+        static float getFrictionComp()
+        {
+            return 39.875460580407044f;
+        }
+    };
+};
+
+class ConfigHolder
+{
+public:
+    static std::unique_ptr<Communication> getCommunicationHandler()
+    {
+        Serial.begin(115200);
+        Serial1.begin(115200);
+        auto com = std::make_unique<Communication>(SerialComOptimizer(&Serial1, &Serial));
+        com->addCommunicationNode(
+                std::make_unique<DCServoCommunicationHandler>(1, createDCServo<SetupConfigHolder>()));
+
+        return com;
+    }
+};
+
+#endif
diff --git a/CadFiles/MicroServoMG90S/baseBox.stl b/CadFiles/MicroServoMG90S/baseBox.stl
diff --git a/CadFiles/MicroServoMG90S/baseBushings.stl b/CadFiles/MicroServoMG90S/baseBushings.stl
diff --git a/CadFiles/MicroServoMG90S/encDisc.stl b/CadFiles/MicroServoMG90S/encDisc.stl
diff --git a/CadFiles/MicroServoMG90S/encDiscTruningMount.stl b/CadFiles/MicroServoMG90S/encDiscTruningMount.stl