From 75726df72765c8a1adee94ccf5467003c2a889f3 Mon Sep 17 00:00:00 2001 From: Johannes-Thiel <jojo.thadfunn@gmail.com> Date: Thu, 4 Apr 2024 16:41:05 +0200 Subject: [PATCH 1/9] Add new methods to ODrive motor module for handling errors --- docs/module_reference.md | 23 +++++--- main/modules/odrive_motor.cpp | 102 +++++++++++++++++++++++++++++++++- main/modules/odrive_motor.h | 3 + 3 files changed, 116 insertions(+), 12 deletions(-) diff --git a/docs/module_reference.md b/docs/module_reference.md index 28a75e26..7b3fac46 100644 --- a/docs/module_reference.md +++ b/docs/module_reference.md @@ -269,15 +269,20 @@ The ODrive motor module controls a motor using an [ODrive motor controller](http | `motor.tick_offset` | Encoder tick offset | `float` | | `motor.m_per_tick` | Meters per encoder tick | `float` | | `motor.reversed` | Reverse motor direction | `bool` | - -| Methods | Description | Arguments | -| ------------------------------ | -------------------------------------- | ---------------- | -| `motor.zero()` | Set current position as zero position | | -| `motor.power(torque)` | Move with given `torque` | `float` | -| `motor.speed(speed)` | Move with given `speed` (m/s) | `float` | -| `motor.position(position)` | Move to given `position` (m) | `float` | -| `motor.limits(speed, current)` | Set speed (m/s) and current (A) limits | `float`, `float` | -| `motor.off()` | Turn motor off (idle state) | | +| `motor.axis_error` | Error code of the axis | `int` | +| `motor.motor_error` | Motor in fault mode | `bool` | + +| Methods | Description | Arguments | +| ------------------------------ | ------------------------------------------- | ---------------- | +| `motor.zero()` | Set current position as zero position | | +| `motor.power(torque)` | Move with given `torque` | `float` | +| `motor.speed(speed)` | Move with given `speed` (m/s) | `float` | +| `motor.position(position)` | Move to given `position` (m) | `float` | +| `motor.limits(speed, current)` | Set speed (m/s) and current (A) limits | `float`, `float` | +| `motor.off()` | Turn motor off (idle state) | | +| `motor.update_motor_error` | Update the motor_error Propertie | | +| `motor.clear_errors` | Clears all error statements | | +| `motor.reset_motor` | Resets the Motor and clears error statments | | ## ODrive Wheels diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 3759dd26..20383723 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -2,15 +2,42 @@ #include <cstring> #include <memory> -ODriveMotor::ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id) - : Module(odrive_motor, name), can_id(can_id), can(can) { +enum AXIS_ERROR { + AXIS_ERROR_NONE = 0, + AXIS_ERROR_INVALID_STATE = 0x01, + AXIS_ERROR_WATCHDOG_TIMER_EXPIRED = 0x800, + AXIS_ERROR_MIN_ENDSTOP_PRESSED = 0x1000, + AXIS_ERROR_MAX_ENDSTOP_PRESSED = 0x2000, + AXIS_ERROR_ESTOP_REQUESTED = 0x4000, + AXIS_ERROR_OVER_TEMP = 0x40000, + AXIS_ERROR_UNKNOWN_POSITION = 0x80000 +}; +enum MOTOR_ERROR { + MOTOR_ERROR_NONE = 0, + MOTOR_ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x01, + MOTOR_ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x02, + MOTOR_ERROR_DRV_FAULT = 0x08, + MOTOR_ERROR_CONTROL_DEADLINE_MISSED = 0x10, + MOTOR_ERROR_MODULATION_MAGNITUDE = 0x80, + MOTOR_ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x400, + MOTOR_ERROR_CURRENT_LIMIT_VIALTION = 0x1000, + // TODO add more error coddes https://docs.odriverobotics.com/v/0.5.4/fibre_types/com_odriverobotics_ODrive.html?highlight=motor+error#ODrive.Motor.Error + +}; + +ODriveMotor::ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id) : Module(odrive_motor, name), + can_id(can_id), can(can) { this->properties["position"] = std::make_shared<NumberVariable>(); this->properties["tick_offset"] = std::make_shared<NumberVariable>(); this->properties["m_per_tick"] = std::make_shared<NumberVariable>(1.0); this->properties["reversed"] = std::make_shared<BooleanVariable>(); + this->properties["motor_error"] = std::make_shared<BooleanVariable>(false); + this->properties["axis_error"] = std::make_shared<IntegerVariable>(); } void ODriveMotor::subscribe_to_can() { + this->can->subscribe(this->can_id + 0x001, std::static_pointer_cast<Module>(this->shared_from_this())); + this->can->subscribe(this->can_id + 0x003, std::static_pointer_cast<Module>(this->shared_from_this())); this->can->subscribe(this->can_id + 0x009, std::static_pointer_cast<Module>(this->shared_from_this())); } @@ -52,6 +79,15 @@ void ODriveMotor::call(const std::string method_name, const std::vector<ConstExp } else if (method_name == "off") { Module::expect(arguments, 0); this->off(); + } else if (method_name == "update_motor_error") { + Module::expect(arguments, 0); + this->update_motor_error(); + } else if (method_name == "reset_motor") { + Module::expect(arguments, 0); + this->reset_motor(); + } else if (method_name == "clear_errors") { + Module::expect(arguments, 0); + this->clear_errors(); } else { Module::call(method_name, arguments); } @@ -60,6 +96,53 @@ void ODriveMotor::call(const std::string method_name, const std::vector<ConstExp void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) { this->is_boot_complete = true; switch (id - this->can_id) { + case 0x001: { + int axis_state; + std::memcpy(&axis_state, data + 4, 2); + this->axis_state = axis_state; + int axis_error; + std::memcpy(&axis_error, data, 4); + switch (axis_error) { + case AXIS_ERROR_INVALID_STATE: + this->properties.at("axis_error")->integer_value = 1; + break; + case AXIS_ERROR_WATCHDOG_TIMER_EXPIRED: + this->properties.at("axis_error")->integer_value = 2; + break; + case AXIS_ERROR_MIN_ENDSTOP_PRESSED: + this->properties.at("axis_error")->integer_value = 3; + break; + case AXIS_ERROR_MAX_ENDSTOP_PRESSED: + this->properties.at("axis_error")->integer_value = 4; + break; + case AXIS_ERROR_ESTOP_REQUESTED: + this->properties.at("axis_error")->integer_value = 5; + break; + case AXIS_ERROR_OVER_TEMP: + this->properties.at("axis_error")->integer_value = 6; + break; + case AXIS_ERROR_UNKNOWN_POSITION: + this->properties.at("axis_error")->integer_value = 7; + break; + default: + this->properties.at("axis_error")->integer_value = axis_error; + + break; + } + break; + case 0x003: { + int motor_error; + std::memcpy(&motor_error, data, 4); + switch (motor_error) { + case MOTOR_ERROR_NONE: + this->properties.at("motor_error")->boolean_value = false; + break; + + default: + this->properties.at("motor_error")->boolean_value = true; + break; + } + } case 0x009: { float tick; std::memcpy(&tick, data, 4); @@ -69,6 +152,7 @@ void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8 this->properties.at("m_per_tick")->number_value; } } + } } void ODriveMotor::power(const float torque) { @@ -112,7 +196,19 @@ void ODriveMotor::limits(const float speed, const float current) { void ODriveMotor::off() { this->set_mode(1); // AXIS_STATE_IDLE } - +void ODriveMotor::clear_errors() { + uint8_t empty_data[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + this->can->send(this->can_id + 0x018, empty_data, true); +} +void ODriveMotor::update_motor_error() { + uint8_t empty_data[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + this->can->send(this->can_id + 0x003, empty_data, true); +} +void ODriveMotor::reset_motor() { + this->clear_errors(); + this->set_mode(8, 2, 1); // AXIS_STATE_CLOSED_LOOP_CONTROL, CONTROL_MODE_VELOCITY_CONTROL, INPUT_MODE_PASSTHROUGH + this->update_motor_error(); +} double ODriveMotor::get_position() { return this->properties.at("position")->number_value; } \ No newline at end of file diff --git a/main/modules/odrive_motor.h b/main/modules/odrive_motor.h index 5ead74b6..0291d45b 100644 --- a/main/modules/odrive_motor.h +++ b/main/modules/odrive_motor.h @@ -28,5 +28,8 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot void position(const float position); void limits(const float speed, const float current); void off(); + void update_motor_error(); + void reset_motor(); + void clear_errors(); double get_position(); }; \ No newline at end of file From e6c20b8c87d8c8c339d37400e1dbff8b69cbd3d2 Mon Sep 17 00:00:00 2001 From: Johannes-Thiel <jojo.thadfunn@gmail.com> Date: Fri, 5 Apr 2024 08:37:20 +0200 Subject: [PATCH 2/9] cahnge flag to 0,1 add motor reset --- docs/module_reference.md | 3 +-- main/modules/odrive_motor.cpp | 14 ++++++++------ main/modules/odrive_motor.h | 3 ++- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/docs/module_reference.md b/docs/module_reference.md index 7b3fac46..09a03fa2 100644 --- a/docs/module_reference.md +++ b/docs/module_reference.md @@ -270,7 +270,7 @@ The ODrive motor module controls a motor using an [ODrive motor controller](http | `motor.m_per_tick` | Meters per encoder tick | `float` | | `motor.reversed` | Reverse motor direction | `bool` | | `motor.axis_error` | Error code of the axis | `int` | -| `motor.motor_error` | Motor in fault mode | `bool` | +| `motor.motor_error` | Motor in fault mode | `int` | | Methods | Description | Arguments | | ------------------------------ | ------------------------------------------- | ---------------- | @@ -280,7 +280,6 @@ The ODrive motor module controls a motor using an [ODrive motor controller](http | `motor.position(position)` | Move to given `position` (m) | `float` | | `motor.limits(speed, current)` | Set speed (m/s) and current (A) limits | `float`, `float` | | `motor.off()` | Turn motor off (idle state) | | -| `motor.update_motor_error` | Update the motor_error Propertie | | | `motor.clear_errors` | Clears all error statements | | | `motor.reset_motor` | Resets the Motor and clears error statments | | diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 20383723..08b659de 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -31,7 +31,7 @@ ODriveMotor::ODriveMotor(const std::string name, const Can_ptr can, const uint32 this->properties["tick_offset"] = std::make_shared<NumberVariable>(); this->properties["m_per_tick"] = std::make_shared<NumberVariable>(1.0); this->properties["reversed"] = std::make_shared<BooleanVariable>(); - this->properties["motor_error"] = std::make_shared<BooleanVariable>(false); + this->properties["motor_error"] = std::make_shared<IntegerVariable>(0); this->properties["axis_error"] = std::make_shared<IntegerVariable>(); } @@ -79,9 +79,6 @@ void ODriveMotor::call(const std::string method_name, const std::vector<ConstExp } else if (method_name == "off") { Module::expect(arguments, 0); this->off(); - } else if (method_name == "update_motor_error") { - Module::expect(arguments, 0); - this->update_motor_error(); } else if (method_name == "reset_motor") { Module::expect(arguments, 0); this->reset_motor(); @@ -135,11 +132,11 @@ void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8 std::memcpy(&motor_error, data, 4); switch (motor_error) { case MOTOR_ERROR_NONE: - this->properties.at("motor_error")->boolean_value = false; + this->properties.at("motor_error")->integer_value = 0; break; default: - this->properties.at("motor_error")->boolean_value = true; + this->properties.at("motor_error")->integer_value = 1; break; } } @@ -211,4 +208,9 @@ void ODriveMotor::reset_motor() { } double ODriveMotor::get_position() { return this->properties.at("position")->number_value; +} + +void ODriveMotor::step() { + this->update_motor_error(); + Module::step(); } \ No newline at end of file diff --git a/main/modules/odrive_motor.h b/main/modules/odrive_motor.h index 0291d45b..4b372504 100644 --- a/main/modules/odrive_motor.h +++ b/main/modules/odrive_motor.h @@ -17,6 +17,7 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot uint8_t axis_input_mode = -1; void set_mode(const uint8_t state, const uint8_t control_mode = 0, const uint8_t input_mode = 0); + void update_motor_error(); public: ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id); @@ -28,8 +29,8 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot void position(const float position); void limits(const float speed, const float current); void off(); - void update_motor_error(); void reset_motor(); void clear_errors(); double get_position(); + void step() override; }; \ No newline at end of file From 2e9f1e273c4ac3006be7c4ac94081c76bdfcb4aa Mon Sep 17 00:00:00 2001 From: Johannes-Thiel <jojo.thadfunn@gmail.com> Date: Wed, 29 May 2024 09:11:42 +0200 Subject: [PATCH 3/9] Refactor ODriveMotor module to handle motor errors and reset motor error flag --- main/modules/odrive_motor.cpp | 59 +++++++++-------------------------- main/modules/odrive_motor.h | 3 +- 2 files changed, 15 insertions(+), 47 deletions(-) diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 08b659de..240440ad 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -12,18 +12,6 @@ enum AXIS_ERROR { AXIS_ERROR_OVER_TEMP = 0x40000, AXIS_ERROR_UNKNOWN_POSITION = 0x80000 }; -enum MOTOR_ERROR { - MOTOR_ERROR_NONE = 0, - MOTOR_ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x01, - MOTOR_ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x02, - MOTOR_ERROR_DRV_FAULT = 0x08, - MOTOR_ERROR_CONTROL_DEADLINE_MISSED = 0x10, - MOTOR_ERROR_MODULATION_MAGNITUDE = 0x80, - MOTOR_ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x400, - MOTOR_ERROR_CURRENT_LIMIT_VIALTION = 0x1000, - // TODO add more error coddes https://docs.odriverobotics.com/v/0.5.4/fibre_types/com_odriverobotics_ODrive.html?highlight=motor+error#ODrive.Motor.Error - -}; ODriveMotor::ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id) : Module(odrive_motor, name), can_id(can_id), can(can) { @@ -31,13 +19,12 @@ ODriveMotor::ODriveMotor(const std::string name, const Can_ptr can, const uint32 this->properties["tick_offset"] = std::make_shared<NumberVariable>(); this->properties["m_per_tick"] = std::make_shared<NumberVariable>(1.0); this->properties["reversed"] = std::make_shared<BooleanVariable>(); - this->properties["motor_error"] = std::make_shared<IntegerVariable>(0); this->properties["axis_error"] = std::make_shared<IntegerVariable>(); + this->properties["motor_error_flag"] = std::make_shared<IntegerVariable>(); } void ODriveMotor::subscribe_to_can() { this->can->subscribe(this->can_id + 0x001, std::static_pointer_cast<Module>(this->shared_from_this())); - this->can->subscribe(this->can_id + 0x003, std::static_pointer_cast<Module>(this->shared_from_this())); this->can->subscribe(this->can_id + 0x009, std::static_pointer_cast<Module>(this->shared_from_this())); } @@ -45,6 +32,9 @@ void ODriveMotor::set_mode(const uint8_t state, const uint8_t control_mode, cons if (!this->is_boot_complete) { return; } + // if (this->properties.at("motor_error")->number_value == 1) { + // return; + // } if (this->axis_state != state) { this->can->send(this->can_id + 0x007, state, 0, 0, 0, 0, 0, 0, 0); this->axis_state = state; @@ -79,13 +69,9 @@ void ODriveMotor::call(const std::string method_name, const std::vector<ConstExp } else if (method_name == "off") { Module::expect(arguments, 0); this->off(); - } else if (method_name == "reset_motor") { + } else if (method_name == "reset_motor_error") { Module::expect(arguments, 0); - this->reset_motor(); - } else if (method_name == "clear_errors") { - Module::expect(arguments, 0); - this->clear_errors(); - } else { + this->reset_motor_error(); Module::call(method_name, arguments); } } @@ -95,8 +81,13 @@ void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8 switch (id - this->can_id) { case 0x001: { int axis_state; - std::memcpy(&axis_state, data + 4, 2); + std::memcpy(&axis_state, data + 4, 1); this->axis_state = axis_state; + + int messege_byte; + std::memcpy(&messege_byte, data + 5, 1); + this->properties.at("motor_error_flag")->integer_value = messege_byte & 0x01; + int axis_error; std::memcpy(&axis_error, data, 4); switch (axis_error) { @@ -127,19 +118,6 @@ void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8 break; } break; - case 0x003: { - int motor_error; - std::memcpy(&motor_error, data, 4); - switch (motor_error) { - case MOTOR_ERROR_NONE: - this->properties.at("motor_error")->integer_value = 0; - break; - - default: - this->properties.at("motor_error")->integer_value = 1; - break; - } - } case 0x009: { float tick; std::memcpy(&tick, data, 4); @@ -193,24 +171,15 @@ void ODriveMotor::limits(const float speed, const float current) { void ODriveMotor::off() { this->set_mode(1); // AXIS_STATE_IDLE } -void ODriveMotor::clear_errors() { + +void ODriveMotor::reset_motor_error() { uint8_t empty_data[8] = {0, 0, 0, 0, 0, 0, 0, 0}; this->can->send(this->can_id + 0x018, empty_data, true); } -void ODriveMotor::update_motor_error() { - uint8_t empty_data[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - this->can->send(this->can_id + 0x003, empty_data, true); -} -void ODriveMotor::reset_motor() { - this->clear_errors(); - this->set_mode(8, 2, 1); // AXIS_STATE_CLOSED_LOOP_CONTROL, CONTROL_MODE_VELOCITY_CONTROL, INPUT_MODE_PASSTHROUGH - this->update_motor_error(); -} double ODriveMotor::get_position() { return this->properties.at("position")->number_value; } void ODriveMotor::step() { - this->update_motor_error(); Module::step(); } \ No newline at end of file diff --git a/main/modules/odrive_motor.h b/main/modules/odrive_motor.h index 4b372504..3356a482 100644 --- a/main/modules/odrive_motor.h +++ b/main/modules/odrive_motor.h @@ -29,8 +29,7 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot void position(const float position); void limits(const float speed, const float current); void off(); - void reset_motor(); - void clear_errors(); + void reset_motor_error(); double get_position(); void step() override; }; \ No newline at end of file From e27dcf669494a3a5a719a62f7e1961f5cec2ce62 Mon Sep 17 00:00:00 2001 From: Johannes-Thiel <jojo.thadfun@gmail.com> Date: Wed, 29 May 2024 15:17:08 +0200 Subject: [PATCH 4/9] test --- main/modules/module.cpp | 30 ++++++++++++++++++++++-------- main/modules/odrive_motor.cpp | 14 +++++++------- main/modules/odrive_motor.h | 3 ++- 3 files changed, 31 insertions(+), 16 deletions(-) diff --git a/main/modules/module.cpp b/main/modules/module.cpp index 471417e9..b547c598 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -166,15 +166,29 @@ Module_ptr Module::create(const std::string type, return std::make_shared<McpLinearMotor>(name, mcp, move_in, move_out, end_in, end_out); } } else if (type == "ODriveMotor") { - Module::expect(arguments, 2, identifier, integer); - std::string can_name = arguments[0]->evaluate_identifier(); - Module_ptr module = Global::get_module(can_name); - if (module->type != can) { - throw std::runtime_error("module \"" + can_name + "\" is no can connection"); + if (arguments.size() == 2) { + Module::expect(arguments, 2, identifier, integer); + std::string can_name = arguments[0]->evaluate_identifier(); + Module_ptr module = Global::get_module(can_name); + if (module->type != can) { + throw std::runtime_error("module \"" + can_name + "\" is no can connection"); + } + const Can_ptr can = std::static_pointer_cast<Can>(module); + uint32_t can_id = arguments[1]->evaluate_integer(); + ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id, 4); + } else { + Module::expect(arguments, 3, identifier, integer, integer); + std::string can_name = arguments[0]->evaluate_identifier(); + Module_ptr module = Global::get_module(can_name); + if (module->type != can) { + throw std::runtime_error("module \"" + can_name + "\" is no can connection"); + } + const Can_ptr can = std::static_pointer_cast<Can>(module); + uint32_t can_id = arguments[1]->evaluate_integer(); + int version = arguments[2]->evaluate_integer(); + ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id, version); } - const Can_ptr can = std::static_pointer_cast<Can>(module); - uint32_t can_id = arguments[1]->evaluate_integer(); - ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id); + odrive_motor->subscribe_to_can(); return odrive_motor; } else if (type == "ODriveWheels") { diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 240440ad..8ad18aa7 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -13,8 +13,8 @@ enum AXIS_ERROR { AXIS_ERROR_UNKNOWN_POSITION = 0x80000 }; -ODriveMotor::ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id) : Module(odrive_motor, name), - can_id(can_id), can(can) { +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["position"] = std::make_shared<NumberVariable>(); this->properties["tick_offset"] = std::make_shared<NumberVariable>(); this->properties["m_per_tick"] = std::make_shared<NumberVariable>(1.0); @@ -83,11 +83,11 @@ void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8 int axis_state; std::memcpy(&axis_state, data + 4, 1); this->axis_state = axis_state; - - int messege_byte; - std::memcpy(&messege_byte, data + 5, 1); - this->properties.at("motor_error_flag")->integer_value = messege_byte & 0x01; - + if (version == 6) { + int messege_byte; + std::memcpy(&messege_byte, data + 5, 1); + this->properties.at("motor_error_flag")->integer_value = messege_byte & 0x01; + } int axis_error; std::memcpy(&axis_error, data, 4); switch (axis_error) { diff --git a/main/modules/odrive_motor.h b/main/modules/odrive_motor.h index 3356a482..0c49b8c9 100644 --- a/main/modules/odrive_motor.h +++ b/main/modules/odrive_motor.h @@ -11,6 +11,7 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot private: const uint32_t can_id; const Can_ptr can; + const uint32_t version; bool is_boot_complete = false; uint8_t axis_state = -1; uint8_t axis_control_mode = -1; @@ -20,7 +21,7 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot void update_motor_error(); public: - ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id); + ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id, const uint32_t version); 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; From d9770b87d9bd99e302221bcad389407d1773c170 Mon Sep 17 00:00:00 2001 From: Johannes-Thiel <jojo.thadfun@gmail.com> Date: Tue, 4 Jun 2024 10:03:15 +0200 Subject: [PATCH 5/9] Refactor ODriveMotor module to handle motor errors and reset motor error flag --- docs/module_reference.md | 3 +-- main/modules/module.cpp | 6 ++++-- main/modules/odrive_motor.cpp | 14 +++++++++----- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/docs/module_reference.md b/docs/module_reference.md index 09a03fa2..d2f79d4d 100644 --- a/docs/module_reference.md +++ b/docs/module_reference.md @@ -280,8 +280,7 @@ The ODrive motor module controls a motor using an [ODrive motor controller](http | `motor.position(position)` | Move to given `position` (m) | `float` | | `motor.limits(speed, current)` | Set speed (m/s) and current (A) limits | `float`, `float` | | `motor.off()` | Turn motor off (idle state) | | -| `motor.clear_errors` | Clears all error statements | | -| `motor.reset_motor` | Resets the Motor and clears error statments | | +| `motor.reset_motor_error()` | Resets the Motor and clears error statments | | ## ODrive Wheels diff --git a/main/modules/module.cpp b/main/modules/module.cpp index b547c598..8f6d7f93 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -176,6 +176,8 @@ Module_ptr Module::create(const std::string type, const Can_ptr can = std::static_pointer_cast<Can>(module); uint32_t can_id = arguments[1]->evaluate_integer(); ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id, 4); + odrive_motor->subscribe_to_can(); + return odrive_motor; } else { Module::expect(arguments, 3, identifier, integer, integer); std::string can_name = arguments[0]->evaluate_identifier(); @@ -187,10 +189,10 @@ Module_ptr Module::create(const std::string type, uint32_t can_id = arguments[1]->evaluate_integer(); int version = arguments[2]->evaluate_integer(); ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id, version); + odrive_motor->subscribe_to_can(); + return odrive_motor; } - odrive_motor->subscribe_to_can(); - return odrive_motor; } else if (type == "ODriveWheels") { Module::expect(arguments, 2, identifier, identifier); std::string left_name = arguments[0]->evaluate_identifier(); diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 8ad18aa7..26f3ca06 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -21,6 +21,7 @@ ODriveMotor::ODriveMotor(const std::string name, const Can_ptr can, const uint32 this->properties["reversed"] = std::make_shared<BooleanVariable>(); this->properties["axis_error"] = std::make_shared<IntegerVariable>(); this->properties["motor_error_flag"] = std::make_shared<IntegerVariable>(); + this->properties["axis_state"] = std::make_shared<IntegerVariable>(); } void ODriveMotor::subscribe_to_can() { @@ -32,9 +33,12 @@ void ODriveMotor::set_mode(const uint8_t state, const uint8_t control_mode, cons if (!this->is_boot_complete) { return; } - // if (this->properties.at("motor_error")->number_value == 1) { - // return; - // } + if (this->properties.at("motor_error_flag")->number_value == 1) { + axis_state = -1; + axis_control_mode = -1; + axis_input_mode = -1; + return; + } if (this->axis_state != state) { this->can->send(this->can_id + 0x007, state, 0, 0, 0, 0, 0, 0, 0); this->axis_state = state; @@ -69,10 +73,9 @@ void ODriveMotor::call(const std::string method_name, const std::vector<ConstExp } else if (method_name == "off") { Module::expect(arguments, 0); this->off(); - } else if (method_name == "reset_motor_error") { + } else if (method_name == "reset_motor") { Module::expect(arguments, 0); this->reset_motor_error(); - Module::call(method_name, arguments); } } @@ -83,6 +86,7 @@ void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8 int axis_state; std::memcpy(&axis_state, data + 4, 1); this->axis_state = axis_state; + this->properties.at("axis_state")->integer_value = axis_state; if (version == 6) { int messege_byte; std::memcpy(&messege_byte, data + 5, 1); From 49cb5480813218f1074ddef61b84271a60d1c097 Mon Sep 17 00:00:00 2001 From: Johannes-Thiel <jojo.thadfun@gmail.com> Date: Tue, 4 Jun 2024 14:06:30 +0200 Subject: [PATCH 6/9] Update ODriveMotor module to include new axis state property --- docs/module_reference.md | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/docs/module_reference.md b/docs/module_reference.md index da62f57b..eac93d79 100644 --- a/docs/module_reference.md +++ b/docs/module_reference.md @@ -265,14 +265,15 @@ The ODrive motor module controls a motor using an [ODrive motor controller](http | ---------------------------------- | ---------------------- | ----------------- | | `motor = ODriveMotor(can, can_id)` | CAN module and node ID | CAN module, `int` | -| Properties | Description | Data type | -| ------------------- | ----------------------- | --------- | -| `motor.position` | Motor position (meters) | `float` | -| `motor.tick_offset` | Encoder tick offset | `float` | -| `motor.m_per_tick` | Meters per encoder tick | `float` | -| `motor.reversed` | Reverse motor direction | `bool` | -| `motor.axis_error` | Error code of the axis | `int` | -| `motor.motor_error` | Motor in fault mode | `int` | +| Properties | Description | Data type | +| ------------------- | --------------------------- | --------- | +| `motor.position` | Motor position (meters) | `float` | +| `motor.tick_offset` | Encoder tick offset | `float` | +| `motor.m_per_tick` | Meters per encoder tick | `float` | +| `motor.reversed` | Reverse motor direction | `bool` | +| `motor.axis_error` | Error code of the axis | `int` | +| `motor.motor_error` | Motor in fault mode | `int` | +| `motor.axis_state` | the state of the motor axis | `int` | | Methods | Description | Arguments | | ------------------------------ | ------------------------------------------- | ---------------- | @@ -282,7 +283,7 @@ The ODrive motor module controls a motor using an [ODrive motor controller](http | `motor.position(position)` | Move to given `position` (m) | `float` | | `motor.limits(speed, current)` | Set speed (m/s) and current (A) limits | `float`, `float` | | `motor.off()` | Turn motor off (idle state) | | -| `motor.reset_motor_error()` | Resets the Motor and clears error statments | | +| `motor.reset_motor()` | Resets the Motor and clears error statments | | ## ODrive Wheels From fc24b10d3dc2b20ba4cdafed6692ebacdc4cd400 Mon Sep 17 00:00:00 2001 From: Johannes-Thiel <jojo.thadfun@gmail.com> Date: Wed, 5 Jun 2024 15:41:31 +0200 Subject: [PATCH 7/9] cleanup step function --- main/modules/odrive_motor.cpp | 4 ---- main/modules/odrive_motor.h | 1 - 2 files changed, 5 deletions(-) diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 26f3ca06..8c6b2865 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -183,7 +183,3 @@ void ODriveMotor::reset_motor_error() { double ODriveMotor::get_position() { return this->properties.at("position")->number_value; } - -void ODriveMotor::step() { - Module::step(); -} \ No newline at end of file diff --git a/main/modules/odrive_motor.h b/main/modules/odrive_motor.h index 0c49b8c9..b5db7677 100644 --- a/main/modules/odrive_motor.h +++ b/main/modules/odrive_motor.h @@ -32,5 +32,4 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot void off(); void reset_motor_error(); double get_position(); - void step() override; }; \ No newline at end of file From 07c8dcd366c76af79ea73b295713175e1a434639 Mon Sep 17 00:00:00 2001 From: Falko Schindler <falko@zauberzeug.com> Date: Wed, 5 Jun 2024 16:29:25 +0200 Subject: [PATCH 8/9] code review --- docs/module_reference.md | 48 +++++++++++++------------ main/modules/module.cpp | 40 ++++++++------------- main/modules/odrive_motor.cpp | 68 +++++++++-------------------------- main/modules/odrive_motor.h | 1 - 4 files changed, 56 insertions(+), 101 deletions(-) diff --git a/docs/module_reference.md b/docs/module_reference.md index eac93d79..8189f440 100644 --- a/docs/module_reference.md +++ b/docs/module_reference.md @@ -261,29 +261,31 @@ This module controls a linear actuator via two output pins (move in, move out) a The ODrive motor module controls a motor using an [ODrive motor controller](https://odriverobotics.com/). -| Constructor | Description | Arguments | -| ---------------------------------- | ---------------------- | ----------------- | -| `motor = ODriveMotor(can, can_id)` | CAN module and node ID | CAN module, `int` | - -| Properties | Description | Data type | -| ------------------- | --------------------------- | --------- | -| `motor.position` | Motor position (meters) | `float` | -| `motor.tick_offset` | Encoder tick offset | `float` | -| `motor.m_per_tick` | Meters per encoder tick | `float` | -| `motor.reversed` | Reverse motor direction | `bool` | -| `motor.axis_error` | Error code of the axis | `int` | -| `motor.motor_error` | Motor in fault mode | `int` | -| `motor.axis_state` | the state of the motor axis | `int` | - -| Methods | Description | Arguments | -| ------------------------------ | ------------------------------------------- | ---------------- | -| `motor.zero()` | Set current position as zero position | | -| `motor.power(torque)` | Move with given `torque` | `float` | -| `motor.speed(speed)` | Move with given `speed` (m/s) | `float` | -| `motor.position(position)` | Move to given `position` (m) | `float` | -| `motor.limits(speed, current)` | Set speed (m/s) and current (A) limits | `float`, `float` | -| `motor.off()` | Turn motor off (idle state) | | -| `motor.reset_motor()` | Resets the Motor and clears error statments | | +| Constructor | Description | Arguments | +| --------------------------------------------- | ------------------------------- | ------------------------ | +| `motor = ODriveMotor(can, can_id[, version])` | CAN module, node ID and version | CAN module, `int`, `int` | + +The `version` parameter is an optional integer indicating the patch number of the ODrive firmware (4, 5 or 6; default: 4 for version "0.5.4"). Version 0.5.6 allows to read the motor error flag. + +| Properties | Description | Data type | +| ------------------- | ----------------------------------------- | --------- | +| `motor.position` | Motor position (meters) | `float` | +| `motor.tick_offset` | Encoder tick offset | `float` | +| `motor.m_per_tick` | Meters per encoder tick | `float` | +| `motor.reversed` | Reverse motor direction | `bool` | +| `motor.axis_state` | State of the motor axis | `int` | +| `motor.axis_error` | Error code of the axis | `int` | +| `motor.motor_error` | Motor error flat (requires version 0.5.6) | `int` | + +| Methods | Description | Arguments | +| ------------------------------ | -------------------------------------- | ---------------- | +| `motor.zero()` | Set current position as zero position | | +| `motor.power(torque)` | Move with given `torque` | `float` | +| `motor.speed(speed)` | Move with given `speed` (m/s) | `float` | +| `motor.position(position)` | Move to given `position` (m) | `float` | +| `motor.limits(speed, current)` | Set speed (m/s) and current (A) limits | `float`, `float` | +| `motor.off()` | Turn motor off (idle state) | | +| `motor.reset_motor()` | Resets the motor and clears errors | | ## ODrive Wheels diff --git a/main/modules/module.cpp b/main/modules/module.cpp index 8f6d7f93..d7b1b04c 100644 --- a/main/modules/module.cpp +++ b/main/modules/module.cpp @@ -166,33 +166,21 @@ Module_ptr Module::create(const std::string type, return std::make_shared<McpLinearMotor>(name, mcp, move_in, move_out, end_in, end_out); } } else if (type == "ODriveMotor") { - if (arguments.size() == 2) { - Module::expect(arguments, 2, identifier, integer); - std::string can_name = arguments[0]->evaluate_identifier(); - Module_ptr module = Global::get_module(can_name); - if (module->type != can) { - throw std::runtime_error("module \"" + can_name + "\" is no can connection"); - } - const Can_ptr can = std::static_pointer_cast<Can>(module); - uint32_t can_id = arguments[1]->evaluate_integer(); - ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id, 4); - odrive_motor->subscribe_to_can(); - return odrive_motor; - } else { - Module::expect(arguments, 3, identifier, integer, integer); - std::string can_name = arguments[0]->evaluate_identifier(); - Module_ptr module = Global::get_module(can_name); - if (module->type != can) { - throw std::runtime_error("module \"" + can_name + "\" is no can connection"); - } - const Can_ptr can = std::static_pointer_cast<Can>(module); - uint32_t can_id = arguments[1]->evaluate_integer(); - int version = arguments[2]->evaluate_integer(); - ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id, version); - odrive_motor->subscribe_to_can(); - return odrive_motor; + if (arguments.size() < 2 || arguments.size() > 3) { + throw std::runtime_error("unexpected number of arguments"); } - + Module::expect(arguments, -1, identifier, integer, integer); + std::string can_name = arguments[0]->evaluate_identifier(); + Module_ptr module = Global::get_module(can_name); + if (module->type != can) { + throw std::runtime_error("module \"" + can_name + "\" is no can connection"); + } + const Can_ptr can = std::static_pointer_cast<Can>(module); + uint32_t can_id = arguments[1]->evaluate_integer(); + int version = arguments.size() > 2 ? arguments[2]->evaluate_integer() : 4; + ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id, version); + odrive_motor->subscribe_to_can(); + return odrive_motor; } else if (type == "ODriveWheels") { Module::expect(arguments, 2, identifier, identifier); std::string left_name = arguments[0]->evaluate_identifier(); diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp index 8c6b2865..fff14d66 100644 --- a/main/modules/odrive_motor.cpp +++ b/main/modules/odrive_motor.cpp @@ -2,26 +2,15 @@ #include <cstring> #include <memory> -enum AXIS_ERROR { - AXIS_ERROR_NONE = 0, - AXIS_ERROR_INVALID_STATE = 0x01, - AXIS_ERROR_WATCHDOG_TIMER_EXPIRED = 0x800, - AXIS_ERROR_MIN_ENDSTOP_PRESSED = 0x1000, - AXIS_ERROR_MAX_ENDSTOP_PRESSED = 0x2000, - AXIS_ERROR_ESTOP_REQUESTED = 0x4000, - AXIS_ERROR_OVER_TEMP = 0x40000, - AXIS_ERROR_UNKNOWN_POSITION = 0x80000 -}; - -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) { +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["position"] = std::make_shared<NumberVariable>(); this->properties["tick_offset"] = std::make_shared<NumberVariable>(); this->properties["m_per_tick"] = std::make_shared<NumberVariable>(1.0); this->properties["reversed"] = std::make_shared<BooleanVariable>(); + this->properties["axis_state"] = std::make_shared<IntegerVariable>(); this->properties["axis_error"] = std::make_shared<IntegerVariable>(); this->properties["motor_error_flag"] = std::make_shared<IntegerVariable>(); - this->properties["axis_state"] = std::make_shared<IntegerVariable>(); } void ODriveMotor::subscribe_to_can() { @@ -34,9 +23,9 @@ void ODriveMotor::set_mode(const uint8_t state, const uint8_t control_mode, cons return; } if (this->properties.at("motor_error_flag")->number_value == 1) { - axis_state = -1; - axis_control_mode = -1; - axis_input_mode = -1; + this->axis_state = -1; + this->axis_control_mode = -1; + this->axis_input_mode = -1; return; } if (this->axis_state != state) { @@ -76,6 +65,8 @@ void ODriveMotor::call(const std::string method_name, const std::vector<ConstExp } else if (method_name == "reset_motor") { Module::expect(arguments, 0); this->reset_motor_error(); + } else { + Module::call(method_name, arguments); } } @@ -83,45 +74,20 @@ void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8 this->is_boot_complete = true; switch (id - this->can_id) { case 0x001: { + int axis_error; + std::memcpy(&axis_error, data, 4); + this->properties.at("axis_error")->integer_value = axis_error; int axis_state; std::memcpy(&axis_state, data + 4, 1); this->axis_state = axis_state; this->properties.at("axis_state")->integer_value = axis_state; if (version == 6) { - int messege_byte; - std::memcpy(&messege_byte, data + 5, 1); - this->properties.at("motor_error_flag")->integer_value = messege_byte & 0x01; - } - int axis_error; - std::memcpy(&axis_error, data, 4); - switch (axis_error) { - case AXIS_ERROR_INVALID_STATE: - this->properties.at("axis_error")->integer_value = 1; - break; - case AXIS_ERROR_WATCHDOG_TIMER_EXPIRED: - this->properties.at("axis_error")->integer_value = 2; - break; - case AXIS_ERROR_MIN_ENDSTOP_PRESSED: - this->properties.at("axis_error")->integer_value = 3; - break; - case AXIS_ERROR_MAX_ENDSTOP_PRESSED: - this->properties.at("axis_error")->integer_value = 4; - break; - case AXIS_ERROR_ESTOP_REQUESTED: - this->properties.at("axis_error")->integer_value = 5; - break; - case AXIS_ERROR_OVER_TEMP: - this->properties.at("axis_error")->integer_value = 6; - break; - case AXIS_ERROR_UNKNOWN_POSITION: - this->properties.at("axis_error")->integer_value = 7; - break; - default: - this->properties.at("axis_error")->integer_value = axis_error; - - break; + int message_byte; + std::memcpy(&message_byte, data + 5, 1); + this->properties.at("motor_error_flag")->integer_value = message_byte & 0x01; } break; + } case 0x009: { float tick; std::memcpy(&tick, data, 4); @@ -131,7 +97,6 @@ void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8 this->properties.at("m_per_tick")->number_value; } } - } } void ODriveMotor::power(const float torque) { @@ -178,8 +143,9 @@ void ODriveMotor::off() { void ODriveMotor::reset_motor_error() { uint8_t empty_data[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - this->can->send(this->can_id + 0x018, empty_data, true); + this->can->send(this->can_id + 0x018, empty_data); // "Clear Errors" } + double ODriveMotor::get_position() { return this->properties.at("position")->number_value; } diff --git a/main/modules/odrive_motor.h b/main/modules/odrive_motor.h index b5db7677..fef868b0 100644 --- a/main/modules/odrive_motor.h +++ b/main/modules/odrive_motor.h @@ -18,7 +18,6 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot uint8_t axis_input_mode = -1; void set_mode(const uint8_t state, const uint8_t control_mode = 0, const uint8_t input_mode = 0); - void update_motor_error(); public: ODriveMotor(const std::string name, const Can_ptr can, const uint32_t can_id, const uint32_t version); From 0ae9018a93d582550f41407a874757cec217f99e Mon Sep 17 00:00:00 2001 From: Falko Schindler <falko@zauberzeug.com> Date: Mon, 10 Jun 2024 16:53:32 +0200 Subject: [PATCH 9/9] simplify RMD motor module using command 0x60 --- docs/module_reference.md | 22 +++-------- main/modules/rmd_motor.cpp | 76 +++++++------------------------------- main/modules/rmd_motor.h | 3 -- 3 files changed, 20 insertions(+), 81 deletions(-) diff --git a/docs/module_reference.md b/docs/module_reference.md index d9d1eef4..18656846 100644 --- a/docs/module_reference.md +++ b/docs/module_reference.md @@ -308,19 +308,16 @@ Now the vehicle can be pushed manually with motors turned off, without taking ca ## RMD Motor -The RMD motor module controls a [Gyems](http://www.gyems.cn/) RMD motor via CAN. +The RMD motor module controls a [MyActuator](https://www.myactuator.com/) RMD motor via CAN. | Constructor | Description | Arguments | | -------------------------------------- | -------------------------------------------------- | ------------------------ | | `rmd = RmdMotor(can, motor_id, ratio)` | CAN module, motor ID (1..8) and transmission ratio | CAN module, `int`, `int` | -| Properties | Description | Data type | -| ----------------- | ------------------------------------------ | --------- | -| `rmd.position` | Multi-turn motor position (deg) | `float` | -| `rmd.torque` | Current torque | `float` | -| `rmd.speed` | Current speed (deg/s) | `float` | -| `rmd.temperature` | Current temperature (C) | `float` | -| `rmd.can_age` | Time since last CAN message from motor (s) | `float` | +| Properties | Description | Data type | +| -------------- | ------------------------------------------ | --------- | +| `rmd.position` | Multi-turn motor position (deg) | `float` | +| `rmd.can_age` | Time since last CAN message from motor (s) | `float` | | Methods | Description | Arguments | | --------------------------- | ----------------------------------------------------------------- | ---------------- | @@ -335,15 +332,8 @@ The RMD motor module controls a [Gyems](http://www.gyems.cn/) RMD motor via CAN. | `rmd.set_pid(...)` | Set PID parameters Kp/Ki for position/speed/torque loop | 6x `int` | | `rmd.get_acceleration()` | Print acceleration (deg/s^2) | | | `rmd.set_acceleration(...)` | Set accelerations/decelerations for position/speed loop (deg/s^2) | 4x `int` | -| `rmd.get_errors()` | Print motor error code | | +| `rmd.get_status()` | Print temperature [˚C], voltage [V] and motor error code | | | `rmd.clear_errors()` | Clear motor error | | -| `rmd.zero()` | Write position to ROM as zero position (see below) | | - -**The zero command** - -The `zero()` method should be used with care! -In contrast to other commands it blocks the main loop for up to 200 ms and requires restarting the motor to take effect. -Furthermore, multiple writes will affect the chip life, thus it is not recommended to use it frequently. **Set acceleration** diff --git a/main/modules/rmd_motor.cpp b/main/modules/rmd_motor.cpp index 50c6ef8a..9a3808a6 100644 --- a/main/modules/rmd_motor.cpp +++ b/main/modules/rmd_motor.cpp @@ -3,15 +3,13 @@ #include "../utils/timing.h" #include "../utils/uart.h" #include <cstring> +#include <inttypes.h> #include <math.h> #include <memory> 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) { + : Module(rmd_motor, name), motor_id(motor_id), can(can), ratio(ratio) { this->properties["position"] = std::make_shared<NumberVariable>(); - this->properties["torque"] = std::make_shared<NumberVariable>(); - this->properties["speed"] = std::make_shared<NumberVariable>(); - this->properties["temperature"] = std::make_shared<NumberVariable>(); this->properties["can_age"] = std::make_shared<NumberVariable>(); } @@ -41,11 +39,7 @@ void RmdMotor::send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const void RmdMotor::step() { this->properties.at("can_age")->number_value = millis_since(this->last_msg_millis) / 1e3; - if (!this->has_last_encoder_position) { - this->send(0x92, 0, 0, 0, 0, 0, 0, 0); - } - - this->send(0x9c, 0, 0, 0, 0, 0, 0, 0); + this->send(0x60, 0, 0, 0, 0, 0, 0, 0); Module::step(); } @@ -100,11 +94,7 @@ void RmdMotor::clear_errors() { } void RmdMotor::call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) { - if (method_name == "zero") { - Module::expect(arguments, 0); - this->send(0x64, 0, 0, 0, 0, 0, 0, 0); - this->send(0x76, 0, 0, 0, 0, 0, 0, 0); - } else if (method_name == "power") { + if (method_name == "power") { Module::expect(arguments, 1, numbery); this->power(arguments[0]->evaluate_number()); } else if (method_name == "speed") { @@ -150,7 +140,7 @@ void RmdMotor::call(const std::string method_name, const std::vector<ConstExpres set_acceleration(i, acceleration); } } - } else if (method_name == "get_errors") { + } else if (method_name == "get_status") { Module::expect(arguments, 0); this->send(0x9a, 0, 0, 0, 0, 0, 0, 0); } else if (method_name == "clear_errors") { @@ -161,54 +151,12 @@ void RmdMotor::call(const std::string method_name, const std::vector<ConstExpres } } -double modulo_encoder_range(double position, double range) { - double result = std::fmod(position, range); - if (result > range / 2) { - return result - range; - } - if (result < -range / 2) { - return result + range; - } - return result; -} - void RmdMotor::handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) { switch (data[0]) { - case 0x92: { - int32_t position = 0; - std::memcpy(&position, data + 4, 4); - this->properties.at("position")->number_value = 0.01 * position; - this->last_encoder_position = modulo_encoder_range(0.01 * position, this->encoder_range); - this->has_last_encoder_position = true; - break; - } - case 0x9c: { - int8_t temperature = 0; - std::memcpy(&temperature, data + 1, 1); - this->properties.at("temperature")->number_value = temperature; - - int16_t torque = 0; - std::memcpy(&torque, data + 2, 2); - this->properties.at("torque")->number_value = 0.01 * torque; - - int16_t speed = 0; - std::memcpy(&speed, data + 4, 2); - this->properties.at("speed")->number_value = speed; - - int16_t position = 0; - std::memcpy(&position, data + 6, 2); - int32_t encoder_position = position; - if (this->has_last_encoder_position) { - this->properties.at("position")->number_value += encoder_position - this->last_encoder_position; - if (encoder_position - this->last_encoder_position > this->encoder_range / 2) { - this->properties.at("position")->number_value -= this->encoder_range; - } - if (encoder_position - this->last_encoder_position < -this->encoder_range / 2) { - this->properties.at("position")->number_value += this->encoder_range; - } - this->last_encoder_position = encoder_position; - } - + case 0x60: { + int32_t encoder = 0; + std::memcpy(&encoder, data + 4, 4); + this->properties.at("position")->number_value = encoder / 16384.0 * 360.0 / this->ratio; // 16384 = 2^14 break; } case 0x30: { @@ -237,9 +185,13 @@ void RmdMotor::handle_can_msg(const uint32_t id, const int count, const uint8_t break; } case 0x9a: { + int8_t temperature = 0; + std::memcpy(&temperature, data + 1, 1); + uint16_t voltage = 0; + std::memcpy(&voltage, data + 4, 2); uint16_t errors = 0; std::memcpy(&errors, data + 6, 2); - echo("%s.errors %d", this->name.c_str(), errors); + echo("%s.status %d %.1f %d", this->name.c_str(), temperature, (float)voltage / 10.0, errors); break; } } diff --git a/main/modules/rmd_motor.h b/main/modules/rmd_motor.h index 6e4fd70b..1dc42336 100644 --- a/main/modules/rmd_motor.h +++ b/main/modules/rmd_motor.h @@ -14,9 +14,6 @@ class RmdMotor : public Module, public std::enable_shared_from_this<RmdMotor> { const Can_ptr can; uint8_t last_msg_id = 0; int ratio; - const double encoder_range; - int32_t last_encoder_position; - bool has_last_encoder_position = false; unsigned long int last_msg_millis = 0; void send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const uint8_t d3,