diff --git a/docs/module_reference.md b/docs/module_reference.md index 45a7ad35..f9af3819 100644 --- a/docs/module_reference.md +++ b/docs/module_reference.md @@ -320,10 +320,13 @@ The RMD motor module controls a [MyActuator](https://www.myactuator.com/) RMD mo | -------------------------------------- | -------------------------------------------------- | ------------------------ | | `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.can_age` | Time since last CAN message from motor (s) | `float` | +| 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` | | Methods | Description | Arguments | | --------------------------- | ----------------------------------------------------------------- | ---------------- | diff --git a/main/modules/rmd_motor.cpp b/main/modules/rmd_motor.cpp index 9a3808a6..85096baf 100644 --- a/main/modules/rmd_motor.cpp +++ b/main/modules/rmd_motor.cpp @@ -3,13 +3,15 @@ #include "../utils/timing.h" #include "../utils/uart.h" #include -#include #include #include 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) { + : Module(rmd_motor, name), motor_id(motor_id), can(can), ratio(ratio), encoder_range(262144.0 / ratio) { this->properties["position"] = std::make_shared(); + this->properties["torque"] = std::make_shared(); + this->properties["speed"] = std::make_shared(); + this->properties["temperature"] = std::make_shared(); this->properties["can_age"] = std::make_shared(); } @@ -17,7 +19,7 @@ void RmdMotor::subscribe_to_can() { can->subscribe(this->motor_id + 0x240, std::static_pointer_cast(this->shared_from_this())); } -void RmdMotor::send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const uint8_t d3, +bool RmdMotor::send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const uint8_t d3, const uint8_t d4, const uint8_t d5, const uint8_t d6, const uint8_t d7, const unsigned long int timeout_ms) { this->last_msg_id = 0; @@ -29,68 +31,72 @@ void RmdMotor::send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const this->can->receive(); } if (this->last_msg_id == d0) { - return; + return true; } else { echo("%s warning: CAN timeout for msg id 0x%02x (attempt %d/%d)", this->name.c_str(), d0, i + 1, max_attempts); } } + return false; } void RmdMotor::step() { this->properties.at("can_age")->number_value = millis_since(this->last_msg_millis) / 1e3; - this->send(0x60, 0, 0, 0, 0, 0, 0, 0); + 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); Module::step(); } -void RmdMotor::power(double target_power) { +bool RmdMotor::power(double target_power) { int16_t power = target_power * 100; - this->send(0xa1, 0, - 0, - 0, - *((uint8_t *)(&power) + 0), - *((uint8_t *)(&power) + 1), - 0, - 0); + return this->send(0xa1, 0, + 0, + 0, + *((uint8_t *)(&power) + 0), + *((uint8_t *)(&power) + 1), + 0, + 0); } -void RmdMotor::speed(double target_speed) { +bool RmdMotor::speed(double target_speed) { int32_t speed = target_speed * 100; - this->send(0xa2, 0, - 0, - 0, - *((uint8_t *)(&speed) + 0), - *((uint8_t *)(&speed) + 1), - *((uint8_t *)(&speed) + 2), - *((uint8_t *)(&speed) + 3)); + return this->send(0xa2, 0, + 0, + 0, + *((uint8_t *)(&speed) + 0), + *((uint8_t *)(&speed) + 1), + *((uint8_t *)(&speed) + 2), + *((uint8_t *)(&speed) + 3)); } -void RmdMotor::position(double target_position, double target_speed) { +bool RmdMotor::position(double target_position, double target_speed) { int32_t position = target_position * 100; uint16_t speed = target_speed; - this->send(0xa4, 0, - *((uint8_t *)(&speed) + 0), - *((uint8_t *)(&speed) + 1), - *((uint8_t *)(&position) + 0), - *((uint8_t *)(&position) + 1), - *((uint8_t *)(&position) + 2), - *((uint8_t *)(&position) + 3)); + return this->send(0xa4, 0, + *((uint8_t *)(&speed) + 0), + *((uint8_t *)(&speed) + 1), + *((uint8_t *)(&position) + 0), + *((uint8_t *)(&position) + 1), + *((uint8_t *)(&position) + 2), + *((uint8_t *)(&position) + 3)); } -void RmdMotor::stop() { - this->send(0x81, 0, 0, 0, 0, 0, 0, 0); +bool RmdMotor::stop() { + return this->send(0x81, 0, 0, 0, 0, 0, 0, 0); } -void RmdMotor::off() { - this->send(0x80, 0, 0, 0, 0, 0, 0, 0); +bool RmdMotor::off() { + return this->send(0x80, 0, 0, 0, 0, 0, 0, 0); } -void RmdMotor::hold() { - this->position(this->properties.at("position")->number_value); +bool RmdMotor::hold() { + return this->position(this->properties.at("position")->number_value); } -void RmdMotor::clear_errors() { - this->send(0x76, 0, 0, 0, 0, 0, 0, 0); +bool RmdMotor::clear_errors() { + return this->send(0x76, 0, 0, 0, 0, 0, 0, 0); } void RmdMotor::call(const std::string method_name, const std::vector arguments) { @@ -151,6 +157,17 @@ void RmdMotor::call(const std::string method_name, const std::vector 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 0x60: { @@ -194,6 +211,42 @@ void RmdMotor::handle_can_msg(const uint32_t id, const int count, const uint8_t echo("%s.status %d %.1f %d", this->name.c_str(), temperature, (float)voltage / 10.0, errors); break; } + 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; + } + break; + } } this->last_msg_id = data[0]; this->last_msg_millis = millis(); @@ -207,14 +260,14 @@ double RmdMotor::get_speed() const { return this->properties.at("speed")->number_value; } -void RmdMotor::set_acceleration(const uint8_t index, const uint32_t acceleration) { - this->send(0x43, - index, - 0, - 0, - *((uint8_t *)(&acceleration) + 0), - *((uint8_t *)(&acceleration) + 1), - *((uint8_t *)(&acceleration) + 2), - *((uint8_t *)(&acceleration) + 3), - 20); +bool RmdMotor::set_acceleration(const uint8_t index, const uint32_t acceleration) { + return this->send(0x43, + index, + 0, + 0, + *((uint8_t *)(&acceleration) + 0), + *((uint8_t *)(&acceleration) + 1), + *((uint8_t *)(&acceleration) + 2), + *((uint8_t *)(&acceleration) + 3), + 20); } diff --git a/main/modules/rmd_motor.h b/main/modules/rmd_motor.h index 1dc42336..713613ed 100644 --- a/main/modules/rmd_motor.h +++ b/main/modules/rmd_motor.h @@ -14,9 +14,12 @@ class RmdMotor : public Module, public std::enable_shared_from_this { 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, + bool send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const uint8_t d3, const uint8_t d4, const uint8_t d5, const uint8_t d6, const uint8_t d7, const unsigned long int timeout_ms = 3); @@ -27,15 +30,15 @@ class RmdMotor : public Module, public std::enable_shared_from_this { void call(const std::string method_name, const std::vector arguments) override; void handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) override; - void power(double target_power); - void speed(double target_speed); - void position(double target_position, double target_speed = 0.0); - void stop(); - void off(); - void hold(); - void clear_errors(); + bool power(double target_power); + bool speed(double target_speed); + bool position(double target_position, double target_speed = 0.0); + bool stop(); + bool off(); + bool hold(); + bool clear_errors(); double get_position() const; double get_speed() const; - void set_acceleration(const uint8_t index, const uint32_t acceleration); + bool set_acceleration(const uint8_t index, const uint32_t acceleration); }; \ No newline at end of file diff --git a/main/modules/rmd_pair.cpp b/main/modules/rmd_pair.cpp index cd463929..4b29a118 100644 --- a/main/modules/rmd_pair.cpp +++ b/main/modules/rmd_pair.cpp @@ -68,12 +68,17 @@ void RmdPair::move(double x, double y) { throttle(t2.part_a, duration / duration2); throttle(t2.part_b, duration / duration2); throttle(t2.part_c, duration / duration2); - this->rmd1->set_acceleration(0, std::abs(t1.part_a.a)); - this->rmd1->set_acceleration(1, std::abs(t1.part_c.a)); - this->rmd2->set_acceleration(0, std::abs(t2.part_a.a)); - this->rmd2->set_acceleration(1, std::abs(t2.part_c.a)); - this->rmd1->position(x, std::abs(t1.part_b.v0)); - this->rmd2->position(y, std::abs(t2.part_b.v0)); + if (!( + this->rmd1->set_acceleration(0, std::abs(t1.part_a.a)) && + this->rmd1->set_acceleration(1, std::abs(t1.part_c.a)) && + this->rmd2->set_acceleration(0, std::abs(t2.part_a.a)) && + this->rmd2->set_acceleration(1, std::abs(t2.part_c.a)) && + this->rmd1->position(x, std::abs(t1.part_b.v0)) && + this->rmd2->position(y, std::abs(t2.part_b.v0)))) { + echo("error: could not move RMD motor pair"); + this->rmd1->stop(); + this->rmd2->stop(); + } } void RmdPair::call(const std::string method_name, const std::vector arguments) {