From 36f8f25677da3820917b19005db29a0d3b180d34 Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Thu, 5 Dec 2024 18:03:04 +0100 Subject: [PATCH 01/11] update: first version with module properties --- main/modules/can.cpp | 2 +- main/modules/core.cpp | 2 +- main/modules/input.cpp | 10 +++++++++- main/modules/module.cpp | 12 +++++++----- main/modules/module.h | 2 -- 5 files changed, 18 insertions(+), 10 deletions(-) diff --git a/main/modules/can.cpp b/main/modules/can.cpp index 4b64fa7..9a74e43 100644 --- a/main/modules/can.cpp +++ b/main/modules/can.cpp @@ -102,7 +102,7 @@ bool Can::receive() { message.data); } - if (this->output_on) { + if (this->properties.at("output_on")->boolean_value) { static char buffer[256]; int pos = csprintf(buffer, sizeof(buffer), "%s %03lx", this->name.c_str(), message.identifier); if (!(message.flags & TWAI_MSG_FLAG_RTR)) { diff --git a/main/modules/core.cpp b/main/modules/core.cpp index 0027f80..706d442 100644 --- a/main/modules/core.cpp +++ b/main/modules/core.cpp @@ -73,7 +73,7 @@ void Core::call(const std::string method_name, const std::vectoroutput_list.push_back({module, property_name, precision}); } } - this->output_on = true; + this->properties.at("output_on")->boolean_value = true; } else if (method_name == "startup_checksum") { uint16_t checksum = 0; for (char const &c : Storage::startup) { diff --git a/main/modules/input.cpp b/main/modules/input.cpp index aefc84c..d3d2dd3 100644 --- a/main/modules/input.cpp +++ b/main/modules/input.cpp @@ -14,7 +14,15 @@ const std::map Input::get_defaults() { } Input::Input(const std::string name) : Module(input, name) { - this->properties = Input::get_defaults(); + // Get default properties + const auto defaults = Input::get_defaults(); + + // Merge defaults into properties, without overwriting existing properties + for (const auto &[key, value] : defaults) { + if (!this->properties.count(key)) { + this->properties[key] = value; + } + } } void Input::step() { diff --git a/main/modules/module.cpp b/main/modules/module.cpp index 0d728c5..12d056e 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -33,6 +33,8 @@ #include Module::Module(const ModuleType type, const std::string name) : type(type), name(name) { + this->properties["output_on"] = std::make_shared(false); + this->properties["broadcast"] = std::make_shared(false); } void Module::Module::expect(const std::vector arguments, const int num, ...) { @@ -351,13 +353,13 @@ Module_ptr Module::create(const std::string type, } void Module::step() { - if (this->output_on) { + if (this->properties["output_on"]->boolean_value) { const std::string output = this->get_output(); if (!output.empty()) { echo("%s %s", this->name.c_str(), output.c_str()); } } - if (this->broadcast && !this->properties.empty()) { + if (this->properties["broadcast"]->boolean_value && !this->properties.empty()) { static char buffer[1024]; int pos = csprintf(buffer, sizeof(buffer), "!!"); for (auto const &[property_name, property] : this->properties) { @@ -372,13 +374,13 @@ void Module::step() { void Module::call(const std::string method_name, const std::vector arguments) { if (method_name == "mute") { Module::expect(arguments, 0); - this->output_on = false; + this->properties.at("output_on")->boolean_value = false; } else if (method_name == "unmute") { Module::expect(arguments, 0); - this->output_on = true; + this->properties.at("output_on")->boolean_value = true; } else if (method_name == "broadcast") { Module::expect(arguments, 0); - this->broadcast = true; + this->properties.at("broadcast")->boolean_value = true; } else if (method_name == "shadow") { Module::expect(arguments, 1, identifier); std::string target_name = arguments[0]->evaluate_identifier(); diff --git a/main/modules/module.h b/main/modules/module.h index 4147b87..1ebd24d 100644 --- a/main/modules/module.h +++ b/main/modules/module.h @@ -47,8 +47,6 @@ class Module { protected: std::map properties; - bool output_on = false; - bool broadcast = false; public: const ModuleType type; From 8ea2cb503576dc319d6a8307c25f40591bfdb834 Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Thu, 5 Dec 2024 18:12:50 +0100 Subject: [PATCH 02/11] update: smoother solution --- main/modules/input.cpp | 10 +--------- main/modules/module.cpp | 8 ++++++++ main/modules/module.h | 1 + 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/main/modules/input.cpp b/main/modules/input.cpp index d3d2dd3..ba33072 100644 --- a/main/modules/input.cpp +++ b/main/modules/input.cpp @@ -14,15 +14,7 @@ const std::map Input::get_defaults() { } Input::Input(const std::string name) : Module(input, name) { - // Get default properties - const auto defaults = Input::get_defaults(); - - // Merge defaults into properties, without overwriting existing properties - for (const auto &[key, value] : defaults) { - if (!this->properties.count(key)) { - this->properties[key] = value; - } - } + this->merge_properties(Input::get_defaults()); } void Input::step() { diff --git a/main/modules/module.cpp b/main/modules/module.cpp index 12d056e..e609ca5 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -477,3 +477,11 @@ const std::map Module::get_module_defaults(const std: throw std::runtime_error("module type \"" + type_name + "\" not found in defaults list"); } } + +void Module::merge_properties(const std::map &defaults) { + for (const auto &[key, value] : defaults) { + if (!this->properties.count(key)) { + this->properties[key] = value; + } + } +} diff --git a/main/modules/module.h b/main/modules/module.h index 1ebd24d..07cbd80 100644 --- a/main/modules/module.h +++ b/main/modules/module.h @@ -66,4 +66,5 @@ class Module { Variable_ptr get_property(const std::string property_name) const; virtual void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander = false); virtual void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data); + virtual void merge_properties(const std::map &defaults); }; From 2ab2a6e1741e962dbcdde6c739cac512d0a36846 Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Fri, 6 Dec 2024 13:07:29 +0100 Subject: [PATCH 03/11] added change to all modules --- main/modules/analog.cpp | 2 +- main/modules/bluetooth.cpp | 2 +- main/modules/can.cpp | 2 +- main/modules/canopen_master.cpp | 2 +- main/modules/canopen_motor.cpp | 2 +- main/modules/d1_motor.cpp | 2 +- main/modules/dunker_motor.cpp | 2 +- main/modules/dunker_wheels.cpp | 2 +- main/modules/expander.cpp | 2 +- main/modules/imu.cpp | 2 +- main/modules/linear_motor.cpp | 2 +- main/modules/mcp23017.cpp | 2 +- main/modules/motor_axis.cpp | 2 +- main/modules/odrive_motor.cpp | 2 +- main/modules/odrive_wheels.cpp | 2 +- main/modules/output.cpp | 2 +- main/modules/pwm_output.cpp | 2 +- main/modules/rmd_motor.cpp | 2 +- main/modules/rmd_pair.cpp | 2 +- main/modules/roboclaw.cpp | 2 +- main/modules/roboclaw_motor.cpp | 2 +- main/modules/roboclaw_wheels.cpp | 2 +- main/modules/serial.cpp | 2 +- main/modules/stepper_motor.cpp | 2 +- 24 files changed, 24 insertions(+), 24 deletions(-) diff --git a/main/modules/analog.cpp b/main/modules/analog.cpp index 89929aa..5bbe927 100644 --- a/main/modules/analog.cpp +++ b/main/modules/analog.cpp @@ -15,7 +15,7 @@ const std::map Analog::get_defaults() { Analog::Analog(const std::string name, uint8_t unit, uint8_t channel, float attenuation_level) : Module(analog, name), unit(unit), channel(channel) { - this->properties = Analog::get_defaults(); + this->merge_properties(Analog::get_defaults()); if (unit < 1 || unit > 2) { echo("error: invalid unit, using default 1"); diff --git a/main/modules/bluetooth.cpp b/main/modules/bluetooth.cpp index 93e8e29..5110993 100644 --- a/main/modules/bluetooth.cpp +++ b/main/modules/bluetooth.cpp @@ -15,7 +15,7 @@ Bluetooth::Bluetooth(const std::string name, const std::string device_name, Mess echo("error in bluetooth message handler: %s", e.what()); } }); - this->properties = Bluetooth::get_defaults(); + this->merge_properties(Bluetooth::get_defaults()); } void Bluetooth::call(const std::string method_name, const std::vector arguments) { diff --git a/main/modules/can.cpp b/main/modules/can.cpp index 9a74e43..f84d687 100644 --- a/main/modules/can.cpp +++ b/main/modules/can.cpp @@ -57,7 +57,7 @@ Can::Can(const std::string name, const gpio_num_t rx_pin, const gpio_num_t tx_pi g_config.rx_queue_len = 20; g_config.tx_queue_len = 20; - this->properties = Can::get_defaults(); + this->merge_properties(Can::get_defaults()); ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config, &f_config)); ESP_ERROR_CHECK(twai_start()); diff --git a/main/modules/canopen_master.cpp b/main/modules/canopen_master.cpp index 8bb8fc1..20a5fc5 100644 --- a/main/modules/canopen_master.cpp +++ b/main/modules/canopen_master.cpp @@ -8,7 +8,7 @@ const std::map CanOpenMaster::get_defaults() { CanOpenMaster::CanOpenMaster(const std::string &name, const Can_ptr can) : Module(canopen_master, name), can(can) { - this->properties = CanOpenMaster::get_defaults(); + this->merge_properties(CanOpenMaster::get_defaults()); } void CanOpenMaster::step() { diff --git a/main/modules/canopen_motor.cpp b/main/modules/canopen_motor.cpp index 45a7df0..8a28609 100644 --- a/main/modules/canopen_motor.cpp +++ b/main/modules/canopen_motor.cpp @@ -74,7 +74,7 @@ CanOpenMotor::CanOpenMotor(const std::string &name, Can_ptr can, int64_t node_id : Module(canopen_motor, name), can(can), node_id(check_node_id(node_id)), current_op_mode_disp(OP_MODE_NONE), current_op_mode(OP_MODE_NONE) { - this->properties = CanOpenMotor::get_defaults(); + this->merge_properties(CanOpenMotor::get_defaults()); } void CanOpenMotor::wait_for_sdo_writes(uint32_t timeout_ms) { diff --git a/main/modules/d1_motor.cpp b/main/modules/d1_motor.cpp index 0f4e184..90e1f19 100644 --- a/main/modules/d1_motor.cpp +++ b/main/modules/d1_motor.cpp @@ -21,7 +21,7 @@ const std::map D1Motor::get_defaults() { D1Motor::D1Motor(const std::string &name, Can_ptr can, int64_t node_id) : Module(d1_motor, name), can(can), node_id(check_node_id(node_id)) { - this->properties = D1Motor::get_defaults(); + this->merge_properties(D1Motor::get_defaults()); } void D1Motor::subscribe_to_can() { diff --git a/main/modules/dunker_motor.cpp b/main/modules/dunker_motor.cpp index b4670a2..80c874a 100644 --- a/main/modules/dunker_motor.cpp +++ b/main/modules/dunker_motor.cpp @@ -14,7 +14,7 @@ const std::map DunkerMotor::get_defaults() { DunkerMotor::DunkerMotor(const std::string &name, Can_ptr can, int64_t node_id) : Module(dunker_motor, name), can(can), node_id(check_node_id(node_id)) { - this->properties = DunkerMotor::get_defaults(); + this->merge_properties(DunkerMotor::get_defaults()); } void DunkerMotor::subscribe_to_can() { diff --git a/main/modules/dunker_wheels.cpp b/main/modules/dunker_wheels.cpp index 449e88a..7a52168 100644 --- a/main/modules/dunker_wheels.cpp +++ b/main/modules/dunker_wheels.cpp @@ -11,7 +11,7 @@ const std::map DunkerWheels::get_defaults() { DunkerWheels::DunkerWheels(const std::string name, const DunkerMotor_ptr left_motor, const DunkerMotor_ptr right_motor) : Module(dunker_wheels, name), left_motor(left_motor), right_motor(right_motor) { - this->properties = DunkerWheels::get_defaults(); + this->merge_properties(DunkerWheels::get_defaults()); } void DunkerWheels::step() { diff --git a/main/modules/expander.cpp b/main/modules/expander.cpp index e4a05e2..f3bcca9 100644 --- a/main/modules/expander.cpp +++ b/main/modules/expander.cpp @@ -30,7 +30,7 @@ Expander::Expander(const std::string name, enable_pin(enable_pin), message_handler(message_handler) { - this->properties = Expander::get_defaults(); + this->merge_properties(Expander::get_defaults()); this->serial->enable_line_detection(); if (boot_pin != GPIO_NUM_NC && enable_pin != GPIO_NUM_NC) { diff --git a/main/modules/imu.cpp b/main/modules/imu.cpp index ed89f08..dd77cae 100644 --- a/main/modules/imu.cpp +++ b/main/modules/imu.cpp @@ -52,7 +52,7 @@ Imu::Imu(const std::string name, i2c_port_t i2c_port, gpio_num_t sda_pin, gpio_n } catch (std::exception &ex) { throw std::runtime_error(std::string("imu setup failed: ") + ex.what()); } - this->properties = Imu::get_defaults(); + this->merge_properties(Imu::get_defaults()); } void Imu::step() { diff --git a/main/modules/linear_motor.cpp b/main/modules/linear_motor.cpp index 8ab355a..02fd2a7 100644 --- a/main/modules/linear_motor.cpp +++ b/main/modules/linear_motor.cpp @@ -9,7 +9,7 @@ const std::map LinearMotor::get_defaults() { } LinearMotor::LinearMotor(const std::string name) : Module(output, name) { - this->properties = LinearMotor::get_defaults(); + this->merge_properties(LinearMotor::get_defaults()); } void LinearMotor::step() { diff --git a/main/modules/mcp23017.cpp b/main/modules/mcp23017.cpp index 7adc013..eab4404 100644 --- a/main/modules/mcp23017.cpp +++ b/main/modules/mcp23017.cpp @@ -29,7 +29,7 @@ Mcp23017::Mcp23017(const std::string name, i2c_port_t i2c_port, gpio_num_t sda_p throw std::runtime_error("could not install i2c driver"); } - this->properties = Mcp23017::get_defaults(); + this->merge_properties(Mcp23017::get_defaults()); this->set_inputs(this->properties.at("inputs")->integer_value); this->set_pullups(this->properties.at("pullups")->integer_value); diff --git a/main/modules/motor_axis.cpp b/main/modules/motor_axis.cpp index f38ff6c..568b401 100644 --- a/main/modules/motor_axis.cpp +++ b/main/modules/motor_axis.cpp @@ -8,7 +8,7 @@ const std::map MotorAxis::get_defaults() { MotorAxis::MotorAxis(const std::string name, const Motor_ptr motor, const Input_ptr input1, const Input_ptr input2) : Module(motor_axis, name), motor(motor), input1(input1), input2(input2) { - this->properties = MotorAxis::get_defaults(); + this->merge_properties(MotorAxis::get_defaults()); } bool MotorAxis::can_move(const float speed) const { diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 2a0b75e..20a5641 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -17,7 +17,7 @@ const std::map ODriveMotor::get_defaults() { ODriveMotor::ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id, const uint32_t version) : Module(odrive_motor, name), can_id(can_id), can(can), version(version) { - this->properties = ODriveMotor::get_defaults(); + this->merge_properties(ODriveMotor::get_defaults()); } void ODriveMotor::subscribe_to_can() { diff --git a/main/modules/odrive_wheels.cpp b/main/modules/odrive_wheels.cpp index 880055a..371223b 100644 --- a/main/modules/odrive_wheels.cpp +++ b/main/modules/odrive_wheels.cpp @@ -13,7 +13,7 @@ const std::map ODriveWheels::get_defaults() { ODriveWheels::ODriveWheels(const std::string name, const ODriveMotor_ptr left_motor, const ODriveMotor_ptr right_motor) : Module(odrive_wheels, name), left_motor(left_motor), right_motor(right_motor) { - this->properties = ODriveWheels::get_defaults(); + this->merge_properties(ODriveWheels::get_defaults()); } void ODriveWheels::step() { diff --git a/main/modules/output.cpp b/main/modules/output.cpp index 7938b3a..2582773 100644 --- a/main/modules/output.cpp +++ b/main/modules/output.cpp @@ -11,7 +11,7 @@ const std::map Output::get_defaults() { } Output::Output(const std::string name) : Module(output, name) { - this->properties = Output::get_defaults(); + this->merge_properties(Output::get_defaults()); } void Output::step() { diff --git a/main/modules/pwm_output.cpp b/main/modules/pwm_output.cpp index dc7362a..b7a17e3 100644 --- a/main/modules/pwm_output.cpp +++ b/main/modules/pwm_output.cpp @@ -15,7 +15,7 @@ PwmOutput::PwmOutput(const std::string name, : Module(pwm_output, name), pin(pin), ledc_timer(ledc_timer), ledc_channel(ledc_channel) { gpio_reset_pin(pin); - this->properties = PwmOutput::get_defaults(); + this->merge_properties(PwmOutput::get_defaults()); ledc_timer_config_t timer_config = { .speed_mode = LEDC_HIGH_SPEED_MODE, diff --git a/main/modules/rmd_motor.cpp b/main/modules/rmd_motor.cpp index cc2e08a..a8fa628 100644 --- a/main/modules/rmd_motor.cpp +++ b/main/modules/rmd_motor.cpp @@ -18,7 +18,7 @@ const std::map RmdMotor::get_defaults() { RmdMotor::RmdMotor(const std::string name, const Can_ptr can, const uint8_t motor_id, const int ratio) : Module(rmd_motor, name), motor_id(motor_id), can(can), ratio(ratio), encoder_range(262144.0 / ratio) { - this->properties = RmdMotor::get_defaults(); + this->merge_properties(RmdMotor::get_defaults()); } void RmdMotor::subscribe_to_can() { diff --git a/main/modules/rmd_pair.cpp b/main/modules/rmd_pair.cpp index 1ee5999..73f60f7 100644 --- a/main/modules/rmd_pair.cpp +++ b/main/modules/rmd_pair.cpp @@ -12,7 +12,7 @@ const std::map RmdPair::get_defaults() { RmdPair::RmdPair(const std::string name, const RmdMotor_ptr rmd1, const RmdMotor_ptr rmd2) : Module(rmd_pair, name), rmd1(rmd1), rmd2(rmd2) { - this->properties = RmdPair::get_defaults(); + this->merge_properties(RmdPair::get_defaults()); } RmdPair::TrajectoryTriple RmdPair::compute_trajectory(double x0, double x1, double v0, double v1) const { diff --git a/main/modules/roboclaw.cpp b/main/modules/roboclaw.cpp index cdcc205..1fc10d0 100644 --- a/main/modules/roboclaw.cpp +++ b/main/modules/roboclaw.cpp @@ -13,7 +13,7 @@ const std::map RoboClaw::get_defaults() { RoboClaw::RoboClaw(const std::string name, const ConstSerial_ptr serial, const uint8_t address) : Module(roboclaw, name), address(address), serial(serial) { - this->properties = RoboClaw::get_defaults(); + this->merge_properties(RoboClaw::get_defaults()); } void RoboClaw::step() { diff --git a/main/modules/roboclaw_motor.cpp b/main/modules/roboclaw_motor.cpp index 3fac09e..70a25d2 100644 --- a/main/modules/roboclaw_motor.cpp +++ b/main/modules/roboclaw_motor.cpp @@ -15,7 +15,7 @@ RoboClawMotor::RoboClawMotor(const std::string name, const RoboClaw_ptr roboclaw if (this->motor_number != motor_number) { throw std::runtime_error("illegal motor number"); } - this->properties = RoboClawMotor::get_defaults(); + this->merge_properties(RoboClawMotor::get_defaults()); } void RoboClawMotor::step() { diff --git a/main/modules/roboclaw_wheels.cpp b/main/modules/roboclaw_wheels.cpp index 2936c6a..3eacddb 100644 --- a/main/modules/roboclaw_wheels.cpp +++ b/main/modules/roboclaw_wheels.cpp @@ -14,7 +14,7 @@ const std::map RoboClawWheels::get_defaults() { RoboClawWheels::RoboClawWheels(const std::string name, const RoboClawMotor_ptr left_motor, const RoboClawMotor_ptr right_motor) : Module(roboclaw_wheels, name), left_motor(left_motor), right_motor(right_motor) { - this->properties = RoboClawWheels::get_defaults(); + this->merge_properties(RoboClawWheels::get_defaults()); } /* Catch unsigned wrap-around by detecting large jumps in encoder deltas */ diff --git a/main/modules/serial.cpp b/main/modules/serial.cpp index b1f5652..c1f7d08 100644 --- a/main/modules/serial.cpp +++ b/main/modules/serial.cpp @@ -16,7 +16,7 @@ const std::map Serial::get_defaults() { Serial::Serial(const std::string name, const gpio_num_t rx_pin, const gpio_num_t tx_pin, const long baud_rate, const uart_port_t uart_num) : Module(serial, name), rx_pin(rx_pin), tx_pin(tx_pin), baud_rate(baud_rate), uart_num(uart_num) { - this->properties = Serial::get_defaults(); + this->merge_properties(Serial::get_defaults()); if (uart_is_driver_installed(uart_num)) { throw std::runtime_error("serial interface is already in use"); diff --git a/main/modules/stepper_motor.cpp b/main/modules/stepper_motor.cpp index 8f491d3..5a21d67 100644 --- a/main/modules/stepper_motor.cpp +++ b/main/modules/stepper_motor.cpp @@ -38,7 +38,7 @@ StepperMotor::StepperMotor(const std::string name, gpio_reset_pin(step_pin); gpio_reset_pin(dir_pin); - this->properties = StepperMotor::get_defaults(); + this->merge_properties(StepperMotor::get_defaults()); pcnt_config_t pcnt_config = { .pulse_gpio_num = step_pin, From 34bc3556a71a3b350bcaf092cafbf10b1e60cdd6 Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Fri, 6 Dec 2024 13:09:08 +0100 Subject: [PATCH 04/11] update: check not necessary -> no doubling in defaults --- main/modules/module.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/main/modules/module.cpp b/main/modules/module.cpp index e609ca5..7f7049f 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -480,8 +480,6 @@ const std::map Module::get_module_defaults(const std: void Module::merge_properties(const std::map &defaults) { for (const auto &[key, value] : defaults) { - if (!this->properties.count(key)) { - this->properties[key] = value; - } + this->properties[key] = value; } } From b8072b2674606466e46b0cb8095e99224817ed64 Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Fri, 6 Dec 2024 13:14:24 +0100 Subject: [PATCH 05/11] update: rename output_on -> muted and inverse logic --- main/modules/can.cpp | 2 +- main/modules/core.cpp | 2 +- main/modules/module.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/main/modules/can.cpp b/main/modules/can.cpp index f84d687..8c7ae0d 100644 --- a/main/modules/can.cpp +++ b/main/modules/can.cpp @@ -102,7 +102,7 @@ bool Can::receive() { message.data); } - if (this->properties.at("output_on")->boolean_value) { + if (!this->properties.at("muted")->boolean_value) { static char buffer[256]; int pos = csprintf(buffer, sizeof(buffer), "%s %03lx", this->name.c_str(), message.identifier); if (!(message.flags & TWAI_MSG_FLAG_RTR)) { diff --git a/main/modules/core.cpp b/main/modules/core.cpp index 706d442..ab645c0 100644 --- a/main/modules/core.cpp +++ b/main/modules/core.cpp @@ -73,7 +73,7 @@ void Core::call(const std::string method_name, const std::vectoroutput_list.push_back({module, property_name, precision}); } } - this->properties.at("output_on")->boolean_value = true; + this->properties.at("muted")->boolean_value = false; } else if (method_name == "startup_checksum") { uint16_t checksum = 0; for (char const &c : Storage::startup) { diff --git a/main/modules/module.cpp b/main/modules/module.cpp index 7f7049f..bcdfb20 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -33,7 +33,7 @@ #include Module::Module(const ModuleType type, const std::string name) : type(type), name(name) { - this->properties["output_on"] = std::make_shared(false); + this->properties["muted"] = std::make_shared(true); this->properties["broadcast"] = std::make_shared(false); } @@ -353,7 +353,7 @@ Module_ptr Module::create(const std::string type, } void Module::step() { - if (this->properties["output_on"]->boolean_value) { + if (!this->properties["muted"]->boolean_value) { const std::string output = this->get_output(); if (!output.empty()) { echo("%s %s", this->name.c_str(), output.c_str()); @@ -374,10 +374,10 @@ void Module::step() { void Module::call(const std::string method_name, const std::vector arguments) { if (method_name == "mute") { Module::expect(arguments, 0); - this->properties.at("output_on")->boolean_value = false; + this->properties.at("muted")->boolean_value = true; } else if (method_name == "unmute") { Module::expect(arguments, 0); - this->properties.at("output_on")->boolean_value = true; + this->properties.at("muted")->boolean_value = false; } else if (method_name == "broadcast") { Module::expect(arguments, 0); this->properties.at("broadcast")->boolean_value = true; From 686c10800ba5b2b79e7082ef10426aaa19c33aca Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Fri, 6 Dec 2024 16:14:30 +0100 Subject: [PATCH 06/11] uppdate: changed init style to match other modules, also to be mergeable in proxy --- main/modules/module.cpp | 10 ++++++++-- main/modules/module.h | 1 + 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/main/modules/module.cpp b/main/modules/module.cpp index bcdfb20..6f40a85 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -32,9 +32,15 @@ #include "stepper_motor.h" #include +const std::map Module::get_defaults() { + return { + {"muted", std::make_shared(true)}, + {"broadcast", std::make_shared(false)}, + }; +} + Module::Module(const ModuleType type, const std::string name) : type(type), name(name) { - this->properties["muted"] = std::make_shared(true); - this->properties["broadcast"] = std::make_shared(false); + this->merge_properties(Module::get_defaults()); } void Module::Module::expect(const std::vector arguments, const int num, ...) { diff --git a/main/modules/module.h b/main/modules/module.h index 07cbd80..f9a2b0f 100644 --- a/main/modules/module.h +++ b/main/modules/module.h @@ -60,6 +60,7 @@ class Module { MessageHandler message_handler); virtual void step(); virtual void call(const std::string method_name, const std::vector arguments); + static const std::map get_defaults(); static const std::map get_module_defaults(const std::string &type_name); void call_with_shadows(const std::string method_name, const std::vector arguments); virtual std::string get_output() const; From 5cf61b422a3b2ce752434e37e8e7bd3d8c252778 Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Fri, 6 Dec 2024 16:14:56 +0100 Subject: [PATCH 07/11] update: disable step & load Module defaults as well --- main/modules/proxy.cpp | 8 +++++++- main/modules/proxy.h | 1 + 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/main/modules/proxy.cpp b/main/modules/proxy.cpp index d249528..f7eeb13 100644 --- a/main/modules/proxy.cpp +++ b/main/modules/proxy.cpp @@ -10,7 +10,8 @@ Proxy::Proxy(const std::string name, const Expander_ptr expander, const std::vector arguments) : Module(proxy, name), expander(expander) { - this->properties = Module::get_module_defaults(module_type); + this->merge_properties(Module::get_defaults()); + this->merge_properties(Module::get_module_defaults(module_type)); this->properties["is_ready"] = expander->get_property("is_ready"); expander->add_proxy(name, module_type, arguments); } @@ -33,3 +34,8 @@ void Proxy::write_property(const std::string property_name, const ConstExpressio } Module::get_property(property_name)->assign(expression); } + +void Proxy::step() { + // proxies should not broadcast or be muted themselves + return; +} diff --git a/main/modules/proxy.h b/main/modules/proxy.h index 0ff90b5..a57f5e2 100644 --- a/main/modules/proxy.h +++ b/main/modules/proxy.h @@ -15,4 +15,5 @@ class Proxy : public Module { const std::vector arguments); void call(const std::string method_name, const std::vector arguments) override; void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; + void step() override; }; From 44f3007df94cd9f2bbee4752169543f5d5ca424b Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Fri, 6 Dec 2024 16:28:04 +0100 Subject: [PATCH 08/11] update: warning for double defaults --- main/modules/module.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/main/modules/module.cpp b/main/modules/module.cpp index 6f40a85..9b537a8 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -486,6 +486,10 @@ const std::map Module::get_module_defaults(const std: void Module::merge_properties(const std::map &defaults) { for (const auto &[key, value] : defaults) { + if (this->properties.count(key) > 0) { + echo("Warning: Module %s: Property '%s' already exists, skipping", this->name.c_str(), key.c_str()); + continue; + } this->properties[key] = value; } } From 5f96d2edccfec43619bca868a1accd71c73b12ff Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Fri, 6 Dec 2024 16:28:22 +0100 Subject: [PATCH 09/11] update: removed double call --- main/modules/proxy.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/main/modules/proxy.cpp b/main/modules/proxy.cpp index f7eeb13..96b753f 100644 --- a/main/modules/proxy.cpp +++ b/main/modules/proxy.cpp @@ -10,7 +10,6 @@ Proxy::Proxy(const std::string name, const Expander_ptr expander, const std::vector arguments) : Module(proxy, name), expander(expander) { - this->merge_properties(Module::get_defaults()); this->merge_properties(Module::get_module_defaults(module_type)); this->properties["is_ready"] = expander->get_property("is_ready"); expander->add_proxy(name, module_type, arguments); From 317b10ea10684f465eb6662bf884fea0f95ac8f5 Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Fri, 6 Dec 2024 16:36:05 +0100 Subject: [PATCH 10/11] filter for broadcast & muted. Their state matches anyway and they are not critical for broadcast iteself --- main/modules/module.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/main/modules/module.cpp b/main/modules/module.cpp index 9b537a8..34a8438 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -369,6 +369,9 @@ void Module::step() { static char buffer[1024]; int pos = csprintf(buffer, sizeof(buffer), "!!"); for (auto const &[property_name, property] : this->properties) { + if (property_name == "broadcast" || property_name == "muted") { + continue; + } pos += csprintf(&buffer[pos], sizeof(buffer) - pos, "%s.%s=", this->name.c_str(), property_name.c_str()); pos += property->print_to_buffer(&buffer[pos], sizeof(buffer) - pos); pos += csprintf(&buffer[pos], sizeof(buffer) - pos, ";"); From 5ef4cf63a69d8eb037e0897ace0279e2063d9c07 Mon Sep 17 00:00:00 2001 From: Jens Ogorek Date: Mon, 9 Dec 2024 16:07:22 +0100 Subject: [PATCH 11/11] added calls to properties --- main/modules/can.cpp | 23 ++++++++++++++ main/modules/can.h | 1 + main/modules/canopen_motor.cpp | 51 ++++++++++++++++++++++++++++++++ main/modules/canopen_motor.h | 1 + main/modules/d1_motor.cpp | 20 +++++++++++++ main/modules/d1_motor.h | 1 + main/modules/dunker_motor.cpp | 17 +++++++++++ main/modules/dunker_motor.h | 1 + main/modules/input.cpp | 17 +++++++++++ main/modules/input.h | 1 + main/modules/linear_motor.cpp | 21 +++++++++++++ main/modules/linear_motor.h | 1 + main/modules/mcp23017.cpp | 18 +++++++++++ main/modules/mcp23017.h | 1 + main/modules/motor_axis.cpp | 15 ++++++++++ main/modules/motor_axis.h | 1 + main/modules/odrive_motor.cpp | 24 +++++++++++++++ main/modules/odrive_motor.h | 1 + main/modules/odrive_wheels.cpp | 11 +++++++ main/modules/odrive_wheels.h | 1 + main/modules/output.cpp | 20 +++++++++++++ main/modules/output.h | 1 + main/modules/pwm_output.cpp | 8 +++++ main/modules/pwm_output.h | 1 + main/modules/rmd_motor.cpp | 23 ++++++++++++++ main/modules/rmd_motor.h | 1 + main/modules/rmd_pair.cpp | 18 +++++++++++ main/modules/rmd_pair.h | 1 + main/modules/roboclaw_motor.cpp | 19 ++++++++++++ main/modules/roboclaw_motor.h | 1 + main/modules/roboclaw_wheels.cpp | 11 +++++++ main/modules/roboclaw_wheels.h | 1 + main/modules/stepper_motor.cpp | 13 ++++++++ main/modules/stepper_motor.h | 1 + 34 files changed, 346 insertions(+) diff --git a/main/modules/can.cpp b/main/modules/can.cpp index 8c7ae0d..22aeb73 100644 --- a/main/modules/can.cpp +++ b/main/modules/can.cpp @@ -190,3 +190,26 @@ void Can::subscribe(const uint32_t id, const Module_ptr module) { } this->subscribers[id] = module; } + +void Can::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "can_mode") { + const std::string can_mode = expression->evaluate_string(); + if (can_mode == "start") { + if (twai_start() != ESP_OK) { + throw std::runtime_error("could not start twai driver"); + } + } else if (can_mode == "stop") { + if (twai_stop() != ESP_OK) { + throw std::runtime_error("could not stop twai driver"); + } + } else if (can_mode == "recover") { + if (twai_initiate_recovery() != ESP_OK) { + throw std::runtime_error("could not initiate recovery"); + } + } else { + throw std::runtime_error("unsupported can_mode"); + } + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/can.h b/main/modules/can.h index 1def1ae..46e6f22 100644 --- a/main/modules/can.h +++ b/main/modules/can.h @@ -15,6 +15,7 @@ class Can : public Module { Can(const std::string name, const gpio_num_t rx_pin, const gpio_num_t tx_pin, const long baud_rate); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); bool receive(); diff --git a/main/modules/canopen_motor.cpp b/main/modules/canopen_motor.cpp index 8a28609..fc74de6 100644 --- a/main/modules/canopen_motor.cpp +++ b/main/modules/canopen_motor.cpp @@ -505,3 +505,54 @@ void CanOpenMotor::speed(const double speed, const double acceleration) { this->properties[PROP_CTRL_HALT]->boolean_value = false; send_control_word(build_ctrl_word(false)); } + +void CanOpenMotor::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "set_target_position") { + int32_t target_position = expression->evaluate_integer(); + int32_t offset = this->properties[PROP_OFFSET]->integer_value; + send_target_position(target_position + offset); + } else if (property_name == "set_target_velocity") { + int32_t target_velocity = expression->evaluate_integer(); + send_target_velocity(target_velocity); + } else if (property_name == "enter_mode") { + const std::string mode = expression->evaluate_string(); + int32_t velocity = this->properties[PROP_VELOCITY]->integer_value; + if (mode == "enter_pp_mode") { + enter_position_mode(velocity); + } else if (mode == "enter_pv_mode") { + enter_velocity_mode(velocity); + } else { + throw std::runtime_error("invalid mode"); + } + } else if (property_name == "set_profile_acceleration") { + uint16_t acceleration = expression->evaluate_integer(); + set_profile_acceleration(acceleration); + } else if (property_name == "set_profile_deceleration") { + uint16_t deceleration = expression->evaluate_integer(); + set_profile_deceleration(deceleration); + } else if (property_name == "set_profile_quick_stop_deceleration") { + uint16_t deceleration = expression->evaluate_integer(); + set_profile_quick_stop_deceleration(deceleration); + } else if (property_name == "commit_target_position") { + if (expression->evaluate_boolean()) { + send_control_word(build_ctrl_word(true)); + } + } else if (property_name == "set_ctrl_halt") { + this->properties[PROP_CTRL_HALT]->boolean_value = expression->evaluate_boolean(); + send_control_word(build_ctrl_word(false)); + } else if (property_name == "set_ctrl_enable") { + this->properties[PROP_CTRL_ENA_OP]->boolean_value = expression->evaluate_boolean(); + send_control_word(build_ctrl_word(false)); + } else if (property_name == "reset_fault") { + if (expression->evaluate_boolean()) { + this->properties[PROP_CTRL_HALT]->boolean_value = true; + uint16_t ctrl_word = build_ctrl_word(false); + ctrl_word |= (1 << 7); + send_control_word(ctrl_word); + ctrl_word &= ~(1 << 7); + send_control_word(ctrl_word); + } + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/canopen_motor.h b/main/modules/canopen_motor.h index 55a7b9d..a8ee6d3 100644 --- a/main/modules/canopen_motor.h +++ b/main/modules/canopen_motor.h @@ -57,6 +57,7 @@ class CanOpenMotor : public Module, public std::enable_shared_from_this arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) override; static const std::map get_defaults(); diff --git a/main/modules/d1_motor.cpp b/main/modules/d1_motor.cpp index 90e1f19..a0046b1 100644 --- a/main/modules/d1_motor.cpp +++ b/main/modules/d1_motor.cpp @@ -197,3 +197,23 @@ void D1Motor::profile_velocity(const int32_t velocity) { void D1Motor::stop() { this->sdo_write(0x6040, 0, 16, 7); } + +void D1Motor::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "setup") { + this->setup(); + } else if (property_name == "home") { + this->home(); + } else if (property_name == "profile_position") { + int32_t position = expression->evaluate_integer(); + this->profile_position(position); + } else if (property_name == "profile_velocity") { + int32_t velocity = expression->evaluate_integer(); + this->profile_velocity(velocity); + } else if (property_name == "stop") { + this->stop(); + } else if (property_name == "reset") { + this->sdo_write(0x6040, 0, 16, 143); + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/d1_motor.h b/main/modules/d1_motor.h index 2a880dd..64ad0ed 100644 --- a/main/modules/d1_motor.h +++ b/main/modules/d1_motor.h @@ -24,6 +24,7 @@ class D1Motor : public Module, public std::enable_shared_from_this { D1Motor(const std::string &name, const Can_ptr can, int64_t node_id); void subscribe_to_can(); void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; void step() override; void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) override; static const std::map get_defaults(); diff --git a/main/modules/dunker_motor.cpp b/main/modules/dunker_motor.cpp index 80c874a..e03be75 100644 --- a/main/modules/dunker_motor.cpp +++ b/main/modules/dunker_motor.cpp @@ -145,3 +145,20 @@ void DunkerMotor::speed(const double speed) { double DunkerMotor::get_speed() { return this->properties.at("speed")->number_value; } + +void DunkerMotor::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "enable") { + if (expression->evaluate_boolean()) { + this->sdo_write(0x4004, 1, 8, 1); + } + } else if (property_name == "disable") { + if (expression->evaluate_boolean()) { + this->sdo_write(0x4004, 1, 8, 0); + } + } else if (property_name == "speed") { + double speed = expression->evaluate_number(); + this->speed(speed); + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/dunker_motor.h b/main/modules/dunker_motor.h index f81057f..615d134 100644 --- a/main/modules/dunker_motor.h +++ b/main/modules/dunker_motor.h @@ -24,6 +24,7 @@ class DunkerMotor : public Module, public std::enable_shared_from_this arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) override; static const std::map get_defaults(); void speed(const double speed); diff --git a/main/modules/input.cpp b/main/modules/input.cpp index ba33072..7525f9a 100644 --- a/main/modules/input.cpp +++ b/main/modules/input.cpp @@ -80,3 +80,20 @@ void McpInput::set_pull_mode(const gpio_pull_mode_t mode) const { } this->mcp->set_pullup(this->number, mode == GPIO_PULLUP_ONLY); } + +void Input::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "pullmode") { + const std::string pull_mode = expression->evaluate_string(); + if (pull_mode == "pullup") { + this->set_pull_mode(GPIO_PULLUP_ONLY); + } else if (pull_mode == "pulldown") { + this->set_pull_mode(GPIO_PULLDOWN_ONLY); + } else if (pull_mode == "pulloff") { + this->set_pull_mode(GPIO_FLOATING); + } else { + throw std::runtime_error("invalid pullmode"); + } + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/input.h b/main/modules/input.h index d809f3c..b7d38ca 100644 --- a/main/modules/input.h +++ b/main/modules/input.h @@ -17,6 +17,7 @@ class Input : public Module { public: void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); std::string get_output() const override; virtual bool get_level() const = 0; diff --git a/main/modules/linear_motor.cpp b/main/modules/linear_motor.cpp index 02fd2a7..4c70569 100644 --- a/main/modules/linear_motor.cpp +++ b/main/modules/linear_motor.cpp @@ -1,5 +1,6 @@ #include "linear_motor.h" #include +#include const std::map LinearMotor::get_defaults() { return { @@ -100,3 +101,23 @@ void McpLinearMotor::set_in(bool level) const { void McpLinearMotor::set_out(bool level) const { return this->mcp->set_level(this->move_out, level); } + +void LinearMotor::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "mode") { + const std::string mode = expression->evaluate_string(); + if (mode == "in") { + this->set_in(1); + this->set_out(0); + } else if (mode == "out") { + this->set_in(0); + this->set_out(1); + } else if (mode == "stop") { + this->set_in(0); + this->set_out(0); + } else { + throw std::runtime_error("invalid mode"); + } + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/linear_motor.h b/main/modules/linear_motor.h index ff39e3c..b5b7080 100644 --- a/main/modules/linear_motor.h +++ b/main/modules/linear_motor.h @@ -17,6 +17,7 @@ class LinearMotor : public Module { public: void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); }; diff --git a/main/modules/mcp23017.cpp b/main/modules/mcp23017.cpp index eab4404..b7fe226 100644 --- a/main/modules/mcp23017.cpp +++ b/main/modules/mcp23017.cpp @@ -157,3 +157,21 @@ void Mcp23017::set_pullup(const uint8_t number, const bool value) const { this->properties.at("pullups")->integer_value = pullups; this->set_pullups(pullups); } + +void Mcp23017::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "levels") { + uint16_t value = expression->evaluate_integer(); + this->properties.at("levels")->integer_value = value; + this->write_pins(value); + } else if (property_name == "pullups") { + uint16_t value = expression->evaluate_integer(); + this->properties.at("pullups")->integer_value = value; + this->set_pullups(value); + } else if (property_name == "inputs") { + uint16_t value = expression->evaluate_integer(); + this->properties.at("inputs")->integer_value = value; + this->set_inputs(value); + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/mcp23017.h b/main/modules/mcp23017.h index d3ab94b..c36dfad 100644 --- a/main/modules/mcp23017.h +++ b/main/modules/mcp23017.h @@ -31,6 +31,7 @@ class Mcp23017 : public Module { Mcp23017(const std::string name, i2c_port_t i2c_port, gpio_num_t sda_pin, gpio_num_t scl_pin, uint8_t address, int clk_speed); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); bool get_level(const uint8_t number) const; diff --git a/main/modules/motor_axis.cpp b/main/modules/motor_axis.cpp index 568b401..d6d70ff 100644 --- a/main/modules/motor_axis.cpp +++ b/main/modules/motor_axis.cpp @@ -60,3 +60,18 @@ void MotorAxis::call(const std::string method_name, const std::vectorevaluate_number(); + if (this->can_move(speed)) { + this->motor->speed(speed, 0); // Use default acceleration of 0 + } else { + this->motor->stop(); + } + } else if (property_name == "stop") { + this->motor->stop(); + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/motor_axis.h b/main/modules/motor_axis.h index c78c323..5ea4626 100644 --- a/main/modules/motor_axis.h +++ b/main/modules/motor_axis.h @@ -15,5 +15,6 @@ class MotorAxis : public Module { MotorAxis(const std::string name, const Motor_ptr motor, const Input_ptr input1, const Input_ptr input2); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); }; diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 20a5641..169e581 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -177,3 +177,27 @@ double ODriveMotor::get_speed() { void ODriveMotor::speed(const double speed, const double acceleration) { this->speed(static_cast(speed)); } + +void ODriveMotor::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "zero") { + this->properties.at("tick_offset")->number_value += + this->properties.at("position")->number_value / + this->properties.at("m_per_tick")->number_value * + (this->properties.at("reversed")->boolean_value ? -1 : 1); + } else if (property_name == "power") { + float torque = expression->evaluate_number(); + this->power(torque); + } else if (property_name == "speed") { + float speed = expression->evaluate_number(); + this->speed(speed); + } else if (property_name == "position") { + float position = expression->evaluate_number(); + this->position(position); + } else if (property_name == "off") { + this->off(); + } else if (property_name == "reset_motor") { + this->reset_motor_error(); + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/odrive_motor.h b/main/modules/odrive_motor.h index 87c7d41..09f3699 100644 --- a/main/modules/odrive_motor.h +++ b/main/modules/odrive_motor.h @@ -24,6 +24,7 @@ class ODriveMotor : public Module, public std::enable_shared_from_this arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) override; static const std::map get_defaults(); void power(const float torque); diff --git a/main/modules/odrive_wheels.cpp b/main/modules/odrive_wheels.cpp index 371223b..a284140 100644 --- a/main/modules/odrive_wheels.cpp +++ b/main/modules/odrive_wheels.cpp @@ -60,3 +60,14 @@ void ODriveWheels::call(const std::string method_name, const std::vectorevaluate_boolean()) { + this->left_motor->off(); + this->right_motor->off(); + } + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/odrive_wheels.h b/main/modules/odrive_wheels.h index aee543c..8fe68cc 100644 --- a/main/modules/odrive_wheels.h +++ b/main/modules/odrive_wheels.h @@ -17,5 +17,6 @@ class ODriveWheels : public Module { ODriveWheels(const std::string name, const ODriveMotor_ptr left_motor, const ODriveMotor_ptr right_motor); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); }; diff --git a/main/modules/output.cpp b/main/modules/output.cpp index 2582773..f506ff4 100644 --- a/main/modules/output.cpp +++ b/main/modules/output.cpp @@ -70,3 +70,23 @@ McpOutput::McpOutput(const std::string name, const Mcp23017_ptr mcp, const uint8 void McpOutput::set_level(bool level) const { this->mcp->set_level(this->number, level); } + +void Output::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "enabled") { + bool enabled = expression->evaluate_boolean(); + this->target_level = enabled ? 1 : 0; + this->pulse_interval = 0; + this->step(); + } else if (property_name == "level") { + bool level = expression->evaluate_boolean(); + this->target_level = level; + this->pulse_interval = 0; + this->step(); + } else if (property_name == "pulse") { + double interval = expression->evaluate_number(); + this->pulse_interval = interval; + this->pulse_duty_cycle = 0.5; + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/output.h b/main/modules/output.h index d336e1d..e893585 100644 --- a/main/modules/output.h +++ b/main/modules/output.h @@ -17,6 +17,7 @@ class Output : public Module { public: void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); }; diff --git a/main/modules/pwm_output.cpp b/main/modules/pwm_output.cpp index b7a17e3..abcbc88 100644 --- a/main/modules/pwm_output.cpp +++ b/main/modules/pwm_output.cpp @@ -60,3 +60,11 @@ void PwmOutput::call(const std::string method_name, const std::vectoris_on = expression->evaluate_boolean(); + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/pwm_output.h b/main/modules/pwm_output.h index f00bcae..43862dc 100644 --- a/main/modules/pwm_output.h +++ b/main/modules/pwm_output.h @@ -18,5 +18,6 @@ class PwmOutput : public Module { const ledc_channel_t ledc_channel); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); }; diff --git a/main/modules/rmd_motor.cpp b/main/modules/rmd_motor.cpp index a8fa628..3d09911 100644 --- a/main/modules/rmd_motor.cpp +++ b/main/modules/rmd_motor.cpp @@ -277,3 +277,26 @@ bool RmdMotor::set_acceleration(const uint8_t index, const uint32_t acceleration *((uint8_t *)(&acceleration) + 3), 20); } + +void RmdMotor::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "power") { + double power = expression->evaluate_number(); + this->power(power); + } else if (property_name == "speed") { + double speed = expression->evaluate_number(); + this->speed(speed); + } else if (property_name == "position") { + double position = expression->evaluate_number(); + this->position(position, 0); // Use default speed of 0 + } else if (property_name == "stop") { + this->stop(); + } else if (property_name == "off") { + this->off(); + } else if (property_name == "hold") { + this->hold(); + } else if (property_name == "clear_errors") { + this->clear_errors(); + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/rmd_motor.h b/main/modules/rmd_motor.h index 0897817..943e11b 100644 --- a/main/modules/rmd_motor.h +++ b/main/modules/rmd_motor.h @@ -28,6 +28,7 @@ class RmdMotor : public Module, public std::enable_shared_from_this { void subscribe_to_can(); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) override; static const std::map get_defaults(); diff --git a/main/modules/rmd_pair.cpp b/main/modules/rmd_pair.cpp index 73f60f7..234e93f 100644 --- a/main/modules/rmd_pair.cpp +++ b/main/modules/rmd_pair.cpp @@ -111,3 +111,21 @@ void RmdPair::call(const std::string method_name, const std::vectorrmd1->stop(); + this->rmd2->stop(); + } else if (property_name == "off") { + this->rmd1->off(); + this->rmd2->off(); + } else if (property_name == "hold") { + this->rmd1->hold(); + this->rmd2->hold(); + } else if (property_name == "clear_errors") { + this->rmd1->clear_errors(); + this->rmd2->clear_errors(); + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/rmd_pair.h b/main/modules/rmd_pair.h index cfdc3a4..c006459 100644 --- a/main/modules/rmd_pair.h +++ b/main/modules/rmd_pair.h @@ -29,5 +29,6 @@ class RmdPair : public Module { public: RmdPair(const std::string name, const RmdMotor_ptr rmd1, const RmdMotor_ptr rmd2); void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); }; diff --git a/main/modules/roboclaw_motor.cpp b/main/modules/roboclaw_motor.cpp index 70a25d2..591c0e7 100644 --- a/main/modules/roboclaw_motor.cpp +++ b/main/modules/roboclaw_motor.cpp @@ -75,3 +75,22 @@ void RoboClawMotor::speed(int value) { } throw std::runtime_error("could not set speed after " + std::to_string(max_retries) + " retries"); } + +void RoboClawMotor::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "power") { + double power = expression->evaluate_number(); + this->power(power); + } else if (property_name == "speed") { + double speed = expression->evaluate_number(); + this->speed(speed); + } else if (property_name == "zero") { + if (expression->evaluate_boolean()) { + bool success = this->motor_number == 1 ? this->roboclaw->SetEncM1(0) : this->roboclaw->SetEncM2(0); + if (!success) { + throw std::runtime_error("could not reset position"); + } + } + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/roboclaw_motor.h b/main/modules/roboclaw_motor.h index 3d1e8e2..b337fee 100644 --- a/main/modules/roboclaw_motor.h +++ b/main/modules/roboclaw_motor.h @@ -15,6 +15,7 @@ class RoboClawMotor : public Module { RoboClawMotor(const std::string name, const RoboClaw_ptr roboclaw, const unsigned int motor_number); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); int64_t get_position() const; diff --git a/main/modules/roboclaw_wheels.cpp b/main/modules/roboclaw_wheels.cpp index 3eacddb..b93490b 100644 --- a/main/modules/roboclaw_wheels.cpp +++ b/main/modules/roboclaw_wheels.cpp @@ -76,3 +76,14 @@ void RoboClawWheels::call(const std::string method_name, const std::vectorevaluate_boolean()) { + this->left_motor->power(0); + this->right_motor->power(0); + } + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/roboclaw_wheels.h b/main/modules/roboclaw_wheels.h index def922c..fe6a63e 100644 --- a/main/modules/roboclaw_wheels.h +++ b/main/modules/roboclaw_wheels.h @@ -16,5 +16,6 @@ class RoboClawWheels : public Module { RoboClawWheels(const std::string name, const RoboClawMotor_ptr left_motor, const RoboClawMotor_ptr right_motor); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); }; diff --git a/main/modules/stepper_motor.cpp b/main/modules/stepper_motor.cpp index 5a21d67..bbc1afd 100644 --- a/main/modules/stepper_motor.cpp +++ b/main/modules/stepper_motor.cpp @@ -219,3 +219,16 @@ void StepperMotor::speed(const double speed, const double acceleration) { this->target_acceleration = static_cast(acceleration); set_state(this->target_speed == 0 ? Idle : Speeding); } + +void StepperMotor::write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) { + if (property_name == "speed") { + double speed = expression->evaluate_number(); + this->speed(speed, 0); + } else if (property_name == "stop") { + if (expression->evaluate_boolean()) { + this->stop(); + } + } else { + Module::write_property(property_name, expression, from_expander); + } +} diff --git a/main/modules/stepper_motor.h b/main/modules/stepper_motor.h index bc9a775..12c42a7 100644 --- a/main/modules/stepper_motor.h +++ b/main/modules/stepper_motor.h @@ -45,6 +45,7 @@ class StepperMotor : public Module, virtual public Motor { const ledc_channel_t ledc_channel); void step() override; void call(const std::string method_name, const std::vector arguments) override; + void write_property(const std::string property_name, const ConstExpression_ptr expression, const bool from_expander) override; static const std::map get_defaults(); StepperState get_state() const { return this->state; }