diff --git a/config/template.json b/config/template.json index deace561..9d545eab 100644 --- a/config/template.json +++ b/config/template.json @@ -39,6 +39,10 @@ "enabled": false, "bus": 2, "device_address": 56 + }, + "wheel_encoders": { + "enabled": false, + "pin": 0 } }, "motors": { diff --git a/lib/debug/repl.cpp b/lib/debug/repl.cpp index bfe780cc..13653a0e 100644 --- a/lib/debug/repl.cpp +++ b/lib/debug/repl.cpp @@ -283,6 +283,26 @@ std::optional> 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(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> Repl::getAdc(const std::uint8_t bus) { const auto adc = adc_.find(bus); diff --git a/lib/debug/repl.hpp b/lib/debug/repl.hpp index c949308f..6a25eeda 100644 --- a/lib/debug/repl.hpp +++ b/lib/debug/repl.hpp @@ -25,6 +25,7 @@ #include #include #include +#include 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); /** diff --git a/lib/sensors/wheel_encoder.cpp b/lib/sensors/wheel_encoder.cpp new file mode 100644 index 00000000..38c57dd0 --- /dev/null +++ b/lib/sensors/wheel_encoder.cpp @@ -0,0 +1,36 @@ +#include "wheel_encoder.hpp" + +namespace hyped::sensors { + +WheelEncoder::WheelEncoder(core::ILogger &logger, std::shared_ptr 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 \ No newline at end of file diff --git a/lib/sensors/wheel_encoder.hpp b/lib/sensors/wheel_encoder.hpp new file mode 100644 index 00000000..188b54a6 --- /dev/null +++ b/lib/sensors/wheel_encoder.hpp @@ -0,0 +1,24 @@ +#pragma once +#include +#include + +#include +#include +#include + +namespace hyped::sensors { +class WheelEncoder { + public: + WheelEncoder(core::ILogger &logger, std::shared_ptr adc); + std::uint64_t getCount(); + core::Result updateCount(); + void resetCount(); + + private: + static constexpr core::Float kVoltageThreshold = 1.7; + core::ILogger &logger_; + std::shared_ptr adc_; + std::uint64_t count_; + core::Float previous_voltage_; +}; +} // namespace hyped::sensors \ No newline at end of file