Merge pull request #108 from ut-ras/encoder

I2C support and encoder driver
ut-ras · May 16, 2024 · 8654c08 · 8654c08
2 parents 2230af5 + 9d0e6d0
commit 8654c08
Show file tree

Hide file tree

Showing 31 changed files with 5,312 additions and 4,327 deletions.
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -1,18 +1,18 @@
 {
-    "version": "0.2.0",
-    "configurations": [
-        {
-            "name": "Debug Hardware",
-            "type": "cortex-debug",
-            "request": "launch",
-            "servertype": "openocd",
-            "cwd": "${workspaceRoot}",
-            "preLaunchTask": "Flash - Debug",
-            "executable": "./ut-robomaster/build/hardware/scons-debug/ut-robomaster.elf",
-            "device": "STM32F427II",
-            "configFiles": [
-                "./ut-robomaster/openocd.cfg"
-            ]
-        }
-    ]
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "Debug Hardware",
+      "type": "cortex-debug",
+      "request": "launch",
+      "servertype": "openocd",
+      "cwd": "${workspaceRoot}",
+      "preLaunchTask": "Flash - Debug",
+      "executable": "./ut-robomaster/build/hardware/scons-debug/ut-robomaster.elf",
+      "device": "STM32F427II",
+      "configFiles": [
+        "./ut-robomaster/openocd.cfg"
+      ]
+    }
+  ]
 }
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -1,96 +1,102 @@
 {
-    "editor.tabSize": 4,
-    "editor.formatOnSave": true,
-    "terminal.integrated.cwd": "ut-robomaster",
-    "files.associations": {
-        "array": "cpp",
-        "string": "cpp",
-        "string_view": "cpp",
-        "functional": "cpp",
-        "limits": "cpp",
-        "any": "cpp",
-        "atomic": "cpp",
-        "*.tcc": "cpp",
-        "bitset": "cpp",
-        "cctype": "cpp",
-        "chrono": "cpp",
-        "cinttypes": "cpp",
-        "clocale": "cpp",
-        "cmath": "cpp",
-        "complex": "cpp",
-        "condition_variable": "cpp",
-        "csetjmp": "cpp",
-        "csignal": "cpp",
-        "cstdarg": "cpp",
-        "cstddef": "cpp",
-        "cstdint": "cpp",
-        "cstdio": "cpp",
-        "cstdlib": "cpp",
-        "cstring": "cpp",
-        "ctime": "cpp",
-        "cuchar": "cpp",
-        "cwchar": "cpp",
-        "cwctype": "cpp",
-        "deque": "cpp",
-        "forward_list": "cpp",
-        "list": "cpp",
-        "unordered_map": "cpp",
-        "unordered_set": "cpp",
-        "vector": "cpp",
-        "exception": "cpp",
-        "algorithm": "cpp",
-        "iterator": "cpp",
-        "map": "cpp",
-        "memory": "cpp",
-        "memory_resource": "cpp",
-        "numeric": "cpp",
-        "optional": "cpp",
-        "random": "cpp",
-        "ratio": "cpp",
-        "regex": "cpp",
-        "set": "cpp",
-        "system_error": "cpp",
-        "tuple": "cpp",
-        "type_traits": "cpp",
-        "utility": "cpp",
-        "fstream": "cpp",
-        "initializer_list": "cpp",
-        "iomanip": "cpp",
-        "iosfwd": "cpp",
-        "iostream": "cpp",
-        "istream": "cpp",
-        "mutex": "cpp",
-        "new": "cpp",
-        "ostream": "cpp",
-        "scoped_allocator": "cpp",
-        "sstream": "cpp",
-        "stdexcept": "cpp",
-        "streambuf": "cpp",
-        "thread": "cpp",
-        "typeinfo": "cpp",
-        "valarray": "cpp",
-        "variant": "cpp",
-        "SConstruct": "python",
-        "SConscript": "python",
-        "stop_token": "cpp",
-        "cassert": "cpp"
+  "editor.tabSize": 4,
+  "editor.formatOnSave": true,
+  "terminal.integrated.cwd": "ut-robomaster",
+  "files.associations": {
+    "array": "cpp",
+    "string": "cpp",
+    "string_view": "cpp",
+    "functional": "cpp",
+    "limits": "cpp",
+    "any": "cpp",
+    "atomic": "cpp",
+    "*.tcc": "cpp",
+    "bitset": "cpp",
+    "cctype": "cpp",
+    "chrono": "cpp",
+    "cinttypes": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "complex": "cpp",
+    "condition_variable": "cpp",
+    "csetjmp": "cpp",
+    "csignal": "cpp",
+    "cstdarg": "cpp",
+    "cstddef": "cpp",
+    "cstdint": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "ctime": "cpp",
+    "cuchar": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "deque": "cpp",
+    "forward_list": "cpp",
+    "list": "cpp",
+    "unordered_map": "cpp",
+    "unordered_set": "cpp",
+    "vector": "cpp",
+    "exception": "cpp",
+    "algorithm": "cpp",
+    "iterator": "cpp",
+    "map": "cpp",
+    "memory": "cpp",
+    "memory_resource": "cpp",
+    "numeric": "cpp",
+    "optional": "cpp",
+    "random": "cpp",
+    "ratio": "cpp",
+    "regex": "cpp",
+    "set": "cpp",
+    "system_error": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "utility": "cpp",
+    "fstream": "cpp",
+    "initializer_list": "cpp",
+    "iomanip": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "mutex": "cpp",
+    "new": "cpp",
+    "ostream": "cpp",
+    "scoped_allocator": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "streambuf": "cpp",
+    "thread": "cpp",
+    "typeinfo": "cpp",
+    "valarray": "cpp",
+    "variant": "cpp",
+    "SConstruct": "python",
+    "SConscript": "python",
+    "stop_token": "cpp",
+    "cassert": "cpp",
+    "bit": "cpp",
+    "compare": "cpp",
+    "concepts": "cpp",
+    "numbers": "cpp",
+    "ranges": "cpp",
+    "span": "cpp"
+  },
+  "C_Cpp.errorSquiggles": "enabled",
+  "C_Cpp.default.includePath": [
+    "ut-robomaster/src",
+    "ut-robomaster/taproot/src",
+  ],
+  "C_Cpp.default.cppStandard": "c++20",
+  "C_Cpp.default.cStandard": "c11",
+  "[markdown]": {
+    "editor.wordWrap": "off",
+    "editor.quickSuggestions": {
+      "comments": "off",
+      "strings": "off",
+      "other": "off"
     },
-    "C_Cpp.errorSquiggles": "enabled",
-    "C_Cpp.default.includePath": [
-        "ut-robomaster/src",
-        "ut-robomaster/taproot/src",
-    ],
-    "C_Cpp.default.cppStandard": "c++20",
-    "C_Cpp.default.cStandard": "c11",
-    "[markdown]": {
-        "editor.wordWrap": "off",
-        "editor.quickSuggestions": {
-            "comments": "off",
-            "strings": "off",
-            "other": "off"
-        },
-        "editor.rulers": [
-            100
-        ]
-    }
+    "editor.rulers": [
+      100
+    ]
+  }
 }
diff --git a/ut-robomaster/src/board.hpp b/ut-robomaster/src/board.hpp
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "tap/board/board.hpp"
+
+namespace Board
+{
+using I2cSda = DigitalInPinPF0;
+using I2cScl = DigitalInPinPF1;
+using I2cMaster = I2cMaster2;
+
+inline void initialize_i2c()
+{
+    I2cMaster::connect<I2cSda::Sda, I2cScl::Scl>(I2cMaster::PullUps::Internal);
+    I2cMaster::initialize<SystemClock>();
+}
+}  // namespace Board
diff --git a/ut-robomaster/src/drivers.hpp b/ut-robomaster/src/drivers.hpp
@@ -1,29 +1,28 @@
-#ifndef DRIVERS_HPP_
-#define DRIVERS_HPP_
-
-#include "tap/drivers.hpp"
-
-#include "communication/cv_board.hpp"
-#include "utils/robot_comms.hpp"
-
-namespace src
-{
-class Drivers : public tap::Drivers
-{
-    friend class DriversSingleton;
-
-#ifdef ENV_UNIT_TESTS
-public:
-#endif
-    Drivers() : tap::Drivers(), cvBoard(this), terminal(this) {}
-
-public:
-    communication::CVBoard cvBoard;
-    comms::RobotComms terminal;
-
-    bool isKillSwitched() { return !remote.isConnected(); }
-};  // class Drivers
-
-}  // namespace src
-
-#endif  // DRIVERS_HPP_
+#ifndef DRIVERS_HPP_
+#define DRIVERS_HPP_
+
+#include "tap/drivers.hpp"
+
+#include "communication/cv_board.hpp"
+#include "utils/robot_comms.hpp"
+
+namespace src
+{
+class Drivers : public tap::Drivers
+{
+    friend class DriversSingleton;
+
+#ifdef ENV_UNIT_TESTS
+public:
+#endif
+    Drivers() : tap::Drivers(), cvBoard(this), terminal(this) {}
+
+public:
+    communication::CVBoard cvBoard;
+    comms::RobotComms terminal;
+    bool isKillSwitched() { return !remote.isConnected(); }
+};  // class Drivers
+
+}  // namespace src
+
+#endif  // DRIVERS_HPP_
diff --git a/ut-robomaster/src/drivers/as5600.hpp b/ut-robomaster/src/drivers/as5600.hpp
@@ -0,0 +1,60 @@
+#pragma once
+
+#include "modm/architecture/interface/i2c_device.hpp"
+#include "modm/processing/protothread/protothread.hpp"
+
+#include "encoder.hpp"
+
+namespace driver
+{
+template <class I2cMaster>
+class As5600 : public Encoder, public modm::I2cDevice<I2cMaster, 1>, public modm::pt::Protothread
+{
+public:
+    As5600() : modm::I2cDevice<I2cMaster, 1>(ADDRESS){};
+
+    void update() { run(); }
+
+    bool run()
+    {
+        PT_BEGIN();
+
+        while (true)
+        {
+            buffer[0] = uint8_t(Register::RAWANGLE);
+            PT_WAIT_UNTIL(this->startWriteRead(buffer, 1, buffer, 2));
+            PT_WAIT_WHILE(this->isTransactionRunning());
+
+            if (this->wasTransactionSuccessful())
+            {
+                angle = (buffer[0] << 8) | buffer[1];
+            }
+        }
+
+        PT_END();
+    }
+
+    float getAngle() override { return angle / 4096.0f; }
+
+protected:
+    enum class Register : uint8_t
+    {
+        ZPOS = 0x01,
+        MPOS = 0x03,
+        CONF = 0x07,
+
+        RAWANGLE = 0x0C,
+        ANGLE = 0x0E,
+
+        STATUS = 0x0B,
+        AGC = 0x1A,
+        MAG = 0x1B,
+    };
+
+private:
+    static const uint8_t ADDRESS = 0x36;
+    uint16_t angle = 0;
+    uint8_t buffer[2];
+};
+
+};  // namespace driver
diff --git a/ut-robomaster/src/drivers/encoder.hpp b/ut-robomaster/src/drivers/encoder.hpp
@@ -0,0 +1,14 @@
+#pragma once
+
+namespace driver
+{
+class Encoder
+{
+public:
+    virtual ~Encoder() {}
+
+    /// @brief Get current measured angle of the encoder
+    /// @return Angle (revs)
+    virtual float getAngle() = 0;
+};
+}  // namespace driver
diff --git a/ut-robomaster/src/drivers/encoder_test.hpp b/ut-robomaster/src/drivers/encoder_test.hpp
@@ -0,0 +1,3 @@
+
+void populate_frequencies(void);
+int func();