Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Proxy defaults with static functions #104

Merged
merged 2 commits into from
Nov 21, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 10 additions & 3 deletions main/modules/analog.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,18 @@
#include "freertos/task.h"
#include "uart.h"

const std::map<std::string, Variable_ptr> &Analog::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{"raw", std::make_shared<IntegerVariable>()},
{"voltage", std::make_shared<NumberVariable>()},
};
return 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();

if (unit < 1 || unit > 2) {
echo("error: invalid unit, using default 1");
unit = 1;
Expand Down Expand Up @@ -61,9 +71,6 @@ Analog::Analog(const std::string name, uint8_t unit, uint8_t channel, float atte
.default_vref = 1100,
};
ESP_ERROR_CHECK(adc_cali_create_scheme_line_fitting(&cali_config, &adc_cali_handle));

this->properties["raw"] = std::make_shared<IntegerVariable>();
this->properties["voltage"] = std::make_shared<NumberVariable>();
}

void Analog::step() {
Expand Down
1 change: 1 addition & 0 deletions main/modules/analog.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,4 +17,5 @@ class Analog : public Module {
public:
Analog(const std::string name, uint8_t unit, uint8_t channel, float attenuation_level);
void step() override;
static const std::map<std::string, Variable_ptr> &get_defaults();
};
27 changes: 17 additions & 10 deletions main/modules/can.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,22 @@
#include "driver/twai.h"
#include <stdexcept>

const std::map<std::string, Variable_ptr> &Can::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{"state", std::make_shared<StringVariable>()},
{"tx_error_counter", std::make_shared<IntegerVariable>()},
{"rx_error_counter", std::make_shared<IntegerVariable>()},
{"msgs_to_tx", std::make_shared<IntegerVariable>()},
{"msgs_to_rx", std::make_shared<IntegerVariable>()},
{"tx_failed_count", std::make_shared<IntegerVariable>()},
{"rx_missed_count", std::make_shared<IntegerVariable>()},
{"rx_overrun_count", std::make_shared<IntegerVariable>()},
{"arb_lost_count", std::make_shared<IntegerVariable>()},
{"bus_error_count", std::make_shared<IntegerVariable>()},
};
return defaults;
}

Can::Can(const std::string name, const gpio_num_t rx_pin, const gpio_num_t tx_pin, const long baud_rate)
: Module(can, name) {
twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(tx_pin, rx_pin, TWAI_MODE_NORMAL);
Expand Down Expand Up @@ -42,16 +58,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["state"] = std::make_shared<StringVariable>();
this->properties["tx_error_counter"] = std::make_shared<IntegerVariable>();
this->properties["rx_error_counter"] = std::make_shared<IntegerVariable>();
this->properties["msgs_to_tx"] = std::make_shared<IntegerVariable>();
this->properties["msgs_to_rx"] = std::make_shared<IntegerVariable>();
this->properties["tx_failed_count"] = std::make_shared<IntegerVariable>();
this->properties["rx_missed_count"] = std::make_shared<IntegerVariable>();
this->properties["rx_overrun_count"] = std::make_shared<IntegerVariable>();
this->properties["arb_lost_count"] = std::make_shared<IntegerVariable>();
this->properties["bus_error_count"] = std::make_shared<IntegerVariable>();
this->properties = Can::get_defaults();

ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config, &f_config));
ESP_ERROR_CHECK(twai_start());
Expand Down
4 changes: 3 additions & 1 deletion main/modules/can.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,12 +14,14 @@ class Can : public Module {
public:
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<ConstExpression_ptr> arguments) override;
static const std::map<std::string, Variable_ptr> &get_defaults();

bool receive();
void send(const uint32_t id, const uint8_t data[8], const bool rtr = false, const uint8_t dlc = 8) const;
void send(const uint32_t id,
const uint8_t d0, const uint8_t d1, const uint8_t d2, const uint8_t d3,
const uint8_t d4, const uint8_t d5, const uint8_t d6, const uint8_t d7,
const bool rtr = false) const;
void call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) override;
void subscribe(const uint32_t id, const Module_ptr module);
};
9 changes: 8 additions & 1 deletion main/modules/canopen_master.cpp
Original file line number Diff line number Diff line change
@@ -1,8 +1,15 @@
#include "canopen_master.h"

const std::map<std::string, Variable_ptr> &CanOpenMaster::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{"sync_interval", std::make_shared<IntegerVariable>(0)},
};
return defaults;
}

CanOpenMaster::CanOpenMaster(const std::string &name, const Can_ptr can)
: Module(canopen_master, name), can(can) {
this->properties["sync_interval"] = std::make_shared<IntegerVariable>(0);
this->properties = CanOpenMaster::get_defaults();
}

void CanOpenMaster::step() {
Expand Down
1 change: 1 addition & 0 deletions main/modules/canopen_master.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,4 +14,5 @@ class CanOpenMaster : public Module, public std::enable_shared_from_this<CanOpen
public:
CanOpenMaster(const std::string &name, const Can_ptr can);
void step() override;
static const std::map<std::string, Variable_ptr> &get_defaults();
};
44 changes: 26 additions & 18 deletions main/modules/canopen_motor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,27 +47,35 @@ static const std::string PROP_PV_IS_MOVING{"pv_is_moving"};
static const std::string PROP_CTRL_ENA_OP{"ctrl_enable"};
static const std::string PROP_CTRL_HALT{"ctrl_halt"};

const std::map<std::string, Variable_ptr> &CanOpenMotor::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{PROP_INITIALIZED, std::make_shared<BooleanVariable>(false)},
{PROP_PENDING_READS, std::make_shared<IntegerVariable>(0)},
{PROP_PENDING_WRITES, std::make_shared<IntegerVariable>(0)},
{PROP_HEARTBEAT, std::make_shared<IntegerVariable>(-1)},
{PROP_301_STATE, std::make_shared<IntegerVariable>(-1)},
{PROP_301_STATE_BOOTING, std::make_shared<BooleanVariable>(false)},
{PROP_301_STATE_PREOP, std::make_shared<BooleanVariable>(false)},
{PROP_301_STATE_OP, std::make_shared<BooleanVariable>(false)},
{PROP_OFFSET, std::make_shared<IntegerVariable>(0)},
{PROP_POSITION, std::make_shared<IntegerVariable>(0)},
{PROP_VELOCITY, std::make_shared<IntegerVariable>(0)},
{PROP_402_OP_ENA, std::make_shared<BooleanVariable>(false)},
{PROP_402_FAULT, std::make_shared<BooleanVariable>(false)},
{PROP_TARGET_REACHED, std::make_shared<BooleanVariable>(false)},
{PROP_PP_SET_POINT_ACK, std::make_shared<BooleanVariable>(false)},
{PROP_PV_IS_MOVING, std::make_shared<BooleanVariable>(false)},
{PROP_CTRL_ENA_OP, std::make_shared<BooleanVariable>(false)},
{PROP_CTRL_HALT, std::make_shared<BooleanVariable>(true)},
};
return defaults;
}

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[PROP_INITIALIZED] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_PENDING_READS] = std::make_shared<IntegerVariable>(0);
this->properties[PROP_PENDING_WRITES] = std::make_shared<IntegerVariable>(0);
this->properties[PROP_HEARTBEAT] = std::make_shared<IntegerVariable>(-1);
this->properties[PROP_301_STATE] = std::make_shared<IntegerVariable>(-1);
this->properties[PROP_301_STATE_BOOTING] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_301_STATE_PREOP] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_301_STATE_OP] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_OFFSET] = std::make_shared<IntegerVariable>(0);
this->properties[PROP_POSITION] = std::make_shared<IntegerVariable>(0);
this->properties[PROP_VELOCITY] = std::make_shared<IntegerVariable>(0);
this->properties[PROP_402_OP_ENA] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_402_FAULT] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_TARGET_REACHED] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_PP_SET_POINT_ACK] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_PV_IS_MOVING] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_CTRL_ENA_OP] = std::make_shared<BooleanVariable>(false);
this->properties[PROP_CTRL_HALT] = std::make_shared<BooleanVariable>(true);

this->properties = CanOpenMotor::get_defaults();
}

void CanOpenMotor::wait_for_sdo_writes(uint32_t timeout_ms) {
Expand Down
1 change: 1 addition & 0 deletions main/modules/canopen_motor.h
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ class CanOpenMotor : public Module, public std::enable_shared_from_this<CanOpenM
void subscribe_to_can();
void call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) override;
void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) override;
static const std::map<std::string, Variable_ptr> &get_defaults();

void stop() override;
double get_position() override;
Expand Down
27 changes: 17 additions & 10 deletions main/modules/d1_motor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,18 +4,25 @@
#include "utils/timing.h"
#include <cinttypes>

const std::map<std::string, Variable_ptr> &D1Motor::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{"switch_search_speed", std::make_shared<IntegerVariable>()},
{"zero_search_speed", std::make_shared<IntegerVariable>()},
{"homing_acceleration", std::make_shared<IntegerVariable>()},
{"profile_acceleration", std::make_shared<IntegerVariable>()},
{"profile_velocity", std::make_shared<IntegerVariable>()},
{"profile_deceleration", std::make_shared<IntegerVariable>()},
{"position", std::make_shared<IntegerVariable>()},
{"velocity", std::make_shared<IntegerVariable>()},
{"status_word", std::make_shared<IntegerVariable>(-1)},
{"status_flags", std::make_shared<IntegerVariable>()},
};
return 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["switch_search_speed"] = std::make_shared<IntegerVariable>();
this->properties["zero_search_speed"] = std::make_shared<IntegerVariable>();
this->properties["homing_acceleration"] = std::make_shared<IntegerVariable>();
this->properties["profile_acceleration"] = std::make_shared<IntegerVariable>();
this->properties["profile_velocity"] = std::make_shared<IntegerVariable>();
this->properties["profile_deceleration"] = std::make_shared<IntegerVariable>();
this->properties["position"] = std::make_shared<IntegerVariable>();
this->properties["velocity"] = std::make_shared<IntegerVariable>();
this->properties["status_word"] = std::make_shared<IntegerVariable>(-1);
this->properties["status_flags"] = std::make_shared<IntegerVariable>();
this->properties = D1Motor::get_defaults();
}

void D1Motor::subscribe_to_can() {
Expand Down
1 change: 1 addition & 0 deletions main/modules/d1_motor.h
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ class D1Motor : public Module, public std::enable_shared_from_this<D1Motor> {
void call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) 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<std::string, Variable_ptr> &get_defaults();
void setup();
void home();
void profile_position(const int32_t position);
Expand Down
13 changes: 10 additions & 3 deletions main/modules/dunker_motor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,18 @@
#include "utils/timing.h"
#include <cinttypes>

const std::map<std::string, Variable_ptr> &DunkerMotor::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{"speed", std::make_shared<NumberVariable>()},
{"m_per_turn", std::make_shared<NumberVariable>(1.0)},
{"reversed", std::make_shared<BooleanVariable>()},
};
return 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["speed"] = std::make_shared<NumberVariable>();
this->properties["m_per_turn"] = std::make_shared<NumberVariable>(1.0);
this->properties["reversed"] = std::make_shared<BooleanVariable>();
this->properties = DunkerMotor::get_defaults();
}

void DunkerMotor::subscribe_to_can() {
Expand Down
1 change: 1 addition & 0 deletions main/modules/dunker_motor.h
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ class DunkerMotor : public Module, public std::enable_shared_from_this<DunkerMot
void subscribe_to_can();
void call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) override;
void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) override;
static const std::map<std::string, Variable_ptr> &get_defaults();
void speed(const double speed);
double get_speed();
};
13 changes: 10 additions & 3 deletions main/modules/dunker_wheels.cpp
Original file line number Diff line number Diff line change
@@ -1,11 +1,18 @@
#include "dunker_wheels.h"
#include <memory>

const std::map<std::string, Variable_ptr> &DunkerWheels::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{"width", std::make_shared<NumberVariable>(1.0)},
{"linear_speed", std::make_shared<NumberVariable>()},
{"angular_speed", std::make_shared<NumberVariable>()},
};
return 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["width"] = std::make_shared<NumberVariable>(1);
this->properties["linear_speed"] = std::make_shared<NumberVariable>();
this->properties["angular_speed"] = std::make_shared<NumberVariable>();
this->properties = DunkerWheels::get_defaults();
}

void DunkerWheels::step() {
Expand Down
3 changes: 2 additions & 1 deletion main/modules/dunker_wheels.h
Original file line number Diff line number Diff line change
Expand Up @@ -12,4 +12,5 @@ class DunkerWheels : public Module {
DunkerWheels(const std::string name, const DunkerMotor_ptr left_motor, const DunkerMotor_ptr right_motor);
void step() override;
void call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) override;
};
static const std::map<std::string, Variable_ptr> &get_defaults();
};
16 changes: 11 additions & 5 deletions main/modules/expander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,16 @@
#include <cstring>
#include <stdexcept>

const std::map<std::string, Variable_ptr> &Expander::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{"boot_timeout", std::make_shared<NumberVariable>(5.0)},
{"ping_interval", std::make_shared<NumberVariable>(1.0)},
{"ping_timeout", std::make_shared<NumberVariable>(2.0)},
{"is_ready", std::make_shared<BooleanVariable>(false)},
{"last_message_age", std::make_shared<IntegerVariable>(0)}};
return defaults;
}

Expander::Expander(const std::string name,
const ConstSerial_ptr serial,
const gpio_num_t boot_pin,
Expand All @@ -20,11 +30,7 @@ Expander::Expander(const std::string name,
enable_pin(enable_pin),
message_handler(message_handler) {

this->properties["boot_timeout"] = std::make_shared<NumberVariable>(5.0);
this->properties["ping_interval"] = std::make_shared<NumberVariable>(1.0);
this->properties["ping_timeout"] = std::make_shared<NumberVariable>(2.0);
this->properties["is_ready"] = std::make_shared<BooleanVariable>(false);
this->properties["last_message_age"] = std::make_shared<IntegerVariable>();
this->properties = Expander::get_defaults();

serial->enable_line_detection();
if (boot_pin != GPIO_NUM_NC && enable_pin != GPIO_NUM_NC) {
Expand Down
1 change: 1 addition & 0 deletions main/modules/expander.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,4 +45,5 @@ class Expander : public Module {
void add_proxy(const std::string module_name,
const std::string module_type,
const std::vector<ConstExpression_ptr> arguments);
static const std::map<std::string, Variable_ptr> &get_defaults();
};
35 changes: 21 additions & 14 deletions main/modules/imu.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,26 @@
#define I2C_MASTER_TX_BUF_DISABLE 0
#define I2C_MASTER_RX_BUF_DISABLE 0

const std::map<std::string, Variable_ptr> &Imu::get_defaults() {
static const std::map<std::string, Variable_ptr> defaults = {
{"acc_x", std::make_shared<NumberVariable>()},
{"acc_y", std::make_shared<NumberVariable>()},
{"acc_z", std::make_shared<NumberVariable>()},
{"roll", std::make_shared<NumberVariable>()},
{"pitch", std::make_shared<NumberVariable>()},
{"yaw", std::make_shared<NumberVariable>()},
{"quat_w", std::make_shared<NumberVariable>()},
{"quat_x", std::make_shared<NumberVariable>()},
{"quat_y", std::make_shared<NumberVariable>()},
{"quat_z", std::make_shared<NumberVariable>()},
{"cal_sys", std::make_shared<NumberVariable>()},
{"cal_gyr", std::make_shared<NumberVariable>()},
{"cal_acc", std::make_shared<NumberVariable>()},
{"cal_mag", std::make_shared<NumberVariable>()},
};
return defaults;
}

Imu::Imu(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)
: Module(imu, name), i2c_port(i2c_port), address(address) {
i2c_config_t config;
Expand Down Expand Up @@ -33,20 +53,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["acc_x"] = std::make_shared<NumberVariable>();
this->properties["acc_y"] = std::make_shared<NumberVariable>();
this->properties["acc_z"] = std::make_shared<NumberVariable>();
this->properties["roll"] = std::make_shared<NumberVariable>();
this->properties["pitch"] = std::make_shared<NumberVariable>();
this->properties["yaw"] = std::make_shared<NumberVariable>();
this->properties["quat_w"] = std::make_shared<NumberVariable>();
this->properties["quat_x"] = std::make_shared<NumberVariable>();
this->properties["quat_y"] = std::make_shared<NumberVariable>();
this->properties["quat_z"] = std::make_shared<NumberVariable>();
this->properties["cal_sys"] = std::make_shared<NumberVariable>();
this->properties["cal_gyr"] = std::make_shared<NumberVariable>();
this->properties["cal_acc"] = std::make_shared<NumberVariable>();
this->properties["cal_mag"] = std::make_shared<NumberVariable>();
this->properties = Imu::get_defaults();
}

void Imu::step() {
Expand Down
1 change: 1 addition & 0 deletions main/modules/imu.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,4 +18,5 @@ class Imu : public Module {
public:
Imu(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;
static const std::map<std::string, Variable_ptr> &get_defaults();
};
Loading