Skip to content

Commit

Permalink
Merge branch 'main' of github.com:zauberzeug/lizard
Browse files Browse the repository at this point in the history
  • Loading branch information
@Johannes-Thiel
Johannes-Thiel committed Jun 11, 2024
2 parents 316b4bc + 4f326c5 commit f86d3e4
Show file tree
Hide file tree
Showing 6 changed files with 75 additions and 97 deletions.
48 changes: 22 additions & 26 deletions docs/module_reference.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,16 +261,21 @@ 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` |
| 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 |
| ------------------------------ | -------------------------------------- | ---------------- |
Expand All @@ -280,6 +285,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()` | Resets the motor and clears errors | |

## ODrive Wheels

Expand Down Expand Up @@ -308,19 +314,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 |
| --------------------------- | ----------------------------------------------------------------- | ---------------- |
Expand All @@ -335,15 +338,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**

Expand Down
8 changes: 6 additions & 2 deletions main/modules/module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,15 +166,19 @@ 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);
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();
ODriveMotor_ptr odrive_motor = std::make_shared<ODriveMotor>(name, can, can_id);
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") {
Expand Down
34 changes: 31 additions & 3 deletions main/modules/odrive_motor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,23 +2,33 @@
#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) {
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["speed"] = 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>();
}

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 + 0x009, std::static_pointer_cast<Module>(this->shared_from_this()));
}

void ODriveMotor::set_mode(const uint8_t state, const uint8_t control_mode, const uint8_t input_mode) {
if (!this->is_boot_complete) {
return;
}
if (this->properties.at("motor_error_flag")->number_value == 1) {
this->axis_state = -1;
this->axis_control_mode = -1;
this->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;
Expand Down Expand Up @@ -53,6 +63,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") {
Module::expect(arguments, 0);
this->reset_motor_error();
} else {
Module::call(method_name, arguments);
}
Expand All @@ -61,6 +74,21 @@ 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_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 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);
Expand Down Expand Up @@ -142,4 +170,4 @@ double ODriveMotor::get_speed() {

void ODriveMotor::speed(const double speed, const double acceleration) {
this->speed(static_cast<float>(speed));
}
}
3 changes: 2 additions & 1 deletion main/modules/odrive_motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,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;
Expand All @@ -20,7 +21,7 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot
void set_mode(const uint8_t state, const uint8_t control_mode = 0, const uint8_t input_mode = 0);

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;
Expand Down
76 changes: 14 additions & 62 deletions main/modules/rmd_motor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>();
}

Expand Down Expand Up @@ -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();
}

Expand Down Expand Up @@ -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") {
Expand Down Expand Up @@ -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") {
Expand All @@ -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: {
Expand Down Expand Up @@ -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;
}
}
Expand Down
3 changes: 0 additions & 3 deletions main/modules/rmd_motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down

0 comments on commit f86d3e4

Please sign in to comment.