SNS - Wheel Encoder

config/template.json

@@ -39,6 +39,10 @@
 				"enabled": false,
 				"bus": 2,
 				"device_address": 56
+			},
+			"wheel_encoders": {
+				"enabled": false,
+				"pin": 0
 			}
 		},
 		"motors": {

lib/debug/repl.cpp

@@ -283,6 +283,26 @@ std::optional<std::unique_ptr<Repl>> Repl::fromFile(const std::string &path)
     const auto bus = motor_controller["bus"].GetString();
     repl->addMotorControllerCommands(bus);
   }
+  if (!sensors.HasMember("wheel_encoders")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'sensors.wheel_encoders' in configuration file");
+    return std::nullopt;
+  }
+  const auto wheel_encoders = sensors["wheel_encoders"].GetObject();
+  if (!wheel_encoders.HasMember("enabled")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'sensors.wheel_encoders.enabled' in configuration file");
+    return std::nullopt;
+  }
+  if (wheel_encoders["enabled"].GetBool()) {
+    if (!wheel_encoders.HasMember("pin")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'sensors.wheel_encoders.pin' in configuration file");
+      return std::nullopt;
+    }
+    const auto pin = wheel_encoders["pin"].GetUint();
+    repl->addWheelEncoderCommands(pin);
+  }
   return repl;
 }
 
@@ -879,6 +899,70 @@ void Repl::addMotorControllerCommands(const std::string &bus)
   addCommand(frequency_time_command);
 }
 
+void Repl::addWheelEncoderCommands(const std::uint8_t pin)
+{
+  const auto optional_adc = getAdc(pin);
+  if (!optional_adc) {
+    logger_.log(core::LogLevel::kFatal, "Failed to create ADC instance on pin %d", pin);
+    return;
+  }
+  const auto adc           = std::move(*optional_adc);
+  const auto wheel_encoder = std::make_shared<sensors::WheelEncoder>(logger_, adc);
+  {
+    Command wheel_encoder_read_count_command;
+    std::stringstream identifier;
+    identifier << "wheel encoder count";
+    wheel_encoder_read_count_command.name = identifier.str();
+    std::stringstream description;
+    description << "Read count wheel encoder";
+    wheel_encoder_read_count_command.description = description.str();
+    wheel_encoder_read_count_command.handler     = [this, wheel_encoder]() {
+      const auto count = wheel_encoder->getCount();
+      logger_.log(core::LogLevel::kInfo, "Wheel encoder value: %d", count);
+    };
+    addCommand(wheel_encoder_read_count_command);
+  }
+  {
+    Command wheel_encoder_reset_count_command;
+    std::stringstream identifier;
+    identifier << "wheel encoder reset";
+    wheel_encoder_reset_count_command.name = identifier.str();
+    std::stringstream description;
+    description << "Reset wheel encoder count";
+    wheel_encoder_reset_count_command.description = description.str();
+    wheel_encoder_reset_count_command.handler     = [this, wheel_encoder]() {
+      wheel_encoder->resetCount();
+      logger_.log(core::LogLevel::kInfo, "Wheel encoder count reset");
+    };
+    addCommand(wheel_encoder_reset_count_command);
+  }
+  {
+    Command wheel_encoder_run_command;
+    std::stringstream identifier;
+    identifier << "wheel encoder run";
+    wheel_encoder_run_command.name = identifier.str();
+    std::stringstream description;
+    description << "Run wheel encoder";
+    wheel_encoder_run_command.description = description.str();
+    wheel_encoder_run_command.handler     = [this, wheel_encoder]() {
+      std::uint64_t previous_wheel_encoder_count_ = 0;
+      while (1) {
+        const auto result = wheel_encoder->updateCount();
+        if (result == core::Result::kFailure) {
+          logger_.log(core::LogLevel::kFatal, "Failed to update wheel encoder count");
+          break;
+        }
+        const std::uint64_t count = wheel_encoder->getCount();
+        if (count != previous_wheel_encoder_count_) {
+          logger_.log(core::LogLevel::kInfo, "Wheel encoder value: %jd", count);
+          previous_wheel_encoder_count_ = count;
+        }
+      }
+    };
+    addCommand(wheel_encoder_run_command);
+  }
+}
+
 std::optional<std::shared_ptr<io::IAdc>> Repl::getAdc(const std::uint8_t bus)
 {
   const auto adc = adc_.find(bus);

lib/debug/repl.hpp

@@ -25,6 +25,7 @@
 #include <motors/time_frequency_calculator.hpp>
 #include <sensors/accelerometer.hpp>
 #include <sensors/temperature.hpp>
+#include <sensors/wheel_encoder.hpp>
 
 namespace hyped::debug {
 
@@ -55,6 +56,7 @@ class Repl {
   void addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t device_address);
   void addTemperatureCommands(const std::uint8_t bus, const std::uint8_t device_address);
   void addUartCommands(const std::uint8_t bus);
+  void addWheelEncoderCommands(const std::uint8_t pin);
   void addMotorControllerCommands(const std::string &bus);
 
   /**

lib/sensors/wheel_encoder.cpp

@@ -0,0 +1,36 @@
+#include "wheel_encoder.hpp"
+
+namespace hyped::sensors {
+
+WheelEncoder::WheelEncoder(core::ILogger &logger, std::shared_ptr<io::IAdc> adc)
+    : logger_(logger),
+      adc_(adc),
+      count_(0),
+      previous_voltage_(0)
+{
+}
+
+std::uint64_t WheelEncoder::getCount()
+{
+  return count_;
+}
+
+core::Result WheelEncoder::updateCount()
+{
+  const auto optional_voltage = adc_->readValue();
+  if (!optional_voltage) {
+    logger_.log(core::LogLevel::kFatal, "Failed to read wheel encoder value from ADC");
+    return core::Result::kFailure;
+  }
+  const core::Float voltage = *optional_voltage;
+  if (voltage > kVoltageThreshold && previous_voltage_ < kVoltageThreshold) { count_++; }
+  previous_voltage_ = voltage;
+  return core::Result::kSuccess;
+}
+
+void WheelEncoder::resetCount()
+{
+  count_ = 0;
+}
+
+}  // namespace hyped::sensors

lib/sensors/wheel_encoder.hpp

@@ -0,0 +1,24 @@
+#pragma once
+#include <memory>
+#include <optional>
+
+#include <core/logger.hpp>
+#include <core/types.hpp>
+#include <io/adc.hpp>
+
+namespace hyped::sensors {
+class WheelEncoder {
+ public:
+  WheelEncoder(core::ILogger &logger, std::shared_ptr<io::IAdc> adc);
+  std::uint64_t getCount();
+  core::Result updateCount();
+  void resetCount();
+
+ private:
+  static constexpr core::Float kVoltageThreshold = 1.7;
+  core::ILogger &logger_;
+  std::shared_ptr<io::IAdc> adc_;
+  std::uint64_t count_;
+  core::Float previous_voltage_;
+};
+}  // namespace hyped::sensors