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,