add enable and disable to actuators (#139)

* add enable and disable to actuators

* reset motor errors on odrive when enabled

* fix table alignment with prettier

* remove comments in canopen_motor

* change enabled to true

* remove enabled from d1 setup

* add notes about the `active` property

* review documentation

* remove inconsistently placed comments

* some cleanup

* first act, then mark module as disabled

* simplify output.cpp a bit

* call the right methods; swap if statement for consistency

* fix end-of-file

* add guard around `output.pulse`

* code review

---------

Co-authored-by: Falko Schindler <falko@zauberzeug.com>

Author: Johannes-Thiel

docs/module_reference.md

@@ -69,6 +69,7 @@ const std::map<std::string, Variable_ptr> CanOpenMotor::get_defaults() {
         {PROP_PV_IS_MOVING, std::make_shared<BooleanVariable>(false)},
         {PROP_CTRL_ENA_OP, std::make_shared<BooleanVariable>(false)},
         {PROP_CTRL_HALT, std::make_shared<BooleanVariable>(true)},
+        {"enabled", std::make_shared<BooleanVariable>(true)},
     };
 }
 
@@ -523,6 +524,9 @@ double CanOpenMotor::get_position() {
 }
 
 void CanOpenMotor::position(const double position, const double speed, const double acceleration) {
+    if (!this->enabled) {
+        return;
+    }
     this->enter_position_mode(static_cast<int32_t>(speed));
     this->send_target_position(static_cast<int32_t>(position) + this->properties[PROP_OFFSET]->integer_value);
     send_control_word(build_ctrl_word(true));
@@ -533,7 +537,39 @@ double CanOpenMotor::get_speed() {
 }
 
 void CanOpenMotor::speed(const double speed, const double acceleration) {
+    if (!this->enabled) {
+        return;
+    }
     this->enter_velocity_mode(speed);
     this->properties[PROP_CTRL_HALT]->boolean_value = false;
     send_control_word(build_ctrl_word(false));
 }
+
+void CanOpenMotor::step() {
+    if (!this->properties[PROP_INITIALIZED]->boolean_value) {
+        return;
+    }
+
+    if (this->properties["enabled"]->boolean_value != this->enabled) {
+        if (this->properties["enabled"]->boolean_value) {
+            this->enable();
+        } else {
+            this->disable();
+        }
+    }
+}
+
+void CanOpenMotor::enable() {
+    this->enabled = true;
+    this->properties["enabled"]->boolean_value = true;
+    this->properties[PROP_CTRL_ENA_OP]->boolean_value = true;
+    send_control_word(build_ctrl_word(false));
+}
+
+void CanOpenMotor::disable() {
+    this->stop();
+    this->properties[PROP_CTRL_ENA_OP]->boolean_value = false;
+    send_control_word(build_ctrl_word(false));
+    this->enabled = false;
+    this->properties["enabled"]->boolean_value = false;
+}

main/modules/canopen_motor.cpp

@@ -13,6 +13,7 @@ using CanOpenMotor_ptr = std::shared_ptr<CanOpenMotor>;
 class CanOpenMotor : public Module, public std::enable_shared_from_this<CanOpenMotor>, virtual public Motor {
     Can_ptr can;
     const uint8_t node_id;
+    bool enabled = true;
 
     enum InitState init_state = WaitingForPreoperational;
 
@@ -66,4 +67,7 @@ class CanOpenMotor : public Module, public std::enable_shared_from_this<CanOpenM
     void position(const double position, const double speed, const double acceleration) override;
     double get_speed() override;
     void speed(const double speed, const double acceleration) override;
+    void enable() override;
+    void disable() override;
+    void step() override;
 };

main/modules/canopen_motor.h

@@ -18,6 +18,7 @@ const std::map<std::string, Variable_ptr> D1Motor::get_defaults() {
         {"velocity", std::make_shared<IntegerVariable>()},
         {"status_word", std::make_shared<IntegerVariable>(-1)},
         {"status_flags", std::make_shared<IntegerVariable>()},
+        {"enabled", std::make_shared<BooleanVariable>(true)},
     };
 }
 
@@ -120,6 +121,12 @@ void D1Motor::call(const std::string method_name, const std::vector<ConstExpress
     } else if (method_name == "nmt_write") {
         Module::expect(arguments, 1, integer);
         this->nmt_write(arguments[0]->evaluate_integer());
+    } else if (method_name == "enable") {
+        Module::expect(arguments, 0);
+        this->enable();
+    } else if (method_name == "disable") {
+        Module::expect(arguments, 0);
+        this->disable();
     } else {
         throw std::runtime_error("Method " + method_name + " not found");
     }
@@ -130,6 +137,13 @@ void D1Motor::step() {
     this->sdo_read(0x2014, 0);
     this->sdo_read(0x6064, 0);
     this->sdo_read(0x606C, 0);
+    if (this->properties.at("enabled")->boolean_value != this->enabled) {
+        if (this->properties.at("enabled")->boolean_value) {
+            this->enable();
+        } else {
+            this->disable();
+        }
+    }
     Module::step();
 }
 
@@ -160,6 +174,8 @@ void D1Motor::setup() {
 }
 
 void D1Motor::home() {
+    if (!this->enabled)
+        return;
     this->sdo_write(0x6060, 0, 8, 6);
     // set specific homing parameters
     this->sdo_write(0x6099, 1, 32, this->properties["switch_search_speed"]->integer_value);
@@ -170,6 +186,8 @@ void D1Motor::home() {
 }
 
 void D1Motor::profile_position(const int32_t position) {
+    if (!this->enabled)
+        return;
     // set mode to profile position mode
     this->sdo_write(0x6060, 0, 8, 1);
     // commit target position
@@ -185,6 +203,8 @@ void D1Motor::profile_position(const int32_t position) {
 }
 
 void D1Motor::profile_velocity(const int32_t velocity) {
+    if (!this->enabled)
+        return;
     // set mode to profile velocity mode
     this->sdo_write(0x6060, 0, 8, 3);
     // commit target velocity
@@ -199,3 +219,14 @@ void D1Motor::profile_velocity(const int32_t velocity) {
 void D1Motor::stop() {
     this->sdo_write(0x6040, 0, 16, 7);
 }
+
+void D1Motor::enable() {
+    this->enabled = true;
+    this->properties.at("enabled")->boolean_value = true;
+}
+
+void D1Motor::disable() {
+    this->stop();
+    this->enabled = false;
+    this->properties.at("enabled")->boolean_value = false;
+}

main/modules/d1_motor.cpp

@@ -14,6 +14,7 @@ class D1Motor : public Module, public std::enable_shared_from_this<D1Motor> {
     const uint8_t node_id;
     int waiting_nmt_writes = 0;
     int waiting_sdo_writes = 0;
+    bool enabled = true;
 
     void sdo_read(const uint16_t index, const uint8_t sub);
     void nmt_write(const uint8_t cs);
@@ -32,4 +33,6 @@ class D1Motor : public Module, public std::enable_shared_from_this<D1Motor> {
     void profile_position(const int32_t position);
     void profile_velocity(const int32_t velocity);
     void stop();
+    void enable();
+    void disable();
 };

main/modules/d1_motor.h

@@ -13,6 +13,7 @@ const std::map<std::string, Variable_ptr> DunkerMotor::get_defaults() {
         {"voltage_power", std::make_shared<NumberVariable>()},
         {"m_per_turn", std::make_shared<NumberVariable>(1.0)},
         {"reversed", std::make_shared<BooleanVariable>()},
+        {"enabled", std::make_shared<BooleanVariable>(true)},
     };
 }
 
@@ -110,10 +111,10 @@ void DunkerMotor::call(const std::string method_name, const std::vector<ConstExp
         this->sdo_write(index, sub, bits, value);
     } else if (method_name == "enable") {
         Module::expect(arguments, 0);
-        this->sdo_write(0x4004, 1, 8, 1);
+        this->enable();
     } else if (method_name == "disable") {
         Module::expect(arguments, 0);
-        this->sdo_write(0x4004, 1, 8, 0);
+        this->disable();
     } else if (method_name == "speed") {
         Module::expect(arguments, 1, numbery);
         this->speed(arguments[0]->evaluate_number());
@@ -149,6 +150,8 @@ void DunkerMotor::handle_can_msg(const uint32_t id, const int count, const uint8
 }
 
 void DunkerMotor::speed(const double speed) {
+    if (!this->enabled)
+        return;
     const int32_t motor_speed = speed /
                                 this->properties.at("m_per_turn")->number_value /
                                 (this->properties.at("reversed")->boolean_value ? -1 : 1) *
@@ -159,3 +162,26 @@ void DunkerMotor::speed(const double speed) {
 double DunkerMotor::get_speed() {
     return this->properties.at("speed")->number_value;
 }
+
+void DunkerMotor::enable() {
+    this->enabled = true;
+    this->properties.at("enabled")->boolean_value = true;
+    this->sdo_write(0x4004, 1, 8, 1);
+}
+
+void DunkerMotor::disable() {
+    this->sdo_write(0x4004, 1, 8, 0);
+    this->enabled = false;
+    this->properties.at("enabled")->boolean_value = false;
+}
+
+void DunkerMotor::step() {
+    if (this->properties.at("enabled")->boolean_value != this->enabled) {
+        if (this->properties.at("enabled")->boolean_value) {
+            this->enable();
+        } else {
+            this->disable();
+        }
+    }
+    Module::step();
+}

main/modules/dunker_motor.cpp

@@ -14,6 +14,7 @@ class DunkerMotor : public Module, public std::enable_shared_from_this<DunkerMot
     const uint8_t node_id;
     int waiting_nmt_writes = 0;
     int waiting_sdo_writes = 0;
+    bool enabled = true;
 
     void sdo_read(const uint16_t index, const uint8_t sub);
     void nmt_write(const uint8_t cs);
@@ -28,4 +29,7 @@ class DunkerMotor : public Module, public std::enable_shared_from_this<DunkerMot
     static const std::map<std::string, Variable_ptr> get_defaults();
     void speed(const double speed);
     double get_speed();
+    void enable();
+    void disable();
+    void step() override;
 };

main/modules/dunker_motor.h

@@ -8,6 +8,7 @@ const std::map<std::string, Variable_ptr> DunkerWheels::get_defaults() {
         {"width", std::make_shared<NumberVariable>(1.0)},
         {"linear_speed", std::make_shared<NumberVariable>()},
         {"angular_speed", std::make_shared<NumberVariable>()},
+        {"enabled", std::make_shared<BooleanVariable>(true)},
     };
 }
 
@@ -23,18 +24,48 @@ void DunkerWheels::step() {
     this->properties.at("linear_speed")->number_value = (left_speed + right_speed) / 2;
     this->properties.at("angular_speed")->number_value = (right_speed - left_speed) / this->properties.at("width")->number_value;
 
+    if (this->properties.at("enabled")->boolean_value != this->enabled) {
+        if (this->properties.at("enabled")->boolean_value) {
+            this->enable();
+        } else {
+            this->disable();
+        }
+    }
+
     Module::step();
 }
 
 void DunkerWheels::call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) {
     if (method_name == "speed") {
         Module::expect(arguments, 2, numbery, numbery);
+        if (!this->enabled)
+            return;
         double linear = arguments[0]->evaluate_number();
         double angular = arguments[1]->evaluate_number();
         double width = this->properties.at("width")->number_value;
         this->left_motor->speed(linear - angular * width / 2.0);
         this->right_motor->speed(linear + angular * width / 2.0);
+    } else if (method_name == "enable") {
+        Module::expect(arguments, 0);
+        this->enable();
+    } else if (method_name == "disable") {
+        Module::expect(arguments, 0);
+        this->disable();
     } else {
         Module::call(method_name, arguments);
     }
 }
+
+void DunkerWheels::enable() {
+    this->enabled = true;
+    this->properties.at("enabled")->boolean_value = true;
+    this->left_motor->enable();
+    this->right_motor->enable();
+}
+
+void DunkerWheels::disable() {
+    this->left_motor->disable();
+    this->right_motor->disable();
+    this->enabled = false;
+    this->properties.at("enabled")->boolean_value = false;
+}

main/modules/dunker_wheels.cpp

@@ -7,10 +7,13 @@ class DunkerWheels : public Module {
 private:
     const DunkerMotor_ptr left_motor;
     const DunkerMotor_ptr right_motor;
+    bool enabled = true;
 
 public:
     DunkerWheels(const std::string name, const DunkerMotor_ptr left_motor, const DunkerMotor_ptr right_motor);
     void step() override;
     void call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) override;
     static const std::map<std::string, Variable_ptr> get_defaults();
+    void enable();
+    void disable();
 };

main/modules/dunker_wheels.h

@@ -7,6 +7,7 @@ const std::map<std::string, Variable_ptr> LinearMotor::get_defaults() {
     return {
         {"in", std::make_shared<BooleanVariable>()},
         {"out", std::make_shared<BooleanVariable>()},
+        {"enabled", std::make_shared<BooleanVariable>(true)},
     };
 }
 
@@ -17,27 +18,58 @@ LinearMotor::LinearMotor(const std::string name) : Module(output, name) {
 void LinearMotor::step() {
     this->properties.at("in")->boolean_value = this->get_in();
     this->properties.at("out")->boolean_value = this->get_out();
+
+    if (this->properties.at("enabled")->boolean_value != this->enabled) {
+        if (this->properties.at("enabled")->boolean_value) {
+            this->enable();
+        } else {
+            this->disable();
+        }
+    }
+
     Module::step();
 }
 
 void LinearMotor::call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) {
     if (method_name == "in") {
         Module::expect(arguments, 0);
+        if (!this->enabled)
+            return;
         this->set_in(1);
         this->set_out(0);
     } else if (method_name == "out") {
         Module::expect(arguments, 0);
+        if (!this->enabled)
+            return;
         this->set_in(0);
         this->set_out(1);
     } else if (method_name == "stop") {
         Module::expect(arguments, 0);
         this->set_in(0);
         this->set_out(0);
+    } else if (method_name == "enable") {
+        Module::expect(arguments, 0);
+        this->enable();
+    } else if (method_name == "disable") {
+        Module::expect(arguments, 0);
+        this->disable();
     } else {
         Module::call(method_name, arguments);
     }
 }
 
+void LinearMotor::enable() {
+    this->enabled = true;
+    this->properties.at("enabled")->boolean_value = true;
+}
+
+void LinearMotor::disable() {
+    this->set_in(0);
+    this->set_out(0);
+    this->enabled = false;
+    this->properties.at("enabled")->boolean_value = false;
+}
+
 GpioLinearMotor::GpioLinearMotor(const std::string name,
                                  const gpio_num_t move_in,
                                  const gpio_num_t move_out,