diff --git a/config/template.json b/config/template.json index deace561..0f83b71a 100644 --- a/config/template.json +++ b/config/template.json @@ -6,11 +6,16 @@ "pins": [] }, "can": { - "enabled": true, + "enabled": false, "buses": [ "can1" ] }, + "gpio": { + "enabled": false, + "read_pins": [], + "write_pins": [] + }, "i2c": { "enabled": false, "buses": [] @@ -39,13 +44,23 @@ "enabled": false, "bus": 2, "device_address": 56 + }, + "pressure": { + "enabled": false, + "pin": 0 } }, "motors": { "motor_controller": { - "enabled": true, + "enabled": false, "bus": "can1" } + }, + "active_suspension": { + "enabled": false, + "pressure_sensor_pin": 0, + "lower_pressure_pin": 0, + "raise_pressure_pin": 0 } } } \ No newline at end of file diff --git a/lib/debug/repl.cpp b/lib/debug/repl.cpp index bfe780cc..a3e71a8d 100644 --- a/lib/debug/repl.cpp +++ b/lib/debug/repl.cpp @@ -11,7 +11,14 @@ namespace hyped::debug { -Repl::Repl(core::ILogger &logger) : logger_(logger), i2c_(), spi_(), pwm_(), adc_(), uart_() +Repl::Repl(core::ILogger &logger) + : logger_(logger), + i2c_(), + spi_(), + pwm_(), + adc_(), + uart_(), + gpio_(io::HardwareGpio(logger)) { } @@ -117,6 +124,36 @@ std::optional> Repl::fromFile(const std::string &path) return std::nullopt; } } + if (!io.HasMember("gpio")) { + logger_.log(core::LogLevel::kFatal, "Missing required field 'io.gpio' in configuration file"); + return std::nullopt; + } + const auto gpio = io["gpio"].GetObject(); + if (!gpio.HasMember("enabled")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'io.gpio.enabled' in configuration file"); + return std::nullopt; + } + if (gpio["enabled"].GetBool()) { + if (!gpio.HasMember("read_pins")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'io.gpio.read_pins' in configuration file"); + return std::nullopt; + } + const auto read_pins = gpio["read_pins"].GetArray(); + for (auto &pin : read_pins) { + repl->addGpioReadCommands(pin.GetUint()); + } + if (!gpio.HasMember("write_pins")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'io.gpio.write_pins' in configuration file"); + return std::nullopt; + } + const auto write_pins = gpio["write_pins"].GetArray(); + for (auto &pin : write_pins) { + repl->addGpioWriteCommands(pin.GetUint()); + } + } const auto buses = i2c["buses"].GetArray(); for (auto &bus : buses) { repl->addI2cCommands(bus.GetUint()); @@ -257,6 +294,26 @@ std::optional> Repl::fromFile(const std::string &path) const auto bus = temperature["bus"].GetUint(); repl->addTemperatureCommands(bus, device_address); } + if (!sensors.HasMember("pressure")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'sensors.pressure' in configuration file"); + return std::nullopt; + } + const auto pressure = sensors["pressure"].GetObject(); + if (!pressure.HasMember("enabled")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'sensors.pressure.enabled' in configuration file"); + return std::nullopt; + } + if (pressure["enabled"].GetBool()) { + if (!pressure.HasMember("pin")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'sensors.pressure.pin' in configuration file"); + return std::nullopt; + } + const auto pin = pressure["pin"].GetUint(); + repl->addPressureCommands(pin); + } if (!debugger.HasMember("motors")) { logger_.log(core::LogLevel::kFatal, "Missing required field 'debugger.motors' in configuration file"); @@ -283,6 +340,41 @@ std::optional> Repl::fromFile(const std::string &path) const auto bus = motor_controller["bus"].GetString(); repl->addMotorControllerCommands(bus); } + if (!debugger.HasMember("active_suspension")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'debugger.active_suspension' in configuration file"); + return std::nullopt; + } + const auto active_suspension = debugger["active_suspension"].GetObject(); + if (!active_suspension.HasMember("enabled")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'active_suspension.enabled' in configuration file"); + return std::nullopt; + } + if (active_suspension["enabled"].GetBool()) { + if (!active_suspension.HasMember("pressure_sensor_pin")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'active_suspension.pressure_sensor_pin' in " + "configuration file"); + return std::nullopt; + } + if (!active_suspension.HasMember("lower_pressure_pin")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'active_suspension.lower_pressure_pin' in " + "configuration file"); + return std::nullopt; + } + if (!active_suspension.HasMember("raise_pressure_pin")) { + logger_.log(core::LogLevel::kFatal, + "Missing required field 'active_suspension.raise_pressure_pin' in " + "configuration file"); + return std::nullopt; + } + const auto pressure_sensor_pin = active_suspension["pressure_sensor_pin"].GetUint(); + const auto lower_pressure_pin = active_suspension["lower_pressure_pin"].GetUint(); + const auto raise_pressure_pin = active_suspension["raise_pressure_pin"].GetUint(); + repl->addActiveSuspensionCommands(pressure_sensor_pin, lower_pressure_pin, raise_pressure_pin); + } return repl; } @@ -393,6 +485,74 @@ void Repl::addCanCommands(const std::string &bus) addCommand(can_write_command); } +void Repl::addGpioReadCommands(const std::uint8_t pin) +{ + const auto optional_gpio_reader = gpio_.getReader(pin); + if (!optional_gpio_reader) { + logger_.log(core::LogLevel::kFatal, "Failed to create GPIO reader on pin %d", pin); + return; + } + const auto gpio_reader = std::move(*optional_gpio_reader); + Command gpio_read_command; + std::stringstream identifier; + identifier << "gpio " << static_cast(pin) << " read"; + gpio_read_command.name = identifier.str(); + std::stringstream description; + description << "Read from GPIO pin " << static_cast(pin); + gpio_read_command.description = description.str(); + gpio_read_command.handler = [this, gpio_reader, pin]() { + const auto value = gpio_reader->read(); + if (!value) { + logger_.log(core::LogLevel::kFatal, "Failed to read from GPIO pin %d", pin); + return; + } + logger_.log( + core::LogLevel::kDebug, "GPIO value from pin %d: %d", pin, static_cast(*value)); + }; +} + +void Repl::addGpioWriteCommands(const std::uint8_t pin) +{ + const auto optional_gpio_writer = gpio_.getWriter(pin); + if (!optional_gpio_writer) { + logger_.log(core::LogLevel::kFatal, "Failed to create GPIO writer on pin %d", pin); + return; + } + const auto gpio_writer = std::move(*optional_gpio_writer); + Command gpio_write_command; + std::stringstream identifier; + identifier << "gpio " << static_cast(pin) << " write"; + gpio_write_command.name = identifier.str(); + std::stringstream description; + description << "Write to GPIO pin " << static_cast(pin); + gpio_write_command.description = description.str(); + gpio_write_command.handler = [this, gpio_writer, pin]() { + std::cout << "Enter GPIO value: "; + std::uint8_t value; + std::cin >> std::hex >> value; + std::cin.ignore(std::numeric_limits::max(), '\n'); + core::DigitalSignal signal; + switch (value) { + case 0: + signal = core::DigitalSignal::kLow; + break; + case 1: + signal = core::DigitalSignal::kHigh; + break; + default: + logger_.log(core::LogLevel::kFatal, "Invalid GPIO value: %d, must be 0 or 1", value); + break; + } + const auto result = gpio_writer->write(signal); + if (result == core::Result::kFailure) { + logger_.log(core::LogLevel::kFatal, "Failed to write to GPIO pin %d", pin); + return; + } + logger_.log(core::LogLevel::kDebug, "Wrote %d to GPIO pin %d", value, pin); + }; + addCommand(gpio_write_command); +} + void Repl::addI2cCommands(const std::uint8_t bus) { const auto optional_i2c = getI2c(bus); @@ -879,6 +1039,107 @@ void Repl::addMotorControllerCommands(const std::string &bus) addCommand(frequency_time_command); } +void Repl::addPressureCommands(const std::uint8_t pin) +{ + const auto optional_adc = getAdc(pin); + if (!optional_adc) { + logger_.log(core::LogLevel::kFatal, "Failed to create adc instance"); + return; + } + const auto adc = std::move(*optional_adc); + const auto pressure_sensor = std::make_shared(logger_, adc); + Command pressure_sensor_read_command; + pressure_sensor_read_command.name = "pressure sensor read"; + pressure_sensor_read_command.description = "Read the pressure sensor"; + pressure_sensor_read_command.handler = [this, pressure_sensor]() { + const auto pressure = pressure_sensor->read(); + if (!pressure) { + logger_.log(core::LogLevel::kFatal, "Failed to read pressure sensor"); + return; + } + logger_.log(core::LogLevel::kInfo, "Pressure: %f", *pressure); + }; + addCommand(pressure_sensor_read_command); +} + +void Repl::addActiveSuspensionCommands(const std::uint8_t adc_pin, + const std::uint8_t lower_pressure_pin, + const std::uint8_t raise_pressure_pin) +{ + const auto optional_adc = getAdc(adc_pin); + if (!optional_adc) { + logger_.log(core::LogLevel::kFatal, "Failed to create adc instance"); + return; + } + const auto adc = std::move(*optional_adc); + const auto optional_raise_pressure_gpio = gpio_.getWriter(raise_pressure_pin); + if (!optional_raise_pressure_gpio) { + logger_.log(core::LogLevel::kFatal, + "Failed to create gpio writer for raise pressure pin %d", + raise_pressure_pin); + return; + } + const auto raise_pressure_gpio = std::move(*optional_raise_pressure_gpio); + const auto optional_lower_pressure_gpio = gpio_.getWriter(lower_pressure_pin); + if (!optional_lower_pressure_gpio) { + logger_.log(core::LogLevel::kFatal, + "Failed to create gpio writer for lower pressure pin %d", + lower_pressure_pin); + return; + } + const auto lower_pressure_gpio = std::move(*optional_lower_pressure_gpio); + const auto pressure_sensor = std::make_shared(logger_, adc); + Command active_suspension_set_command; + active_suspension_set_command.name = "active suspension set"; + active_suspension_set_command.description = "Set the active suspension to specified pressure"; + active_suspension_set_command.handler + = [this, pressure_sensor, lower_pressure_gpio, raise_pressure_gpio]() { + std::cout << "Enter pressure to set (bar)" << std::endl; + core::Float pressure; + std::cin >> pressure; + std::cout << "Enter precision (%)" << std::endl; + core::Float precision; + std::cin >> precision; + std::cin.ignore(std::numeric_limits::max(), '\n'); + const core::Float lower_bound = pressure * (1 - precision / 100); + const core::Float upper_bound = pressure * (1 + precision / 100); + std::uint8_t in_range_count = 0; + while (1) { + const auto optional_current_pressure = pressure_sensor->read(); + if (!optional_current_pressure) { + logger_.log(core::LogLevel::kFatal, "Failed to read pressure sensor"); + return; + } + const core::Float current_pressure = *optional_current_pressure; + if (in_range_count == 100) { + logger_.log(core::LogLevel::kInfo, "Pressure set to %f", current_pressure); + return; + } + if (current_pressure >= lower_bound && current_pressure <= upper_bound) { + logger_.log( + core::LogLevel::kInfo, "Pressure in range, current pressure %f", current_pressure); + raise_pressure_gpio->write(core::DigitalSignal::kLow); + lower_pressure_gpio->write(core::DigitalSignal::kLow); + ++in_range_count; + continue; + } + in_range_count = 0; + if (current_pressure < lower_bound) { + logger_.log( + core::LogLevel::kInfo, "Lowering pressure, current pressure %f", current_pressure); + raise_pressure_gpio->write(core::DigitalSignal::kHigh); + lower_pressure_gpio->write(core::DigitalSignal::kLow); + } + if (current_pressure > upper_bound) { + logger_.log( + core::LogLevel::kInfo, "Raising pressure, current pressure %f", current_pressure); + raise_pressure_gpio->write(core::DigitalSignal::kLow); + lower_pressure_gpio->write(core::DigitalSignal::kHigh); + } + } + }; +} + 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..cb7be218 100644 --- a/lib/debug/repl.hpp +++ b/lib/debug/repl.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace hyped::debug { @@ -49,6 +50,8 @@ class Repl { void addHelpCommand(); void addAdcCommands(const std::uint8_t pin); void addCanCommands(const std::string &bus); + void addGpioReadCommands(const std::uint8_t pin); + void addGpioWriteCommands(const std::uint8_t pin); void addI2cCommands(const std::uint8_t bus); void addPwmCommands(const std::uint8_t module, const std::uint32_t period); void addSpiCommands(const std::uint8_t bus); @@ -56,6 +59,10 @@ class Repl { void addTemperatureCommands(const std::uint8_t bus, const std::uint8_t device_address); void addUartCommands(const std::uint8_t bus); void addMotorControllerCommands(const std::string &bus); + void addPressureCommands(const std::uint8_t pin); + void addActiveSuspensionCommands(const std::uint8_t pressure_sensor_pin, + const std::uint8_t lower_pressure_pin, + const std::uint8_t raise_pressure_pin); /** * @brief Get the Adc object associated with the given pin or create a new one if it doesn't exist @@ -125,8 +132,9 @@ class Repl { core::ILogger &logger_; std::map command_map_; - std::unordered_map> can_; std::unordered_map> adc_; + std::unordered_map> can_; + io::HardwareGpio gpio_; std::unordered_map> i2c_; std::unordered_map> pwm_; std::unordered_map> spi_; diff --git a/lib/sensors/pressure.cpp b/lib/sensors/pressure.cpp new file mode 100644 index 00000000..c504f33f --- /dev/null +++ b/lib/sensors/pressure.cpp @@ -0,0 +1,23 @@ +#include + +namespace hyped::sensors { + +Pressure::Pressure(core::ILogger &logger, std::shared_ptr adc) + : logger_(logger), + adc_(adc) +{ +} + +std::optional Pressure::read() +{ + const auto pressure = adc_->readValue(); + if (!pressure) { + logger_.log(core::LogLevel::kFatal, "Failed to read pressure from ADC"); + return std::nullopt; + } + logger_.log(core::LogLevel::kDebug, "Successfully read pressure from ADC"); + // Equation determined from testing + return 6.944 * (*pressure) - 2.5; +} + +} // namespace hyped::sensors \ No newline at end of file diff --git a/lib/sensors/pressure.hpp b/lib/sensors/pressure.hpp new file mode 100644 index 00000000..857dbf25 --- /dev/null +++ b/lib/sensors/pressure.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace hyped::sensors { +class Pressure { + public: + Pressure(core::ILogger &logger, std::shared_ptr adc); + + std::optional read(); + + private: + core::ILogger &logger_; + std::shared_ptr adc_; +}; + +} // namespace hyped::sensors \ No newline at end of file