Add a service for talking to an ExpansionHub (#9)

* Initial ExpansionHub support

* Tuning

* Unroll loops so things update as fast as possible

* Major cleanup

* Add enablement support

* Add motor currents, refactor how NT happens

* Updates to enable reading before doing commands

* Add encoder resetting, and java impl

* Add PID modes

* Only send not often updating commands on reset

* 10ms loop time

* Fix PID

* Fix voltage

* Major refactor to clean up how everything works

* A bit more formatting

* Add python

* Fix pythong

* Another python fix

Author: ThadHouse

.vscode/settings.json

@@ -100,6 +100,20 @@
         "xtree": "cpp",
         "xutility": "cpp",
         "*.tcc": "cpp",
-        "string_view": "cpp"
+        "string_view": "cpp",
+        "codecvt": "cpp",
+        "module.h": "c",
+        "serial.h": "c",
+        "termios.h": "c",
+        "__bit_reference": "cpp",
+        "__config": "cpp",
+        "__hash_table": "cpp",
+        "__locale": "cpp",
+        "__node_handle": "cpp",
+        "__split_buffer": "cpp",
+        "__threading_support": "cpp",
+        "__tree": "cpp",
+        "__verbose_abort": "cpp",
+        "execution": "cpp"
     }
 }

CMakeLists.txt

@@ -29,7 +29,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 option(WITH_NTCORE "Include ntcore" ON)
 option(WITH_JAVA "Include java and JNI in the build" OFF)
 option(WITH_CSCORE "Build cscore (needs OpenCV)" OFF)
-option(WITH_WPIMATH "Build wpimath" OFF)
+option(WITH_WPIMATH "Build wpimath" ON)
 option(WITH_WPILIB "Build hal, wpilibc/j, and myRobot (needs OpenCV)" OFF)
 option(WITH_OLD_COMMANDS "Build old commands" OFF)
 option(WITH_EXAMPLES "Build examples" OFF)
@@ -94,11 +94,12 @@ add_subdirectory(radio)
 
 if (MRC_BUILD)
 add_subdirectory(powerdistribution)
+add_subdirectory(expansionhub)
 endif()
 
 if (MRC_BUILD)
 install(
-    TARGETS RadioDaemon PowerDistributionDaemon
+    TARGETS RadioDaemon PowerDistributionDaemon ExpansionHubDaemon
     RUNTIME DESTINATION bin
     LIBRARY DESTINATION lib
     ARCHIVE DESTINATION lib

expansionhub/CMakeLists.txt

@@ -0,0 +1,25 @@
+project(ExpansionHubDaemon)
+
+set(CMAKE_EXE_LINKER_FLAGS "-Wl,--allow-shlib-undefined")
+
+add_executable(ExpansionHubDaemon
+    main.cpp
+    PidConstants.cpp
+    SerialPort.cpp
+    EnabledState.cpp
+    MotorNtState.cpp
+    ServoNtState.cpp
+    ExpansionHubNtState.cpp
+    ExpansionHubSerial.cpp
+    SystemDUsbMonitor.cpp
+)
+target_compile_features(ExpansionHubDaemon PRIVATE cxx_std_20)
+target_compile_features(ExpansionHubDaemon PRIVATE c_std_17)
+wpilib_target_warnings(ExpansionHubDaemon)
+target_link_libraries(ExpansionHubDaemon PRIVATE Common wpimath ntcore wpinet wpiutil)
+target_link_libraries(ExpansionHubDaemon PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/libsystemd.so.0.37.0)
+target_include_directories(ExpansionHubDaemon PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/rhsp/include)
+
+if (MRC_BUILD)
+target_compile_definitions(ExpansionHubDaemon PRIVATE MRC_DAEMON_BUILD)
+endif()

expansionhub/CachedCommand.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include <optional>
+
+namespace eh {
+
+template <typename T>
+struct CachedCommand {
+    T subscriber;
+    std::optional<typename T::ValueType> lastAckedValue;
+    T::ValueType lastAttemptedValue;
+
+    CachedCommand() = default;
+
+    CachedCommand(const CachedCommand&) = delete;
+    CachedCommand& operator=(const CachedCommand&) = delete;
+    CachedCommand(CachedCommand&&) = delete;
+    CachedCommand& operator=(CachedCommand&&) = delete;
+
+    CachedCommand& operator=(T sub) {
+        subscriber = std::move(sub);
+        return *this;
+    }
+
+    void ForceReset() { lastAckedValue.reset(); }
+
+    void Ack() { lastAckedValue = lastAttemptedValue; }
+
+    std::optional<typename T::ValueType> Get() {
+        auto newValue = subscriber.Get();
+
+        if (newValue != lastAckedValue) {
+            lastAckedValue.reset();
+            lastAttemptedValue = newValue;
+            return newValue;
+        }
+
+        return std::nullopt;
+    }
+
+    std::optional<typename T::ValueType> GetWithCanEnable(bool canEnable) {
+        auto newValue = canEnable ? subscriber.Get() : false;
+
+        if (newValue != lastAckedValue) {
+            lastAckedValue.reset();
+            lastAttemptedValue = newValue;
+            return newValue;
+        }
+
+        return std::nullopt;
+    }
+};
+}  // namespace eh

expansionhub/EnabledState.cpp

@@ -0,0 +1,50 @@
+#include "EnabledState.h"
+
+#include <fcntl.h>
+#include <stdint.h>
+#include <thread>
+#include <chrono>
+#include <string.h>
+
+#define CONTROL_DATA_PATH "/sys/kernel/can_heartbeat/controldataro"
+
+namespace eh {
+
+EnabledState::~EnabledState() {
+    if (fd == -1) {
+        return;
+    }
+    close(fd);
+}
+
+bool EnabledState::Initialize() {
+    int retries = 0;
+    while (fd == -1 && retries < 50) {
+        fd = open(CONTROL_DATA_PATH, O_RDONLY);
+        if (fd == -1) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(250));
+            retries++;
+        }
+    }
+
+    return fd != -1;
+}
+
+bool EnabledState::IsEnabled() const {
+    if (fd == -1) {
+        return false;
+    }
+
+    char buf[128];
+
+    ssize_t control_data_size = pread(fd, buf, sizeof(buf), 0);
+
+    buf[control_data_size] = '\0';
+
+    unsigned int control_data = strtol(buf, NULL, 16);
+
+    bool system_watchdog = (control_data & 0x1) != 0 ? true : false;
+
+    return system_watchdog;
+}
+}  // namespace eh

expansionhub/EnabledState.h

@@ -0,0 +1,22 @@
+#pragma once
+
+namespace eh {
+
+class EnabledState {
+   public:
+    EnabledState() = default;
+    ~EnabledState() noexcept;
+
+    bool Initialize();
+
+    bool IsEnabled() const;
+
+    EnabledState(const EnabledState&) = delete;
+    EnabledState(EnabledState&&) = delete;
+    EnabledState& operator=(const EnabledState&) = delete;
+    EnabledState& operator=(EnabledState&&) = delete;
+
+   private:
+    int fd = -1;
+};
+}  // namespace eh

expansionhub/ExpansionHub.service

@@ -0,0 +1,12 @@
+[Unit]
+Description=ExpansionHub
+After=network.target
+
+[Service]
+Type=simple
+ExecStart=/usr/bin/ExpansionHubDaemon
+Restart=always
+RestartSec=3
+
+[Install]
+WantedBy=multi-user.target

expansionhub/ExpansionHubNtState.cpp

@@ -0,0 +1,50 @@
+#include "ExpansionHubNtState.h"
+
+#include "networktables/NetworkTableInstance.h"
+
+using namespace eh;
+
+void ExpansionHubNtState::Initialize(const nt::NetworkTableInstance& instance,
+                                    int deviceNum) {
+    if (isConnectedPublisher) {
+        return;
+    }
+
+    nt::PubSubOptions options;
+    options.sendAll = true;
+    options.keepDuplicates = true;
+    options.periodic = 0.005;
+
+    auto busIdStr = std::to_string(deviceNum);
+
+    for (int i = 0; i < static_cast<int>(motors.size()); i++) {
+        motors[i].Initialize(instance, i, busIdStr, options);
+    }
+
+    for (int i = 0; i < static_cast<int>(servos.size()); i++) {
+        servos[i].Initialize(instance, i, busIdStr, options);
+    }
+
+    transactionTimePublisher =
+        instance.GetIntegerTopic("/rhsp/" + busIdStr + "/transactionTime")
+            .Publish(options);
+
+    batteryVoltagePublisher =
+        instance.GetDoubleTopic("/rhsp/" + busIdStr + "/battery")
+            .Publish(options);
+    isConnectedPublisher =
+        instance.GetBooleanTopic("/rhsp/" + busIdStr + "/connected")
+            .Publish(options);
+
+    numCrcFailuresPublisher =
+        instance.GetIntegerTopic("/rhsp/" + busIdStr + "/numCrcFailures")
+            .Publish(options);
+
+    numMissedSendLoopsPublisher =
+        instance.GetIntegerTopic("/rhsp/" + busIdStr + "/numMissedSendLoops")
+            .Publish(options);
+
+    numNacksPublisher =
+        instance.GetIntegerTopic("/rhsp/" + busIdStr + "/numNacks")
+            .Publish(options);
+}

expansionhub/ExpansionHubNtState.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <array>
+#include "MotorNtState.h"
+#include "ServoNtState.h"
+
+#define NUM_MOTORS_PER_HUB 4
+#define NUM_SERVOS_PER_HUB 6
+
+namespace eh {
+
+struct ExpansionHubNtState {
+    std::array<MotorNtState, NUM_MOTORS_PER_HUB> motors;
+    std::array<ServoNtState, NUM_SERVOS_PER_HUB> servos;
+
+    double lastBattery{0};
+
+    nt::DoublePublisher batteryVoltagePublisher;
+
+    nt::BooleanPublisher isConnectedPublisher;
+
+    nt::IntegerPublisher numNacksPublisher;
+    nt::IntegerPublisher numCrcFailuresPublisher;
+    nt::IntegerPublisher numMissedSendLoopsPublisher;
+
+    nt::IntegerPublisher transactionTimePublisher;
+
+    uint64_t numNacks{0};
+    uint64_t numCrcFailures{0};
+    uint64_t numMissedSendLoops{0};
+
+    void Initialize(const nt::NetworkTableInstance& instance, int deviceNum);
+};
+
+}  // namespace eh