From 0911b6d454a7d40734d3c4d4266aa4bc58a6662c Mon Sep 17 00:00:00 2001 From: Khalil Estell Date: Fri, 3 Jan 2025 16:59:35 -0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Add=20rmd=5Fdrc=5Fv2=20and=20rmd=5F?= =?UTF-8?q?mc=5Fx=5Fv2=20drivers=20=20(#20)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * :sparkles: Add rmd_drc_v2 and rmd_mc_x_v2 drivers These drivers use the v2 can interfaces, primarily `hal::can_transceiver`. * :bug: Add can filter to drivers * :arrow_up: libhal/5.4.1 * Remove demos updates, move to new branch --- CMakeLists.txt | 3 + conanfile.py | 8 +- demos/applications/drc_v2.cpp | 157 +++++++ demos/applications/drc_v2_resources.cpp | 131 ++++++ demos/applications/mc_x_v2.cpp | 192 +++++++++ demos/applications/mc_x_v2_resources.cpp | 133 ++++++ .../smart_servo/rmd/drc_v2.hpp | 390 ++++++++++++++++++ .../smart_servo/rmd/mc_x_v2.hpp | 324 +++++++++++++++ src/smart_servo/rmd/drc.cpp | 24 +- src/smart_servo/rmd/drc_adaptors.cpp | 5 - src/smart_servo/rmd/drc_v2.cpp | 341 +++++++++++++++ src/smart_servo/rmd/mc_x_constants.hpp | 2 + src/smart_servo/rmd/mc_x_v2.cpp | 364 ++++++++++++++++ tests/smart_servo/rmd/drc.test.cpp | 6 +- tests/smart_servo/rmd/drc_v2.test.cpp | 358 ++++++++++++++++ 15 files changed, 2414 insertions(+), 24 deletions(-) create mode 100644 demos/applications/drc_v2.cpp create mode 100644 demos/applications/drc_v2_resources.cpp create mode 100644 demos/applications/mc_x_v2.cpp create mode 100644 demos/applications/mc_x_v2_resources.cpp create mode 100644 include/libhal-actuator/smart_servo/rmd/drc_v2.hpp create mode 100644 include/libhal-actuator/smart_servo/rmd/mc_x_v2.hpp create mode 100644 src/smart_servo/rmd/drc_v2.cpp create mode 100644 src/smart_servo/rmd/mc_x_v2.cpp create mode 100644 tests/smart_servo/rmd/drc_v2.test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2c73cca..6784540 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,13 +22,16 @@ libhal_test_and_make_library( src/rc_servo.cpp src/smart_servo/rmd/drc_adaptors.cpp src/smart_servo/rmd/drc.cpp + src/smart_servo/rmd/drc_v2.cpp src/smart_servo/rmd/mc_x_adaptors.cpp src/smart_servo/rmd/mc_x.cpp + src/smart_servo/rmd/mc_x_v2.cpp TEST_SOURCES tests/main.test.cpp tests/rc_servo.test.cpp tests/smart_servo/rmd/drc.test.cpp + # tests/smart_servo/rmd/drc_v2.test.cpp tests/smart_servo/rmd/mc_x.test.cpp tests/smart_servo/rmd/drc_adaptors.test.cpp diff --git a/conanfile.py b/conanfile.py index 09e7119..3b83a35 100644 --- a/conanfile.py +++ b/conanfile.py @@ -26,14 +26,12 @@ class libhal_actuator_conan(ConanFile): topics = ("actuator", "libhal", "driver") settings = "compiler", "build_type", "os", "arch" - python_requires = "libhal-bootstrap/[^3.0.0]" + python_requires = "libhal-bootstrap/[^4.1.1]" python_requires_extend = "libhal-bootstrap.library" def requirements(self): - # Adds libhal and libhal-util as transitive headers, meaning library - # consumers get the libhal and libhal-util headers downstream. - bootstrap = self.python_requires["libhal-bootstrap"] - bootstrap.module.add_library_requirements(self) + self.requires("libhal/[^4.9.0]", transitive_headers=True) + self.requires("libhal-util/[^5.4.1]", transitive_headers=True) self.requires("libhal-canrouter/[^3.0.0]") def package_info(self): diff --git a/demos/applications/drc_v2.cpp b/demos/applications/drc_v2.cpp new file mode 100644 index 0000000..4c9229c --- /dev/null +++ b/demos/applications/drc_v2.cpp @@ -0,0 +1,157 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +void application(resource_list& p_resources) +{ + using namespace std::chrono_literals; + using namespace hal::literals; + + auto& clock = *p_resources.clock.value(); + auto& console = *p_resources.console.value(); + auto& can_transceiver = *p_resources.can_transceiver.value(); + auto& can_bus_manager = *p_resources.can_bus_manager.value(); + auto& can_identifier_filter = *p_resources.can_identifier_filter.value(); + + // Needs to be set to this baud rate to work with the default firmware CAN + // baud rate. + can_bus_manager.baud_rate(1.0_MHz); + + hal::print(console, "RMD DRC Smart Servo Application Starting...\n\n"); + + constexpr std::uint16_t starting_device_address = 0x140; + std::uint16_t address_offset = 0; + + while (true) { + try { + auto const address = starting_device_address + address_offset; + hal::actuator::rmd_drc_v2 drc( + can_transceiver, can_identifier_filter, clock, 6.0f, address); + + auto print_feedback = [&drc, &console]() { + drc.feedback_request(hal::actuator::rmd_drc_v2::read::status_2); + drc.feedback_request( + hal::actuator::rmd_drc_v2::read::multi_turns_angle); + drc.feedback_request( + hal::actuator::rmd_drc_v2::read::status_1_and_error_flags); + + hal::print<2048>( + console, + "[%u] =================================\n" + "raw_multi_turn_angle = %f\n" + "raw_current = %d\n" + "raw_speed = %d\n" + "raw_volts = %d\n" + "encoder = %d\n" + "raw_motor_temperature = %d" + "\n" + "over_voltage_protection_tripped = %d\n" + "over_temperature_protection_tripped = %d\n" + "-------\n" + "angle() = %f °deg\n" + "current() = %f A\n" + "speed() = %f rpm\n" + "volts() = %f V\n" + "temperature() = %f °C\n\n", + + drc.feedback().message_number, + static_cast(drc.feedback().raw_multi_turn_angle), + drc.feedback().raw_current, + drc.feedback().raw_speed, + drc.feedback().raw_volts, + drc.feedback().encoder, + drc.feedback().raw_motor_temperature, + drc.feedback().over_voltage_protection_tripped(), + drc.feedback().over_temperature_protection_tripped(), + drc.feedback().angle(), + drc.feedback().current(), + drc.feedback().speed(), + drc.feedback().volts(), + drc.feedback().temperature()); + }; + + hal::delay(clock, 500ms); + + while (true) { + drc.velocity_control(10.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + drc.velocity_control(-10.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + drc.position_control(0.0_deg, 50.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + drc.position_control(-45.0_deg, 50.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + drc.position_control(90.0_deg, 50.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + drc.position_control(180.0_deg, 50.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + drc.position_control(-360.0_deg, 50.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + drc.position_control(0.0_deg, 50.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + } + } catch (hal::timed_out const&) { + hal::print( + console, + "hal::timed_out exception! which means that the device did not " + "respond. Moving to the next device address in the list.\n"); + } catch (hal::resource_unavailable_try_again const& p_error) { + hal::print(console, "hal::resource_unavailable_try_again\n"); + if (p_error.instance() == &can_transceiver) { + hal::print( + console, + "\n" + "The CAN peripheral has received no acknowledgements from any other " + "device on the bus. It appears as if the peripheral is not connected " + "to a can network. This can happen if the baud rate is incorrect, " + "the CAN transceiver is not functioning, or the devices on the bus " + "are not responding." + "\n" + "Calling terminate!" + "\n" + "Consider powering down the system and checking all of your " + "connections before restarting the application."); + std::terminate(); + } + // otherwise keep trying with other addresses + } catch (...) { + hal::print(console, "Unknown exception caught in (...) block\n"); + throw; // see if anyone else can handle the exception + } + + address_offset = (address_offset + 1) % 8; + hal::delay(clock, 1s); + } +} diff --git a/demos/applications/drc_v2_resources.cpp b/demos/applications/drc_v2_resources.cpp new file mode 100644 index 0000000..26cf442 --- /dev/null +++ b/demos/applications/drc_v2_resources.cpp @@ -0,0 +1,131 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +void application(resource_list& p_resources) +{ + using namespace std::chrono_literals; + using namespace hal::literals; + + auto& clock = *p_resources.clock.value(); + auto& console = *p_resources.console.value(); + auto& can_transceiver = *p_resources.can_transceiver.value(); + auto& can_bus_manager = *p_resources.can_bus_manager.value(); + auto& can_identifier_filter = *p_resources.can_identifier_filter.value(); + + // Needs to be set to this baud rate to work with the default firmware CAN + // baud rate. + can_bus_manager.baud_rate(1.0_MHz); + + hal::print(console, "RMD DRC Smart Servo Application Starting...\n\n"); + + constexpr std::uint16_t starting_device_address = 0x140; + std::uint16_t address_offset = 0; + + while (true) { + try { + auto const address = starting_device_address + address_offset; + hal::actuator::rmd_drc_v2 drc( + can_transceiver, can_identifier_filter, clock, 6.0f, address); + + auto motor = drc.acquire_motor(20.0_rpm); + auto servo = drc.acquire_servo(20.0_rpm); + auto temperature_sensor = drc.acquire_temperature_sensor(); + auto rotation_sensor = drc.acquire_rotation_sensor(); + + auto print_feedback = + [&console, &temperature_sensor, &rotation_sensor]() { + hal::print<2048>(console, + "[%u] =================================\n" + "shaft angle = %f deg\n" + "temperature = %f C\n" + "\n\n", + temperature_sensor.read(), + rotation_sensor.read()); + }; + + hal::delay(clock, 500ms); + + while (true) { + motor.power(0.5f); + hal::delay(clock, 5000ms); + print_feedback(); + + motor.power(-0.5f); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(0.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(-45.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(90.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(180.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(-360.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(0.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + } + } catch (hal::timed_out const&) { + hal::print( + console, + "hal::timed_out exception! which means that the device did not " + "respond. Moving to the next device address in the list.\n"); + } catch (hal::resource_unavailable_try_again const& p_error) { + hal::print(console, "hal::resource_unavailable_try_again\n"); + if (p_error.instance() == &can_transceiver) { + hal::print( + console, + "\n" + "The CAN peripheral has received no acknowledgements from any other " + "device on the bus. It appears as if the peripheral is not connected " + "to a can network. This can happen if the baud rate is incorrect, " + "the CAN transceiver is not functioning, or the devices on the bus " + "are not responding." + "\n" + "Calling terminate!" + "\n" + "Consider powering down the system and checking all of your " + "connections before restarting the application."); + std::terminate(); + } + // otherwise keep trying with other addresses + } catch (...) { + hal::print(console, "Unknown exception caught in (...) block\n"); + throw; // see if anyone else can handle the exception + } + + address_offset = (address_offset + 1) % 8; + hal::delay(clock, 1s); + } +} diff --git a/demos/applications/mc_x_v2.cpp b/demos/applications/mc_x_v2.cpp new file mode 100644 index 0000000..139b022 --- /dev/null +++ b/demos/applications/mc_x_v2.cpp @@ -0,0 +1,192 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include + +void application(resource_list& p_map) +{ + using namespace std::chrono_literals; + using namespace hal::literals; + + auto& clock = *p_map.clock.value(); + auto& console = *p_map.console.value(); + auto& can_transceiver = *p_map.can_transceiver.value(); + auto& can_bus_manager = *p_map.can_bus_manager.value(); + auto& can_identifier_filter = *p_map.can_identifier_filter.value(); + + // Needs to be set to this baud rate to work with the default firmware CAN + // baud rate. + can_bus_manager.baud_rate(1.0_MHz); + + hal::print(console, "RMD MC-X Smart Servo Application Starting...\n\n"); + + constexpr std::uint16_t starting_device_address = 0x140; + std::uint16_t address_offset = 0; + + while (true) { + try { + auto const address = starting_device_address + address_offset; + hal::print<32>(console, "Using address: 0x%04X\n", address); + hal::actuator::rmd_mc_x_v2 mc_x( + can_transceiver, can_identifier_filter, clock, 36.0f, address); + + auto print_feedback = [&mc_x, &console]() { + mc_x.feedback_request(hal::actuator::rmd_mc_x_v2::read::status_2); + mc_x.feedback_request( + hal::actuator::rmd_mc_x_v2::read::multi_turns_angle); + mc_x.feedback_request( + hal::actuator::rmd_mc_x_v2::read::status_1_and_error_flags); + + hal::print<2048>( + console, + "[%u] =================================\n" + "raw_multi_turn_angle = %f\n" + "raw_current = %d\n" + "raw_speed = %d\n" + "raw_volts = %d\n" + "encoder = %d\n" + "raw_motor_temperature = %d" + "\n" + "-------\n" + "angle() = %f °deg\n" + "current() = %f A\n" + "speed() = %f rpm\n" + "volts() = %f V\n" + "temperature() = %f °C\n" + "motor_stall() = %d\n" + "low_pressure() = %d\n" + "over_voltage() = %d\n" + "over_current() = %d\n" + "power_overrun() = %d\n" + "speeding() = %d\n" + "over_temperature() = %d\n" + "encoder_calibration_error() = %d\n" + "\n", + + mc_x.feedback().message_number, + static_cast(mc_x.feedback().raw_multi_turn_angle), + mc_x.feedback().raw_current, + mc_x.feedback().raw_speed, + mc_x.feedback().raw_volts, + mc_x.feedback().encoder, + mc_x.feedback().raw_motor_temperature, + mc_x.feedback().angle(), + mc_x.feedback().current(), + mc_x.feedback().speed(), + mc_x.feedback().volts(), + mc_x.feedback().temperature(), + mc_x.feedback().motor_stall(), + mc_x.feedback().low_pressure(), + mc_x.feedback().over_voltage(), + mc_x.feedback().over_current(), + mc_x.feedback().power_overrun(), + mc_x.feedback().speeding(), + mc_x.feedback().over_temperature(), + mc_x.feedback().encoder_calibration_error()); + }; + + while (true) { + mc_x.velocity_control(50.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + mc_x.velocity_control(0.0_rpm); + hal::delay(clock, 2000ms); + print_feedback(); + + mc_x.velocity_control(-50.0_rpm); + hal::delay(clock, 5000ms); + print_feedback(); + + mc_x.velocity_control(0.0_rpm); + hal::delay(clock, 2000ms); + print_feedback(); + + // Position control above 40 RPM seems to cause issues with position + // control + mc_x.position_control(0.0_deg, 30.0_rpm); + hal::delay(clock, 1s); + print_feedback(); + + mc_x.position_control(90.0_deg, 30.0_rpm); + hal::delay(clock, 2s); + print_feedback(); + + mc_x.position_control(180.0_deg, 30.0_rpm); + hal::delay(clock, 2s); + print_feedback(); + + mc_x.position_control(90.0_deg, 30.0_rpm); + hal::delay(clock, 2s); + print_feedback(); + + mc_x.position_control(0.0_deg, 30.0_rpm); + hal::delay(clock, 2s); + print_feedback(); + + mc_x.position_control(-45.0_deg, 30.0_rpm); + hal::delay(clock, 2s); + print_feedback(); + + mc_x.position_control(-90.0_deg, 30.0_rpm); + hal::delay(clock, 2s); + print_feedback(); + + mc_x.position_control(-45.0_deg, 30.0_rpm); + hal::delay(clock, 2s); + print_feedback(); + + mc_x.position_control(0.0_deg, 30.0_rpm); + hal::delay(clock, 2s); + print_feedback(); + } + } catch (hal::timed_out const&) { + hal::print( + console, + "hal::timed_out exception! which means that the device did not " + "respond. Moving to the next device address in the list.\n"); + } catch (hal::resource_unavailable_try_again const& p_error) { + hal::print(console, "hal::resource_unavailable_try_again\n"); + if (p_error.instance() == &can_transceiver) { + hal::print( + console, + "\n" + "The CAN peripheral has received no acknowledgements from any other " + "device on the bus. It appears as if the peripheral is not connected " + "to a can network. This can happen if the baud rate is incorrect, " + "the CAN transceiver is not functioning, or the devices on the bus " + "are not responding." + "\n" + "Calling terminate!" + "\n" + "Consider powering down the system and checking all of your " + "connections before restarting the application."); + std::terminate(); + } + // otherwise keep trying with other addresses + } catch (...) { + hal::print(console, "Unknown exception caught in (...) block\n"); + throw; + } + + address_offset = (address_offset + 1) % 32; + hal::delay(clock, 1s); + } +} diff --git a/demos/applications/mc_x_v2_resources.cpp b/demos/applications/mc_x_v2_resources.cpp new file mode 100644 index 0000000..fe9fda9 --- /dev/null +++ b/demos/applications/mc_x_v2_resources.cpp @@ -0,0 +1,133 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include + +void application(resource_list& p_map) +{ + using namespace std::chrono_literals; + using namespace hal::literals; + + auto& clock = *p_map.clock.value(); + auto& console = *p_map.console.value(); + auto& can_transceiver = *p_map.can_transceiver.value(); + auto& can_bus_manager = *p_map.can_bus_manager.value(); + auto& can_identifier_filter = *p_map.can_identifier_filter.value(); + + // Needs to be set to this baud rate to work with the default firmware CAN + // baud rate. + can_bus_manager.baud_rate(1.0_MHz); + + hal::print(console, "RMD MC-X Smart Servo Application Starting...\n\n"); + + constexpr std::uint16_t starting_device_address = 0x140; + std::uint16_t address_offset = 0; + + while (true) { + try { + auto const address = starting_device_address + address_offset; + hal::print<32>(console, "Using address: 0x%04X\n", address); + hal::actuator::rmd_mc_x_v2 mc_x( + can_transceiver, can_identifier_filter, clock, 36.0f, address); + + auto motor = mc_x.acquire_motor(20.0_rpm); + auto servo = mc_x.acquire_servo(20.0_rpm); + auto temperature_sensor = mc_x.acquire_temperature_sensor(); + auto rotation_sensor = mc_x.acquire_rotation_sensor(); + + auto print_feedback = + [&console, &temperature_sensor, &rotation_sensor]() { + hal::print<2048>(console, + "[%u] =================================\n" + "shaft angle = %f deg\n" + "temperature = %f C\n" + "\n\n", + temperature_sensor.read(), + rotation_sensor.read()); + }; + + hal::delay(clock, 500ms); + + while (true) { + motor.power(0.5f); + hal::delay(clock, 5000ms); + print_feedback(); + + motor.power(-0.5f); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(0.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(-45.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(90.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(180.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(-360.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + + servo.position(0.0_deg); + hal::delay(clock, 5000ms); + print_feedback(); + } + } catch (hal::timed_out const&) { + hal::print( + console, + "hal::timed_out exception! which means that the device did not " + "respond. Moving to the next device address in the list.\n"); + } catch (hal::resource_unavailable_try_again const& p_error) { + hal::print(console, "hal::resource_unavailable_try_again\n"); + if (p_error.instance() == &can_transceiver) { + hal::print( + console, + "\n" + "The CAN peripheral has received no acknowledgements from any other " + "device on the bus. It appears as if the peripheral is not connected " + "to a can network. This can happen if the baud rate is incorrect, " + "the CAN transceiver is not functioning, or the devices on the bus " + "are not responding." + "\n" + "Calling terminate!" + "\n" + "Consider powering down the system and checking all of your " + "connections before restarting the application."); + std::terminate(); + } + // otherwise keep trying with other addresses + } catch (...) { + hal::print(console, "Unknown exception caught in (...) block\n"); + throw; // see if anyone else can handle the exception + } + + address_offset = (address_offset + 1) % 8; + hal::delay(clock, 1s); + } +} diff --git a/include/libhal-actuator/smart_servo/rmd/drc_v2.hpp b/include/libhal-actuator/smart_servo/rmd/drc_v2.hpp new file mode 100644 index 0000000..7412bb3 --- /dev/null +++ b/include/libhal-actuator/smart_servo/rmd/drc_v2.hpp @@ -0,0 +1,390 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace hal::actuator { +/** + * @brief Driver for RMD motors equip with the DRC motor drivers + * + */ +class rmd_drc_v2 +{ +public: + /// Commands that can be issued to a RMD-X motor + enum class read : hal::byte + { + /** + * @brief Status1 + error flag information read request command + * + * Sending this request will update the following fields in the feedback_t: + * + * - raw_multi_turn_angle + */ + multi_turns_angle = 0x92, + /** + * @brief Status1 + error flag information read request command + * + * Sending this request will update the following fields in the feedback_t: + * + * - raw_motor_temperature + * - over_voltage_protection_tripped + * - over_temperature_protection_tripped + */ + status_1_and_error_flags = 0x9A, + /** + * @brief Status2 read request command + * + * Sending this request will update the following fields in the feedback_t: + * + * - raw_motor_temperature + * - raw_current + * - raw_speed + * - encoder + */ + status_2 = 0x9C, + }; + + /// Commands for actuate the motor + enum class actuate : hal::byte + { + speed = 0xA2, + position_2 = 0xA4, + }; + + /// Commands for updating motor configuration data + enum class write : hal::byte + { + pid_to_ram = 0x31, + pid_to_rom = 0x32, + acceleration_data_to_ram = 0x34, + encoder_offset = 0x91, + current_position_to_rom_as_motor_zero = 0x19, + }; + + /// Commands for controlling the motor as a whole + enum class system : hal::byte + { + clear_error_flag = 0x9B, + off = 0x80, + stop = 0x81, + running = 0x88, + }; + + /// Structure containing all of the forms of feedback acquired by an RMD-X + /// motor + struct feedback_t + { + /// Every time a message from our motor is received this number increments. + /// This can be used to indicate if the feedback has updated since the last + /// time it was read. + std::uint32_t message_number = 0; + /// Raw multi-turn angle (0.01°/LSB) + std::int64_t raw_multi_turn_angle{ 0 }; + /// Current flowing through the motor windings + /// (-2048 <-> 2048 ==> -33A <-> 33A) + std::int16_t raw_current{ 0 }; + /// Rotational velocity of the motor (1 degrees per second (dps)/LSB) + std::int16_t raw_speed{ 0 }; + /// Motor's supply voltage (0.1V/LSB) + std::int16_t raw_volts{ 0 }; + /// Signed 16-bit raw encoder count value of the motor + std::int16_t encoder{ 0 }; + /// Core temperature of the motor (1C/LSB) + std::int8_t raw_motor_temperature{ 0 }; + /// 8-bit value containing error flag information + std::uint8_t raw_error_state{ 0 }; + + hal::ampere current() const noexcept; + hal::rpm speed() const noexcept; + hal::volts volts() const noexcept; + hal::celsius temperature() const noexcept; + hal::degrees angle() const noexcept; + + /** + * @brief Return if the motor has detected an over voltage event + * + * In order for this field to be updated a feedback_request with + * status_1_and_error_flags must be issued. + * + * @return true - over voltage protection tripped + * @return false - over voltage protection has not tripped + */ + bool over_voltage_protection_tripped() const noexcept; + + /** + * @brief Return if the motor has detected an over temperature event + * + * In order for this field to be updated a feedback_request with + * status_1_and_error_flags must be issued. + * + * @return true - over temperature protection tripped + * @return false - over temperature protection has not tripped + */ + bool over_temperature_protection_tripped() const noexcept; + }; + + /** + * @brief Rotation sensor adaptor for DRC motors + * + */ + class rotation_sensor : public hal::rotation_sensor + { + public: + rotation_sensor(rotation_sensor const&) = delete; + rotation_sensor& operator=(rotation_sensor const&) = delete; + rotation_sensor(rotation_sensor&&) = default; + rotation_sensor& operator=(rotation_sensor&&) = default; + ~rotation_sensor() = default; + + private: + friend class rmd_drc_v2; + rotation_sensor(rmd_drc_v2& p_drc); + hal::rotation_sensor::read_t driver_read() override; + rmd_drc_v2* m_drc = nullptr; + }; + + /** + * @brief Temperature sensor adaptor for DRC motors + * + */ + class temperature_sensor : public hal::temperature_sensor + { + public: + temperature_sensor(temperature_sensor const&) = delete; + temperature_sensor& operator=(temperature_sensor const&) = delete; + temperature_sensor(temperature_sensor&&) = default; + temperature_sensor& operator=(temperature_sensor&&) = default; + ~temperature_sensor() = default; + + private: + friend class rmd_drc_v2; + temperature_sensor(rmd_drc_v2& p_drc); + celsius driver_read() override; + rmd_drc_v2* m_drc = nullptr; + }; + + /** + * @brief Motor interface adaptor for DRC + * + */ + class motor : public hal::motor + { + public: + motor(motor const&) = delete; + motor& operator=(motor const&) = delete; + motor(motor&&) = default; + motor& operator=(motor&&) = default; + ~motor() = default; + + private: + friend class rmd_drc_v2; + motor(rmd_drc_v2& p_drc, hal::rpm p_max_speed); + void driver_power(float p_power) override; + + rmd_drc_v2* m_drc = nullptr; + hal::rpm m_max_speed; + }; + /** + * @brief Servo interface adaptor for DRC + * + */ + class servo : public hal::servo + { + public: + servo(servo const&) = delete; + servo& operator=(servo const&) = delete; + servo(servo&&) = default; + servo& operator=(servo&&) = default; + ~servo() = default; + + private: + friend class rmd_drc_v2; + servo(rmd_drc_v2& p_drc, hal::rpm p_max_speed); + void driver_position(hal::degrees p_position) override; + rmd_drc_v2* m_drc = nullptr; + hal::rpm m_max_speed; + }; + + /** + * @brief angular velocity sensor adaptor for DRC + * + */ + class angular_velocity_sensor : public hal::angular_velocity_sensor + { + public: + angular_velocity_sensor(angular_velocity_sensor const&) = delete; + angular_velocity_sensor& operator=(angular_velocity_sensor const&) = delete; + angular_velocity_sensor(angular_velocity_sensor&&) = default; + angular_velocity_sensor& operator=(angular_velocity_sensor&&) = default; + ~angular_velocity_sensor() = default; + + private: + friend class rmd_drc_v2; + angular_velocity_sensor(rmd_drc_v2& p_drc); + hal::rpm driver_read() override; + rmd_drc_v2* m_drc = nullptr; + }; + + /** + * @brief Create a new device driver drc + * + * This factory function will power cycle the motor + * + * @param p_transceiver - The can transceiver connected to the bus with the + * servo present. + * @param p_filter - Identifier filter to allow responses from the servo to be + * received by the device. + * @param p_device_id - The CAN ID of the motor + * @param p_clock - clocked used to determine timeouts + * @param p_gear_ratio - gear ratio of the motor + * @param p_max_response_time - maximum amount of time to wait for a response + * from the motor. + * @throws hal::timed_out - if the p_max_response_time is exceeded + */ + rmd_drc_v2( + hal::can_transceiver& p_transceiver, + hal::can_identifier_filter& p_filter, + hal::steady_clock& p_clock, + float p_gear_ratio, + hal::u32 p_device_id, + hal::time_duration p_max_response_time = std::chrono::milliseconds(10)); + + rmd_drc_v2(rmd_drc_v2&) = delete; + rmd_drc_v2& operator=(rmd_drc_v2&) = delete; + rmd_drc_v2(rmd_drc_v2&&) noexcept = delete; + rmd_drc_v2& operator=(rmd_drc_v2&&) noexcept = delete; + ~rmd_drc_v2() = default; + + /** + * @brief Create a hal::rotation_sensor driver using the drc driver + * + * @return drc_rotation_sensor - motor implementation based on the drc + * driver. This object's lifetime must exceed the lifetime of the returned + * object. + */ + rotation_sensor acquire_rotation_sensor(); + + /** + * @brief Create a hal::temperature_sensor driver using the drc driver + * + * @return drc_temperature_sensor - temperature sensor implementation based on + * the drc driver. This object's lifetime must exceed the lifetime of the + * returned object. + */ + temperature_sensor acquire_temperature_sensor(); + + /** + * @brief Create a hal::motor implementation from the drc driver + * + * @param p_max_speed - maximum speed of the motor represented by +1.0 and + * -1.0 + * @return drc_motor - motor implementation based on the drc driver. This + * object's lifetime must exceed the lifetime of the returned object. + */ + motor acquire_motor(hal::rpm p_max_speed); + + /** + * @brief Create a hal::servo driver using the drc driver + * + * @param p_max_speed - maximum speed of the servo when moving to an angle + * @return drc_servo - servo implementation based on the drc driver. This + * object's lifetime must exceed the lifetime of the returned object. + */ + servo acquire_servo(hal::rpm p_max_speed); + + /** + * @brief Create a hal::angular_velocity_sensor driver using the drc driver + * + * @return angular_velocity_sensor - angular_velocity_sensor implementation + * based on the drc driver. This object's lifetime must exceed the lifetime of + * the returned object. + */ + angular_velocity_sensor acquire_angular_velocity_sensor(); + + /** + * @brief Request feedback from the motor + * + * @param p_command - the request to command the motor to respond with + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + void feedback_request(read p_command); + + /** + * @brief Rotate motor shaft at the designated speed + * + * @param p_speed - speed in rpm to move the motor shaft at. Positive values + * rotate the motor shaft clockwise, negative values rotate the motor shaft + * counter-clockwise assuming you are looking directly at the motor shaft. + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + void velocity_control(rpm p_speed); + + /** + * @brief Move motor shaft to a specific angle + * + * @param p_angle - angle position in degrees to move to + * @param p_speed - maximum speed in rpm's + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + void position_control(degrees p_angle, rpm p_speed); + + /** + * @brief Send system control commands to the device + * + * @param p_system_command - system control command to send to the device + * status. + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + void system_control(system p_system_command); + + /** + * @brief Returns a reference to the internal feedback + * + * @return feedback_t const& + */ + feedback_t const& feedback() const; + +private: + void handle_message(can_message const& p_message); + /** + * @brief Send command on can bus to the motor using its device ID + * + * @param p_payload - command data to be sent to the device + */ + void send(std::array p_payload); + + feedback_t m_feedback{}; + hal::can_message_finder m_can; + hal::steady_clock* m_clock; + float m_gear_ratio; + hal::time_duration m_max_response_time; +}; +} // namespace hal::actuator diff --git a/include/libhal-actuator/smart_servo/rmd/mc_x_v2.hpp b/include/libhal-actuator/smart_servo/rmd/mc_x_v2.hpp new file mode 100644 index 0000000..4806cf6 --- /dev/null +++ b/include/libhal-actuator/smart_servo/rmd/mc_x_v2.hpp @@ -0,0 +1,324 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace hal::actuator { +/** + * @brief Driver for RMD series motors equip with the MC-X motor driver + * + */ +class rmd_mc_x_v2 +{ +public: + /// Commands that can be issued to a RMD-X motor + enum class read : hal::byte + { + multi_turns_angle = 0x92, + status_1_and_error_flags = 0x9A, + status_2 = 0x9C, + }; + + /// Commands for actuate the motor + enum class actuate : hal::byte + { + torque = 0xA1, + speed = 0xA2, + position = 0xA5, + }; + + /// Commands for updating motor configuration data + enum class write : hal::byte + { + // None supported currently + }; + + /// Commands for controlling the motor as a whole + enum class system : hal::byte + { + off = 0x80, + stop = 0x81, + }; + + /// Structure containing all of the forms of feedback acquired by an RMD-X + /// motor + struct feedback_t + { + /// Every time a message from our motor is received this number increments. + /// This can be used to indicate if the feedback has updated since the last + /// time it was read. + std::uint32_t message_number = 0; + /// Represents the multi-turn absolute angle of the encoder relative to its + /// zero starting point (0.01°/LSB) + std::int64_t raw_multi_turn_angle{ 0 }; + /// 16-bit value containing error flag information + std::uint16_t raw_error_state{ 0 }; + /// Current flowing through the motor windings + /// (-2048 <-> 2048 ==> -33A <-> 33A) + std::int16_t raw_current{ 0 }; + /// Rotational velocity of the motor (1 degrees per second (dps)/LSB) + std::int16_t raw_speed{ 0 }; + /// Motor's supply voltage (0.1V/LSB) + std::int16_t raw_volts{ 0 }; + /// Signed 16-bit raw encoder count value of the motor + std::int16_t encoder{ 0 }; + /// Core temperature of the motor (1C/LSB) + std::int8_t raw_motor_temperature{ 0 }; + + hal::ampere current() const noexcept; + hal::rpm speed() const noexcept; + hal::volts volts() const noexcept; + hal::celsius temperature() const noexcept; + hal::degrees angle() const noexcept; + bool motor_stall() const noexcept; + bool low_pressure() const noexcept; + bool over_voltage() const noexcept; + bool over_current() const noexcept; + bool power_overrun() const noexcept; + bool speeding() const noexcept; + bool over_temperature() const noexcept; + bool encoder_calibration_error() const noexcept; + }; + + /** + * @brief Control a mc_x motor driver like a hal::motor + * + */ + class motor : public hal::motor + { + private: + motor(rmd_mc_x_v2& p_mc_x, hal::rpm p_max_speed); + void driver_power(float p_power) override; + friend class rmd_mc_x_v2; + rmd_mc_x_v2* m_mc_x = nullptr; + hal::rpm m_max_speed; + }; + + /** + * @brief Reports the rotation of the DRC motor + * + */ + class rotation : public hal::rotation_sensor + { + private: + rotation(rmd_mc_x_v2& p_mc_x); + hal::rotation_sensor::read_t driver_read() override; + friend class rmd_mc_x_v2; + rmd_mc_x_v2* m_mc_x = nullptr; + }; + + /** + * @brief Control a mc_x motor driver like a hal::servo + * + */ + class servo : public hal::servo + { + private: + servo(rmd_mc_x_v2& p_mc_x, hal::rpm p_max_speed); + void driver_position(hal::degrees p_position) override; + friend class rmd_mc_x_v2; + rmd_mc_x_v2* m_mc_x = nullptr; + hal::rpm m_max_speed; + }; + + /** + * @brief Reports the temperature of the DRC motor + * + */ + class temperature : public hal::temperature_sensor + { + private: + temperature(rmd_mc_x_v2& p_mc_x); + hal::celsius driver_read() override; + friend class rmd_mc_x_v2; + rmd_mc_x_v2* m_mc_x = nullptr; + }; + + /** + * @brief current sensor adaptor for mc_x + * + */ + class current_sensor : public hal::current_sensor + { + private: + current_sensor(rmd_mc_x_v2& p_mc_x); + hal::ampere driver_read() override; + friend class rmd_mc_x_v2; + rmd_mc_x_v2* m_mc_x = nullptr; + }; + + /** + * @brief Create a new rmd_mc_x_v2 device driver + * + * @param p_router - can router to use + * @param p_filter - identifier filter to allow messages from the MC_X device + * to be received. + * @param p_clock - clocked used to determine timeouts + * @param p_gear_ratio - gear ratio of the motor + * @param p_device_id - The message ID of the motor. Valid inputs are 0x140 to + * 0x160. Creating two rmd_mc_x_v2 with the same ID on the same + * can_transceiver is undefined behavior. + * @param p_max_response_time - maximum amount of time to wait for a response + * from the motor. + * @throws hal::timed_out - if the p_max_response_time is exceeded + * @throws hal::argument_out_of_domain - in two situations. If p_device_id is + * outside of its boundary and if can transceiver's baud rate is not 1_MHz + * which is the operating frequency of the RMD MC X devices. + */ + rmd_mc_x_v2( + hal::can_transceiver& p_router, + can_identifier_filter& p_filter, + hal::steady_clock& p_clock, + float p_gear_ratio, + hal::u32 p_device_id, + hal::time_duration p_max_response_time = std::chrono::milliseconds(10)); + + rmd_mc_x_v2(rmd_mc_x_v2&) = delete; + rmd_mc_x_v2& operator=(rmd_mc_x_v2&) = delete; + rmd_mc_x_v2(rmd_mc_x_v2&&) noexcept = delete; + rmd_mc_x_v2& operator=(rmd_mc_x_v2&&) noexcept = delete; + + /** + * @brief Create a hal::motor driver using the MC-X driver + * + * @param p_max_speed - maximum speed of the motor represented by +1.0 and + * -1.0 + * @return motor - motor implementation using the MC-X driver + */ + motor acquire_motor(hal::rpm p_max_speed); + + /** + * @brief Create a hal::rotation_sensor driver using the MC-X driver + * + * @return rotation - rotation sensor implementation based on the + * MC-X driver + */ + rotation acquire_rotation_sensor(); + + /** + * @brief Create a hal::rotation_sensor driver using the MC-X driver + * + * @param p_max_speed - maximum speed of the motor when moving to an angle + * @return rotation - rotation sensor implementation based on the + * MC-X driver + */ + servo acquire_servo(hal::rpm p_max_speed); + + /** + * @brief Create a hal::temperature_sensor driver using the MC-X driver + * + * @return temperature - temperature sensor implementation based on + * the MC-X driver + */ + temperature acquire_temperature_sensor(); + + /** + * @brief Create a hal::current_sensor driver using the mc_x driver + * + * @return current_sensor - current_sensor implementation based on the + * mc_x driver + */ + current_sensor acquire_current_sensor(); + + /** + * @brief Get feedback about the motor + * + * This object contains cached data from each response returned from the + * motor. It is updated when any of the control or feedback APIs are called. + * This object will not update without one of those APIs being called. + * + * @return const feedback_t& - information about the motor + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + feedback_t const& feedback() const; + + /** + * @brief Request feedback from the motor + * + * @param p_command - the request to command the motor to respond with + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + void feedback_request(read p_command); + + /** + * @brief Rotate motor shaft at the designated speed + * + * @param p_speed - speed in rpm to move the motor shaft at. Positive values + * rotate the motor shaft clockwise, negative values rotate the motor shaft + * counter-clockwise assuming you are looking directly at the motor shaft. + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + void velocity_control(rpm p_speed); + + /** + * @brief Move motor shaft to a specific angle + * + * @param p_angle - angle position in degrees to move to + * @param speed - speed in rpm's + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + void position_control(degrees p_angle, rpm speed); + + /** + * @brief Send system control commands to the device + * + * @param p_system_command - system control command to send to the device + * status + * @throws hal::timed_out - if a response is not returned within the max + * response time set at creation. + */ + void system_control(system p_system_command); + + /** + * @brief Handle messages from the can bus with this devices ID + * + * Meant mostly for testing purposes. + * + * @param p_message - message received from the bus + */ + void handle_message(can_message const& p_message); + +private: + /** + * @brief Send command on can bus to the motor using its device ID + * + * @param p_payload - command data to be sent to the device + */ + void send(std::array p_payload); + + feedback_t m_feedback{}; + hal::steady_clock* m_clock; + hal::can_message_finder m_can; + float m_gear_ratio; + hal::u32 m_device_id; + hal::time_duration m_max_response_time; +}; +} // namespace hal::actuator diff --git a/src/smart_servo/rmd/drc.cpp b/src/smart_servo/rmd/drc.cpp index a0d3d75..0a5257f 100644 --- a/src/smart_servo/rmd/drc.cpp +++ b/src/smart_servo/rmd/drc.cpp @@ -28,6 +28,19 @@ #include "drc_constants.hpp" namespace hal::actuator { +namespace { +std::int32_t rpm_to_drc_speed(rpm p_rpm, + float p_gear_ratio, + float p_dps_per_lsb) +{ + static constexpr float dps_per_rpm = (1.0f / 1.0_deg_per_sec); + + float const dps_float = (p_rpm * p_gear_ratio * dps_per_rpm) / p_dps_per_lsb; + + return bounds_check(dps_float); +} +} // namespace + hal::ampere rmd_drc::feedback_t::current() const noexcept { static constexpr float raw_current_range = 2048.0f; @@ -94,17 +107,6 @@ rmd_drc::rmd_drc(hal::can_router& p_router, rmd_drc::system_control(system::running); } -std::int32_t rpm_to_drc_speed(rpm p_rpm, - float p_gear_ratio, - float p_dps_per_lsb) -{ - static constexpr float dps_per_rpm = (1.0f / 1.0_deg_per_sec); - - float const dps_float = (p_rpm * p_gear_ratio * dps_per_rpm) / p_dps_per_lsb; - - return bounds_check(dps_float); -} - void rmd_drc::send(std::array p_payload) { // Capture the message number prior to the send command diff --git a/src/smart_servo/rmd/drc_adaptors.cpp b/src/smart_servo/rmd/drc_adaptors.cpp index 0132998..6f38eee 100644 --- a/src/smart_servo/rmd/drc_adaptors.cpp +++ b/src/smart_servo/rmd/drc_adaptors.cpp @@ -81,11 +81,6 @@ rmd_drc_motor make_motor(rmd_drc& p_drc, hal::rpm p_max_speed) return { p_drc, std::abs(p_max_speed) }; } -int make_servo(hal::rpm p_max_speed) -{ - return static_cast(5 * p_max_speed); -} - rmd_drc_angular_velocity_sensor::rmd_drc_angular_velocity_sensor(rmd_drc& p_drc) : m_drc(&p_drc) { diff --git a/src/smart_servo/rmd/drc_v2.cpp b/src/smart_servo/rmd/drc_v2.cpp new file mode 100644 index 0000000..d4e2d9f --- /dev/null +++ b/src/smart_servo/rmd/drc_v2.cpp @@ -0,0 +1,341 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common.hpp" +#include "drc_constants.hpp" + +namespace hal::actuator { +namespace { +std::int32_t rpm_to_drc_speed(rpm p_rpm, + float p_gear_ratio, + float p_dps_per_lsb) +{ + static constexpr float dps_per_rpm = (1.0f / 1.0_deg_per_sec); + + float const dps_float = (p_rpm * p_gear_ratio * dps_per_rpm) / p_dps_per_lsb; + + return bounds_check(dps_float); +} +} // namespace + +hal::ampere rmd_drc_v2::feedback_t::current() const noexcept +{ + static constexpr float raw_current_range = 2048.0f; + static constexpr auto current_range = 33.0_A; + return hal::map(static_cast(raw_current), + std::make_pair(-raw_current_range, raw_current_range), + std::make_pair(-current_range, current_range)); +} + +hal::rpm rmd_drc_v2::feedback_t::speed() const noexcept +{ + static constexpr auto velocity_per_lsb = 1.0_deg_per_sec; + return static_cast(raw_speed) * velocity_per_lsb; +} + +hal::volts rmd_drc_v2::feedback_t::volts() const noexcept +{ + static constexpr float volts_per_lsb = 0.1f; + return static_cast(raw_volts) * volts_per_lsb; +} + +hal::celsius rmd_drc_v2::feedback_t::temperature() const noexcept +{ + static constexpr float celsius_per_lsb = 1.0f; + return static_cast(raw_motor_temperature) * celsius_per_lsb; +} + +hal::degrees rmd_drc_v2::feedback_t::angle() const noexcept +{ + return static_cast(raw_multi_turn_angle) * dps_per_lsb_speed; +} + +bool rmd_drc_v2::feedback_t::over_voltage_protection_tripped() const noexcept +{ + return raw_error_state & over_temperature_protection_tripped_mask; +} + +bool rmd_drc_v2::feedback_t::over_temperature_protection_tripped() + const noexcept +{ + return raw_error_state & over_temperature_protection_tripped_mask; +} + +rmd_drc_v2::feedback_t const& rmd_drc_v2::feedback() const +{ + return m_feedback; +} + +rmd_drc_v2::rmd_drc_v2(hal::can_transceiver& p_can, + hal::can_identifier_filter& p_filter, + hal::steady_clock& p_clock, + float p_gear_ratio, // NOLINT + hal::u32 p_device_id, + hal::time_duration p_max_response_time) + : m_feedback{} + , m_can(p_can, p_device_id) + , m_clock(&p_clock) + , m_gear_ratio(p_gear_ratio) + , m_max_response_time(p_max_response_time) +{ + p_filter.allow(p_device_id); + rmd_drc_v2::system_control(system::off); + rmd_drc_v2::system_control(system::running); +} + +void rmd_drc_v2::send(std::array p_payload) +{ + hal::can_message const payload{ + .id = m_can.id(), + .length = 8, + .payload = p_payload, + }; + + // Send payload + m_can.transceiver().send(payload); + + // Wait for the message number to increment + auto const deadline = hal::future_deadline(*m_clock, m_max_response_time); + while (deadline > m_clock->uptime()) { + if (auto const message = m_can.find(); message.has_value()) { + handle_message(*message); + return; + } + } + + hal::safe_throw(hal::timed_out(this)); +} + +void rmd_drc_v2::velocity_control(rpm p_rpm) +{ + auto const speed_data = + rpm_to_drc_speed(p_rpm, m_gear_ratio, dps_per_lsb_speed); + + send({ + hal::value(actuate::speed), + 0x00, + 0x00, + 0x00, + static_cast((speed_data >> 0) & 0xFF), + static_cast((speed_data >> 8) & 0xFF), + static_cast((speed_data >> 16) & 0xFF), + static_cast((speed_data >> 24) & 0xFF), + }); +} + +void rmd_drc_v2::position_control(degrees p_angle, rpm p_rpm) // NOLINT +{ + static constexpr float deg_per_lsb = 0.01f; + auto const angle = (p_angle * m_gear_ratio) / deg_per_lsb; + auto const angle_data = bounds_check(angle); + auto const speed_data = + rpm_to_drc_speed(p_rpm, m_gear_ratio, dps_per_lsb_angle); + + send({ + hal::value(actuate::position_2), + 0x00, + static_cast((speed_data >> 0) & 0xFF), + static_cast((speed_data >> 8) & 0xFF), + static_cast((angle_data >> 0) & 0xFF), + static_cast((angle_data >> 8) & 0xFF), + static_cast((angle_data >> 16) & 0xFF), + static_cast((angle_data >> 24) & 0xFF), + }); +} + +void rmd_drc_v2::feedback_request(read p_command) +{ + send({ + hal::value(p_command), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + }); +} + +void rmd_drc_v2::system_control(system p_system_command) +{ + send({ + hal::value(p_system_command), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + }); +} + +void rmd_drc_v2::handle_message(can_message const& p_message) +{ + m_feedback.message_number++; + + switch (p_message.payload[0]) { + case hal::value(read::status_2): + case hal::value(actuate::speed): + case hal::value(actuate::position_2): { + auto& data = p_message.payload; + m_feedback.raw_motor_temperature = static_cast(data[1]); + m_feedback.raw_current = + static_cast((data[3] << 8) | data[2] << 0); + m_feedback.raw_speed = + static_cast((data[5] << 8) | data[4] << 0); + m_feedback.encoder = + static_cast((data[7] << 8) | data[6] << 0); + break; + } + case hal::value(read::status_1_and_error_flags): { + auto& data = p_message.payload; + m_feedback.raw_motor_temperature = static_cast(data[1]); + m_feedback.raw_volts = + static_cast((data[4] << 8) | data[3]); + m_feedback.raw_error_state = data[7]; + break; + } + case hal::value(read::multi_turns_angle): { + auto& data = p_message.payload; + + m_feedback.raw_multi_turn_angle = hal::bit_value(0U) + .insert>(data[1]) + .insert>(data[2]) + .insert>(data[3]) + .insert>(data[4]) + .insert>(data[5]) + .insert>(data[6]) + .insert>(data[7]) + .to(); + break; + } + default: + break; + } +} + +// ============================================================================= +// +// Interface Implementation constructors +// +// ============================================================================= + +rmd_drc_v2::servo::servo(rmd_drc_v2& p_drc, hal::rpm p_max_speed) + : m_drc(&p_drc) + , m_max_speed(p_max_speed) +{ +} + +rmd_drc_v2::temperature_sensor::temperature_sensor(rmd_drc_v2& p_drc) + : m_drc(&p_drc) +{ +} + +rmd_drc_v2::rotation_sensor::rotation_sensor(rmd_drc_v2& p_drc) + : m_drc(&p_drc) +{ +} + +rmd_drc_v2::motor::motor(rmd_drc_v2& p_drc, hal::rpm p_max_speed) + : m_drc(&p_drc) + , m_max_speed(p_max_speed) +{ +} + +rmd_drc_v2::angular_velocity_sensor::angular_velocity_sensor(rmd_drc_v2& p_drc) + : m_drc(&p_drc) +{ +} + +// ============================================================================= +// +// Object acquisition functions +// +// ============================================================================= + +rmd_drc_v2::motor rmd_drc_v2::acquire_motor(hal::rpm p_max_speed) +{ + return { *this, std::abs(p_max_speed) }; +} + +rmd_drc_v2::rotation_sensor rmd_drc_v2::acquire_rotation_sensor() +{ + return { *this }; +} + +rmd_drc_v2::servo rmd_drc_v2::acquire_servo(hal::rpm p_max_speed) +{ + return { *this, std::abs(p_max_speed) }; +} + +rmd_drc_v2::temperature_sensor rmd_drc_v2::acquire_temperature_sensor() +{ + return { *this }; +} + +rmd_drc_v2::angular_velocity_sensor +rmd_drc_v2::acquire_angular_velocity_sensor() +{ + return { *this }; +} + +// ============================================================================= +// +// Interface Implementations +// +// ============================================================================= + +void rmd_drc_v2::servo::driver_position(hal::degrees p_position) +{ + m_drc->position_control(p_position, m_max_speed); +} + +hal::celsius rmd_drc_v2::temperature_sensor::driver_read() +{ + m_drc->feedback_request(read::status_2); + return m_drc->feedback().temperature(); +} + +hal::rotation_sensor::read_t rmd_drc_v2::rotation_sensor::driver_read() +{ + m_drc->feedback_request(read::multi_turns_angle); + return { .angle = m_drc->feedback().angle() }; +} + +void rmd_drc_v2::motor::driver_power(float p_power) +{ + m_drc->velocity_control(m_max_speed * p_power); +} + +hal::rpm rmd_drc_v2::angular_velocity_sensor::driver_read() +{ + m_drc->feedback_request(read::status_2); + return m_drc->feedback().speed(); +} +} // namespace hal::actuator diff --git a/src/smart_servo/rmd/mc_x_constants.hpp b/src/smart_servo/rmd/mc_x_constants.hpp index ce8a482..993393f 100644 --- a/src/smart_servo/rmd/mc_x_constants.hpp +++ b/src/smart_servo/rmd/mc_x_constants.hpp @@ -7,6 +7,8 @@ namespace hal::actuator { static constexpr hertz baudrate_hz = 1'000'000; static constexpr float dps_per_lsb_speed = 0.01f; static constexpr float dps_per_lsb_angle = 1.0f; +static constexpr hal::u32 first_device_address = 0x140; +static constexpr hal::u32 last_device_address = first_device_address + 32; /// Messages returned from these motor drivers are the same as motor ID plus /// this offset. static constexpr std::uint32_t response_id_offset = 0x100; diff --git a/src/smart_servo/rmd/mc_x_v2.cpp b/src/smart_servo/rmd/mc_x_v2.cpp new file mode 100644 index 0000000..7f17471 --- /dev/null +++ b/src/smart_servo/rmd/mc_x_v2.cpp @@ -0,0 +1,364 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include "common.hpp" +#include "mc_x_constants.hpp" + +namespace hal::actuator { +namespace { +inline std::int32_t rpm_to_mc_x_speed(rpm p_rpm, float p_dps_per_lsb) +{ + static constexpr float dps_per_rpm = (1.0f / 1.0_deg_per_sec); + float const dps_float = (p_rpm * dps_per_rpm) / p_dps_per_lsb; + return bounds_check(dps_float); +} + +inline hal::u32 motor_id(hal::can_message_finder const& p_message_finder) +{ + // Paranoid safety assertions, such that a change to this value in the future + // alerts the developer to see this function. + static_assert(response_id_offset == 0x100); + // Safe so long as the ID is bounds checked at construction to be between + // 0x140 and 0x160. + return p_message_finder.id() - response_id_offset; +} +} // namespace + +hal::ampere rmd_mc_x_v2::feedback_t::current() const noexcept +{ + static constexpr auto amps_per_lsb = 0.1_A; + return static_cast(raw_current) * amps_per_lsb; +} + +hal::rpm rmd_mc_x_v2::feedback_t::speed() const noexcept +{ + static constexpr auto velocity_per_lsb = 1.0_deg_per_sec; + return static_cast(raw_speed) * velocity_per_lsb; +} + +hal::volts rmd_mc_x_v2::feedback_t::volts() const noexcept +{ + static constexpr float volts_per_lsb = 0.1f; + return static_cast(raw_volts) * volts_per_lsb; +} + +hal::celsius rmd_mc_x_v2::feedback_t::temperature() const noexcept +{ + static constexpr float celsius_per_lsb = 1.0f; + return static_cast(raw_motor_temperature) * celsius_per_lsb; +} + +hal::degrees rmd_mc_x_v2::feedback_t::angle() const noexcept +{ + return static_cast(raw_multi_turn_angle) * dps_per_lsb_speed; +} + +bool rmd_mc_x_v2::feedback_t::motor_stall() const noexcept +{ + return raw_error_state & motor_stall_mask; +} +bool rmd_mc_x_v2::feedback_t::low_pressure() const noexcept +{ + return raw_error_state & low_pressure_mask; +} +bool rmd_mc_x_v2::feedback_t::over_voltage() const noexcept +{ + return raw_error_state & over_voltage_mask; +} +bool rmd_mc_x_v2::feedback_t::over_current() const noexcept +{ + return raw_error_state & over_current_mask; +} +bool rmd_mc_x_v2::feedback_t::power_overrun() const noexcept +{ + return raw_error_state & power_overrun_mask; +} +bool rmd_mc_x_v2::feedback_t::speeding() const noexcept +{ + return raw_error_state & speeding_mask; +} +bool rmd_mc_x_v2::feedback_t::over_temperature() const noexcept +{ + return raw_error_state & over_temperature_mask; +} +bool rmd_mc_x_v2::feedback_t::encoder_calibration_error() const noexcept +{ + return raw_error_state & encoder_calibration_error_mask; +} + +rmd_mc_x_v2::rmd_mc_x_v2(hal::can_transceiver& p_can_transceiver, + hal::can_identifier_filter& p_filter, + hal::steady_clock& p_clock, + float p_gear_ratio, // NOLINT + hal::u32 p_device_id, + hal::time_duration p_max_response_time) + : m_feedback{} + , m_clock(&p_clock) + , m_can(p_can_transceiver, p_device_id + response_id_offset) + , m_gear_ratio(p_gear_ratio) + , m_device_id(p_device_id) + , m_max_response_time(p_max_response_time) +{ + bool const valid_device_id = + first_device_address <= p_device_id && p_device_id <= last_device_address; + + p_filter.allow(p_device_id + response_id_offset); + + bool const invalid_baud_rate = p_can_transceiver.baud_rate() != 1_MHz; + + if (not valid_device_id || invalid_baud_rate) { + hal::safe_throw(hal::argument_out_of_domain(this)); + } + + // Will throw hal::timed_out if the device is not present on the bus to + // respond to this. + feedback_request(read::status_1_and_error_flags); +} + +void rmd_mc_x_v2::send(std::array p_payload) +{ + hal::can_message const payload{ + .id = motor_id(m_can), + .length = 8, + .payload = p_payload, + }; + + // Send payload + m_can.transceiver().send(payload); + + // Wait for the message number to increment + auto const deadline = hal::future_deadline(*m_clock, m_max_response_time); + while (deadline > m_clock->uptime()) { + if (auto const message = m_can.find(); message.has_value()) { + handle_message(*message); + return; + } + } + + hal::safe_throw(hal::timed_out(this)); +} + +void rmd_mc_x_v2::velocity_control(rpm p_rpm) +{ + auto const speed_data = rpm_to_mc_x_speed(p_rpm, dps_per_lsb_speed); + + send({ + hal::value(actuate::speed), + 0x00, + 0x00, + 0x00, + static_cast((speed_data >> 0) & 0xFF), + static_cast((speed_data >> 8) & 0xFF), + static_cast((speed_data >> 16) & 0xFF), + static_cast((speed_data >> 24) & 0xFF), + }); +} + +void rmd_mc_x_v2::position_control(degrees p_angle, rpm p_rpm) // NOLINT +{ + static constexpr float deg_per_lsb = 0.01f; + auto const angle = p_angle / deg_per_lsb; + auto const angle_data = bounds_check(angle); + auto const speed_data = + rpm_to_mc_x_speed(std::abs(p_rpm * m_gear_ratio), dps_per_lsb_angle); + + send({ + hal::value(actuate::position), + 0x00, + static_cast((speed_data >> 0) & 0xFF), + static_cast((speed_data >> 8) & 0xFF), + static_cast((angle_data >> 0) & 0xFF), + static_cast((angle_data >> 8) & 0xFF), + static_cast((angle_data >> 16) & 0xFF), + static_cast((angle_data >> 24) & 0xFF), + }); +} + +void rmd_mc_x_v2::feedback_request(read p_command) +{ + send({ + hal::value(p_command), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + }); +} + +void rmd_mc_x_v2::system_control(system p_system_command) +{ + send({ + hal::value(p_system_command), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + }); +} + +rmd_mc_x_v2::feedback_t const& rmd_mc_x_v2::feedback() const +{ + return m_feedback; +} + +void rmd_mc_x_v2::handle_message(can_message const& p_message) +{ + m_feedback.message_number++; + + if (p_message.length != 8 || + p_message.id != m_device_id + response_id_offset) { + return; + } + + switch (p_message.payload[0]) { + case hal::value(read::status_2): + case hal::value(actuate::torque): + case hal::value(actuate::speed): + case hal::value(actuate::position): { + auto& data = p_message.payload; + m_feedback.raw_motor_temperature = static_cast(data[1]); + m_feedback.raw_current = static_cast((data[3] << 8) | data[2]); + m_feedback.raw_speed = static_cast((data[5] << 8) | data[4]); + m_feedback.encoder = static_cast((data[7] << 8) | data[6]); + break; + } + case hal::value(read::status_1_and_error_flags): { + auto& data = p_message.payload; + m_feedback.raw_motor_temperature = static_cast(data[1]); + // data[3] = Brake release command + m_feedback.raw_volts = static_cast((data[5] << 8) | data[4]); + auto error_state = data[7] << 8 | data[6]; + m_feedback.raw_error_state = static_cast(error_state); + break; + } + case hal::value(read::multi_turns_angle): { + auto& data = p_message.payload; + m_feedback.raw_multi_turn_angle = static_cast( + data[7] << 24 | data[6] << 16 | data[5] << 8 | data[4]); + break; + } + default: + return; + } +} + +// ============================================================================= +// +// Interface Implementation constructors +// +// ============================================================================= + +rmd_mc_x_v2::servo::servo(rmd_mc_x_v2& p_mc_x, hal::rpm p_max_speed) + : m_mc_x(&p_mc_x) + , m_max_speed(p_max_speed) +{ +} +rmd_mc_x_v2::motor::motor(rmd_mc_x_v2& p_mc_x, hal::rpm p_max_speed) + : m_mc_x(&p_mc_x) + , m_max_speed(p_max_speed) +{ +} +rmd_mc_x_v2::temperature::temperature(rmd_mc_x_v2& p_mc_x) + : m_mc_x(&p_mc_x) +{ +} +rmd_mc_x_v2::rotation::rotation(rmd_mc_x_v2& p_mc_x) + : m_mc_x(&p_mc_x) +{ +} +rmd_mc_x_v2::current_sensor::current_sensor(rmd_mc_x_v2& p_mc_x) + : m_mc_x(&p_mc_x) +{ +} + +// ============================================================================= +// +// Interface Implementations +// +// ============================================================================= + +void rmd_mc_x_v2::servo::driver_position(hal::degrees p_position) +{ + m_mc_x->position_control(p_position, m_max_speed); +} + +void rmd_mc_x_v2::motor::driver_power(float p_power) +{ + m_mc_x->velocity_control(m_max_speed * p_power); +} + +hal::celsius rmd_mc_x_v2::temperature::driver_read() +{ + m_mc_x->feedback_request(read::multi_turns_angle); + return m_mc_x->feedback().temperature(); +} + +hal::rotation_sensor::read_t rmd_mc_x_v2::rotation::driver_read() +{ + m_mc_x->feedback_request(read::status_2); + return { .angle = m_mc_x->feedback().angle() }; +} + +hal::ampere rmd_mc_x_v2::current_sensor::driver_read() +{ + m_mc_x->feedback_request(read::status_2); + + return m_mc_x->feedback().current(); +} + +// ============================================================================= +// +// Object acquisition functions +// +// ============================================================================= + +rmd_mc_x_v2::motor rmd_mc_x_v2::acquire_motor(hal::rpm p_max_speed) +{ + return { *this, p_max_speed }; +} +rmd_mc_x_v2::rotation rmd_mc_x_v2::acquire_rotation_sensor() +{ + return { *this }; +} +rmd_mc_x_v2::servo rmd_mc_x_v2::acquire_servo(hal::rpm p_max_speed) +{ + return { *this, p_max_speed }; +} +rmd_mc_x_v2::temperature rmd_mc_x_v2::acquire_temperature_sensor() +{ + return { *this }; +} + +rmd_mc_x_v2::current_sensor rmd_mc_x_v2::acquire_current_sensor() +{ + return { *this }; +} +} // namespace hal::actuator diff --git a/tests/smart_servo/rmd/drc.test.cpp b/tests/smart_servo/rmd/drc.test.cpp index b4d22b9..9778878 100644 --- a/tests/smart_servo/rmd/drc.test.cpp +++ b/tests/smart_servo/rmd/drc.test.cpp @@ -94,11 +94,11 @@ struct rmd_responder : public hal::can }; } // namespace -boost::ut::suite test_rmd_drc = [] { +boost::ut::suite<"test_rmd_drc"> test_rmd_drc = [] { using namespace boost::ut; using namespace std::literals; - "rmd_drc::create()"_test = []() { + "rmd_drc::rmd_drc()"_test = []() { // Setup rmd_responder mock_can; mock::steady_clock mock_steady; @@ -119,7 +119,7 @@ boost::ut::suite test_rmd_drc = [] { expect(that % expected[1] == mock_can.spy_send.history<0>(1)); }; - "rmd_drc::create() failure"_test = []() { + "rmd_drc::rmd_drc() failure"_test = []() { // Setup rmd_responder mock_can; mock::steady_clock mock_steady; diff --git a/tests/smart_servo/rmd/drc_v2.test.cpp b/tests/smart_servo/rmd/drc_v2.test.cpp new file mode 100644 index 0000000..312caef --- /dev/null +++ b/tests/smart_servo/rmd/drc_v2.test.cpp @@ -0,0 +1,358 @@ +// Copyright 2024 Khalil Estell +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include +#include + +namespace hal::actuator { +namespace { +constexpr hal::u32 expected_id = 0x140; +constexpr hal::u32 expected_gear_ratio = 6.0f; + +template +constexpr auto prefilled_messages(hal::byte p_command_byte = 0x00) +{ + std::array prefilled_expected_messages{}; + for (auto& message : prefilled_expected_messages) { + message.id = expected_id; + message.length = 8; + message.payload.fill(0x00); + message.payload[0] = p_command_byte; + } + return prefilled_expected_messages; +}; + +std::queue create_queue() +{ + std::queue new_queue; + for (std::uint64_t i = 0; i < 255; i++) { + new_queue.push(i); + } + return new_queue; +} + +struct rmd_responder : public hal::can_transceiver +{ + /** + * @brief Reset spy information for functions + * + */ + void reset() + { + spy_send.reset(); + } + + /// Spy handler for hal::can::send() + spy_handler spy_send; + std::array receive_buffer; + std::size_t cursor; + + // void recieve + +private: + hal::u32 driver_baud_rate() override + { + return 1_MHz; + } + + void driver_send(can_message const& p_message) override + { + spy_send.record(p_message); + } +}; +} // namespace + +boost::ut::suite<"test_rmd_drc_v2"> test_rmd_drc_v2 = [] { + using namespace boost::ut; + using namespace std::literals; + + "rmd_drc_v2::rmd_drc_v2()"_test = []() { + // Setup + rmd_responder mock_can; + mock::steady_clock mock_steady; + auto queue = create_queue(); + mock_steady.set_uptimes(queue); + mock_steady.set_frequency(1.0_MHz); + hal::can_router router(mock_can); + auto expected = prefilled_messages<2>(); + expected[0].payload[0] = hal::value(rmd_drc_v2::system::off); + expected[1].payload[0] = hal::value(rmd_drc_v2::system::running); + + // Exercise + rmd_drc_v2 driver(router, mock_steady, expected_gear_ratio, expected_id); + + // Verify + expect(that % 2 == mock_can.spy_send.call_history().size()); + expect(that % expected[0] == mock_can.spy_send.history<0>(0)); + expect(that % expected[1] == mock_can.spy_send.history<0>(1)); + }; + + "rmd_drc_v2::rmd_drc_v2() failure"_test = []() { + // Setup + rmd_responder mock_can; + mock::steady_clock mock_steady; + auto queue = create_queue(); + mock_steady.set_uptimes(queue); + mock_steady.set_frequency(1.0_MHz); + hal::can_router router(mock_can); + mock_can.spy_send.trigger_error_on_call( + 1, []() { throw hal::io_error(nullptr); }); + + constexpr can::message_t expected1{ + .id = expected_id, + .payload = { hal::value(rmd_drc_v2::system::off), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00 }, + .length = 8, + }; + + // Exercise + expect(throws([&]() { + rmd_drc_v2 driver(router, mock_steady, expected_gear_ratio, expected_id); + })); + + // Verify + expect(that % 1 == mock_can.spy_send.call_history().size()); + expect(that % expected1 == mock_can.spy_send.history<0>(0)); + }; + + "rmd_drc_v2::velocity_control()"_test = []() { + // Setup + rmd_responder mock_can; + mock::steady_clock mock_steady; + auto queue = create_queue(); + mock_steady.set_uptimes(queue); + mock_steady.set_frequency(1.0_MHz); + hal::can_router router(mock_can); + rmd_drc_v2 driver(router, mock_steady, expected_gear_ratio, expected_id); + mock_can.reset(); + auto expected = + prefilled_messages<7>(hal::value(rmd_drc_v2::actuate::speed)); + std::array injected_rpm{ + 0.0_rpm, 10.0_rpm, 10.0_rpm, 123.0_rpm, 0.0_rpm, 1024.0_rpm, + }; + + using payload_t = decltype(expected[0].payload); + + // Setup: values verified to be correct + expected[1].payload = payload_t{ + 0xa2, 0x0, 0x0, 0x0, 0xa0, 0x8c, 0x0, 0x0, + }; + expected[2].payload = payload_t{ + 0xa2, 0x0, 0x0, 0x0, 0xa0, 0x8c, 0x0, 0x0, + }; + expected[3].payload = payload_t{ + 0xa2, 0x0, 0x0, 0x0, 0xb0, 0xc1, 0x6, 0x0, + }; + expected[4].payload = payload_t{ + 0xa2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + }; + expected[5].payload = payload_t{ + 0xa2, 0x0, 0x0, 0x0, 0x0, 0x40, 0x38, 0x0, + }; + + // Exercise + driver.velocity_control(injected_rpm[0]); + driver.velocity_control(injected_rpm[1]); + driver.velocity_control(injected_rpm[2]); + driver.velocity_control(injected_rpm[3]); + driver.velocity_control(injected_rpm[4]); + driver.velocity_control(injected_rpm[5]); + + // Verify + expect(that % 6 == mock_can.spy_send.call_history().size()); + expect(that % expected[0] == mock_can.spy_send.history<0>(0)); + expect(that % expected[1] == mock_can.spy_send.history<0>(1)); + expect(that % expected[2] == mock_can.spy_send.history<0>(2)); + expect(that % expected[3] == mock_can.spy_send.history<0>(3)); + expect(that % expected[4] == mock_can.spy_send.history<0>(4)); + expect(that % expected[5] == mock_can.spy_send.history<0>(5)); + }; + + "rmd_drc_v2::position_control()"_test = []() { + // Setup + rmd_responder mock_can; + mock::steady_clock mock_steady; + auto queue = create_queue(); + mock_steady.set_uptimes(queue); + mock_steady.set_frequency(1.0_MHz); + hal::can_router router(mock_can); + rmd_drc_v2 driver(router, mock_steady, expected_gear_ratio, expected_id); + mock_can.reset(); + auto expected = + prefilled_messages<6>(hal::value(rmd_drc_v2::actuate::position_2)); + using payload_t = decltype(expected[0].payload); + + // Setup: values verified to be correct + expected[0].payload = payload_t{ + 0xa4, 0x0, 0x68, 0x1, 0x0, 0x0, 0x0, 0x0, + }; + expected[1].payload = payload_t{ + 0xa4, 0x0, 0x68, 0x1, 0x78, 0x69, 0x0, 0x0, + }; + expected[2].payload = payload_t{ + 0xa4, 0x0, 0x68, 0x1, 0xf0, 0xd2, 0x0, 0x0, + }; + expected[3].payload = payload_t{ + 0xa4, 0x0, 0x68, 0x1, 0x20, 0x1c, 0x0, 0x0, + }; + expected[4].payload = payload_t{ + 0xa4, 0x0, 0x68, 0x1, 0xd8, 0xdc, 0xff, 0xff, + }; + expected[5].payload = payload_t{ + 0xa4, 0x0, 0x68, 0x1, 0x40, 0xc6, 0xf9, 0xff, + }; + + // Exercise + driver.position_control(0.0_deg, 10.0_rpm); + driver.position_control(45.0_deg, 10.0_rpm); + driver.position_control(90.0_deg, 10.0_rpm); + driver.position_control(12.0_deg, 10.0_rpm); + driver.position_control(-15.0_deg, 10.0_rpm); + driver.position_control(-680.0_deg, 10.0_rpm); + + // Verify + expect(that % expected[0] == mock_can.spy_send.history<0>(0)); + expect(that % expected[1] == mock_can.spy_send.history<0>(1)); + expect(that % expected[2] == mock_can.spy_send.history<0>(2)); + expect(that % expected[3] == mock_can.spy_send.history<0>(3)); + expect(that % expected[4] == mock_can.spy_send.history<0>(4)); + expect(that % expected[5] == mock_can.spy_send.history<0>(5)); + }; + + "rmd_drc_v2::feedback_request()"_test = []() { + // Setup + rmd_responder mock_can; + mock::steady_clock mock_steady; + auto queue = create_queue(); + mock_steady.set_uptimes(queue); + mock_steady.set_frequency(1.0_MHz); + hal::can_router router(mock_can); + rmd_drc_v2 driver(router, mock_steady, expected_gear_ratio, expected_id); + mock_can.reset(); + + auto expected = prefilled_messages<3>(); + + expected[0].payload[0] = hal::value(rmd_drc_v2::read::multi_turns_angle); + expected[1].payload[0] = + hal::value(rmd_drc_v2::read::status_1_and_error_flags); + expected[2].payload[0] = hal::value(rmd_drc_v2::read::status_2); + + // Exercise + driver.feedback_request(rmd_drc_v2::read::multi_turns_angle); + driver.feedback_request(rmd_drc_v2::read::status_1_and_error_flags); + driver.feedback_request(rmd_drc_v2::read::status_2); + + // Verify + expect(that % 3 == mock_can.spy_send.call_history().size()); + expect(that % expected[0] == mock_can.spy_send.history<0>(0)); + expect(that % expected[1] == mock_can.spy_send.history<0>(1)); + expect(that % expected[2] == mock_can.spy_send.history<0>(2)); + }; + + "rmd_drc_v2::system_control()"_test = []() { + // Setup + rmd_responder mock_can; + mock::steady_clock mock_steady; + auto queue = create_queue(); + mock_steady.set_uptimes(queue); + mock_steady.set_frequency(1.0_MHz); + hal::can_router router(mock_can); + rmd_drc_v2 driver(router, mock_steady, expected_gear_ratio, expected_id); + mock_can.reset(); + + auto expected = prefilled_messages<4>(); + + expected[0].payload[0] = hal::value(rmd_drc_v2::system::clear_error_flag); + expected[1].payload[0] = hal::value(rmd_drc_v2::system::off); + expected[2].payload[0] = hal::value(rmd_drc_v2::system::stop); + expected[3].payload[0] = hal::value(rmd_drc_v2::system::running); + + // Exercise + driver.system_control(rmd_drc_v2::system::clear_error_flag); + driver.system_control(rmd_drc_v2::system::off); + driver.system_control(rmd_drc_v2::system::stop); + driver.system_control(rmd_drc_v2::system::running); + + // Verify + expect(that % 4 == mock_can.spy_send.call_history().size()); + expect(that % expected[0] == mock_can.spy_send.history<0>(0)); + expect(that % expected[1] == mock_can.spy_send.history<0>(1)); + expect(that % expected[2] == mock_can.spy_send.history<0>(2)); + expect(that % expected[3] == mock_can.spy_send.history<0>(3)); + }; + + "rmd_drc_v2::operator() update feedback status_2 "_test = []() { + // Setup + rmd_responder mock_can; + mock::steady_clock mock_steady; + auto queue = create_queue(); + mock_steady.set_uptimes(queue); + mock_steady.set_frequency(1.0_MHz); + hal::can_router router(mock_can); + rmd_drc_v2 driver(router, mock_steady, expected_gear_ratio, expected_id); + + auto expected = prefilled_messages<1>(); + expected[0].payload[0] = hal::value(rmd_drc_v2::read::status_2); + expected[0].payload[1] = 0x11; // temperature + expected[0].payload[2] = 0x22; // current low byte + expected[0].payload[3] = 0x33; // current high byte + expected[0].payload[4] = 0x44; // speed low byte + expected[0].payload[5] = 0x55; // speed high byte + expected[0].payload[6] = 0x66; // encoder low byte + expected[0].payload[7] = 0x77; // encoder high byte + + // Exercise + driver(expected[0]); + + // Verify + expect(that % 0x11 == driver.feedback().raw_motor_temperature); + expect(that % 0x3322 == driver.feedback().raw_current); + expect(that % 0x5544 == driver.feedback().raw_speed); + expect(that % 0x7766 == driver.feedback().encoder); + }; + + "rmd_drc_v2::feedback().current() "_test = []() {}; + + "hal::make_()"_test = []() { + // Setup + rmd_responder mock_can; + mock::steady_clock mock_steady; + auto queue = create_queue(); + mock_steady.set_uptimes(queue); + mock_steady.set_frequency(1.0_MHz); + hal::can_router router(mock_can); + rmd_drc_v2 driver(router, mock_steady, expected_gear_ratio, expected_id); + + // Exercise + // Verify + auto motor = hal::make_motor(driver, 100.0_rpm); + auto servo = hal::make_servo(driver, 100.0_rpm); + auto temp = hal::make_temperature_sensor(driver); + auto rotation = hal::make_rotation_sensor(driver); + }; +}; +} // namespace hal::actuator