Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Re-introduce RMD properties (speed, torque, temperature) and improve RMD pair synchronization #52

Merged
merged 3 commits into from
Jul 2, 2024
Merged

Re-introduce RMD properties (speed, torque, temperature) and improve RMD pair synchronization #52

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
392114d
prevent RMD pair from moving out of sync
falkoschindler Jun 27, 2024
3116645
switch back to 0x92 and 0x9C instead of 0x60
falkoschindler Jun 27, 2024
f490b7a
fix number of zeros
falkoschindler Jun 27, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 7 additions & 4 deletions docs/module_reference.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
| --------------------------- | ----------------------------------------------------------------- | ---------------- |
Expand Down
147 changes: 100 additions & 47 deletions main/modules/rmd_motor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,21 +3,23 @@
#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) {
: Module(rmd_motor, name), motor_id(motor_id), can(can), ratio(ratio), encoder_range(262144.0 / 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>();
}

void RmdMotor::subscribe_to_can() {
can->subscribe(this->motor_id + 0x240, std::static_pointer_cast<Module>(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;
Expand All @@ -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<ConstExpression_ptr> arguments) {
Expand Down Expand Up @@ -151,6 +157,17 @@ 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 0x60: {
Expand Down Expand Up @@ -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();
Expand All @@ -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);
}
21 changes: 12 additions & 9 deletions main/modules/rmd_motor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,12 @@ 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,
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);

Expand All @@ -27,15 +30,15 @@ class RmdMotor : public Module, public std::enable_shared_from_this<RmdMotor> {
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;

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);
};
17 changes: 11 additions & 6 deletions main/modules/rmd_pair.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<ConstExpression_ptr> arguments) {
Expand Down