From 2ce36fdcbf8e6e4ba52add7c4b24f4e23c87f1df Mon Sep 17 00:00:00 2001 From: Khalil Estell Date: Sun, 4 Aug 2024 16:43:25 -0700 Subject: [PATCH] :tada: Initial dump of content --- CMakeLists.txt | 19 +- conanfile.py | 1 + demos/CMakeLists.txt | 7 +- demos/applications/drc.cpp | 111 ++++++ demos/applications/mc_x.cpp | 143 +++++++ demos/applications/rc_servo.cpp | 73 ++++ demos/main.cpp | 43 +- demos/platforms/lpc4078.cpp | 11 +- demos/platforms/stm32f103c8.cpp | 6 +- demos/resource_list.hpp | 13 +- include/libhal-actuator/actuator.hpp | 20 - include/libhal-actuator/rc_servo.hpp | 77 ++++ .../libhal-actuator/smart_servo/rmd/drc.hpp | 370 +++++++++++++++++ .../libhal-actuator/smart_servo/rmd/mc_x.hpp | 334 ++++++++++++++++ src/actuator.cpp | 18 - src/rc_servo.cpp | 85 ++++ src/smart_servo/rmd/common.hpp | 25 ++ src/smart_servo/rmd/drc.cpp | 239 +++++++++++ src/smart_servo/rmd/drc_adaptors.cpp | 104 +++++ src/smart_servo/rmd/drc_constants.hpp | 12 + src/smart_servo/rmd/mc_x.cpp | 244 ++++++++++++ src/smart_servo/rmd/mc_x_adaptors.cpp | 95 +++++ src/smart_servo/rmd/mc_x_constants.hpp | 34 ++ test_package/main.cpp | 11 +- tests/main.test.cpp | 12 +- tests/rc_servo.test.cpp | 159 ++++++++ tests/smart_servo/rmd/drc.test.cpp | 371 ++++++++++++++++++ .../smart_servo/rmd/drc_adaptors.test.cpp | 45 ++- .../rmd/mc_x.test.cpp} | 18 +- 29 files changed, 2605 insertions(+), 95 deletions(-) create mode 100644 demos/applications/drc.cpp create mode 100644 demos/applications/mc_x.cpp create mode 100644 demos/applications/rc_servo.cpp delete mode 100644 include/libhal-actuator/actuator.hpp create mode 100644 include/libhal-actuator/rc_servo.hpp create mode 100644 include/libhal-actuator/smart_servo/rmd/drc.hpp create mode 100644 include/libhal-actuator/smart_servo/rmd/mc_x.hpp delete mode 100644 src/actuator.cpp create mode 100644 src/rc_servo.cpp create mode 100644 src/smart_servo/rmd/common.hpp create mode 100644 src/smart_servo/rmd/drc.cpp create mode 100644 src/smart_servo/rmd/drc_adaptors.cpp create mode 100644 src/smart_servo/rmd/drc_constants.hpp create mode 100644 src/smart_servo/rmd/mc_x.cpp create mode 100644 src/smart_servo/rmd/mc_x_adaptors.cpp create mode 100644 src/smart_servo/rmd/mc_x_constants.hpp create mode 100644 tests/rc_servo.test.cpp create mode 100644 tests/smart_servo/rmd/drc.test.cpp rename demos/applications/actuator.cpp => tests/smart_servo/rmd/drc_adaptors.test.cpp (50%) rename tests/{actuator.test.cpp => smart_servo/rmd/mc_x.test.cpp} (69%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e1bc75..2c73cca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,9 +19,24 @@ libhal_test_and_make_library( LIBRARY_NAME libhal-actuator SOURCES - src/actuator.cpp + src/rc_servo.cpp + src/smart_servo/rmd/drc_adaptors.cpp + src/smart_servo/rmd/drc.cpp + src/smart_servo/rmd/mc_x_adaptors.cpp + src/smart_servo/rmd/mc_x.cpp TEST_SOURCES - tests/actuator.test.cpp tests/main.test.cpp + tests/rc_servo.test.cpp + tests/smart_servo/rmd/drc.test.cpp + tests/smart_servo/rmd/mc_x.test.cpp + tests/smart_servo/rmd/drc_adaptors.test.cpp + + PACKAGES + libhal-canrouter + libhal-mock + + LINK_LIBRARIES + libhal::canrouter + libhal::mock ) diff --git a/conanfile.py b/conanfile.py index 3ed5cce..09e7119 100644 --- a/conanfile.py +++ b/conanfile.py @@ -34,6 +34,7 @@ def requirements(self): # consumers get the libhal and libhal-util headers downstream. bootstrap = self.python_requires["libhal-bootstrap"] bootstrap.module.add_library_requirements(self) + self.requires("libhal-canrouter/[^3.0.0]") def package_info(self): self.cpp_info.libs = ["libhal-actuator"] diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index 6b7ad1d..c7ce024 100644 --- a/demos/CMakeLists.txt +++ b/demos/CMakeLists.txt @@ -18,7 +18,12 @@ project(demos LANGUAGES CXX) libhal_build_demos( DEMOS - actuator + drc + mc_x + rc_servo + + INCLUDES + . PACKAGES libhal-actuator diff --git a/demos/applications/drc.cpp b/demos/applications/drc.cpp new file mode 100644 index 0000000..b010e48 --- /dev/null +++ b/demos/applications/drc.cpp @@ -0,0 +1,111 @@ +// 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_map) +{ + using namespace std::chrono_literals; + using namespace hal::literals; + + auto& clock = *p_map.clock.value(); + auto& console = *p_map.console.value(); + auto& can = *p_map.can.value(); + + hal::print(console, "RMD DRC Smart Servo Application Starting...\n\n"); + + hal::can_router router(can); + hal::actuator::rmd_drc drc(router, clock, 6.0f, 0x141); + + auto print_feedback = [&drc, &console]() { + drc.feedback_request(hal::actuator::rmd_drc::read::status_2); + drc.feedback_request(hal::actuator::rmd_drc::read::multi_turns_angle); + drc.feedback_request( + hal::actuator::rmd_drc::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(); + } +} diff --git a/demos/applications/mc_x.cpp b/demos/applications/mc_x.cpp new file mode 100644 index 0000000..cec7900 --- /dev/null +++ b/demos/applications/mc_x.cpp @@ -0,0 +1,143 @@ +// 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_map) +{ + using namespace std::chrono_literals; + using namespace hal::literals; + + auto& clock = *p_map.clock.value(); + auto& console = *p_map.console.value(); + auto& can = *p_map.can.value(); + + hal::print(console, "RMD MC-X Smart Servo Application Starting...\n\n"); + + hal::can_router router(can); + hal::actuator::rmd_mc_x mc_x(router, clock, 36.0f, 0x141); + + auto print_feedback = [&mc_x, &console]() { + mc_x.feedback_request(hal::actuator::rmd_mc_x::read::status_2); + mc_x.feedback_request(hal::actuator::rmd_mc_x::read::multi_turns_angle); + mc_x.feedback_request( + hal::actuator::rmd_mc_x::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(); + } +} diff --git a/demos/applications/rc_servo.cpp b/demos/applications/rc_servo.cpp new file mode 100644 index 0000000..7f4614a --- /dev/null +++ b/demos/applications/rc_servo.cpp @@ -0,0 +1,73 @@ +// 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 + +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& pwm = *p_map.pwm.value(); + + hal::print(console, "RC Servo Application Starting...\n\n"); + + hal::actuator::rc_servo::settings rc_servo_settings{ + .frequency = 50, + // Total 180 deg, change for your use case. + .min_angle = -90, + .max_angle = 90, + // Change to 500us and 2500us if your rc servo + // supports those pulse widths. + .min_microseconds = 1000, + .max_microseconds = 2000, + }; + hal::actuator::rc_servo servo(pwm, rc_servo_settings); + + hal::print(console, "In 5 seconds...\n"); + hal::delay(clock, 2000ms); + + // Oscillate from the servo horn back and forth + while (true) { + hal::print(console, "0 deg\n"); + servo.position(0.0_deg); + hal::delay(clock, 5000ms); + + hal::print(console, "45 deg\n"); + servo.position(45.0_deg); + hal::delay(clock, 5000ms); + + hal::print(console, "90 deg\n"); + servo.position(90.0_deg); + hal::delay(clock, 5000ms); + + hal::print(console, "45 deg\n"); + servo.position(45.0_deg); + hal::delay(clock, 5000ms); + + hal::print(console, "0 deg\n"); + servo.position(0.0_deg); + hal::delay(clock, 5000ms); + + hal::print(console, "-45 deg\n"); + servo.position(-45.0_deg); + hal::delay(clock, 5000ms); + } +} diff --git a/demos/main.cpp b/demos/main.cpp index 4e1620b..c1c333e 100644 --- a/demos/main.cpp +++ b/demos/main.cpp @@ -13,21 +13,31 @@ // limitations under the License. #include +#include #include #include -#include "resource_list.hpp" +#include resource_list resources{}; [[noreturn]] void terminate_handler() noexcept { - using namespace std::chrono_literals; - auto& led = *resources.led; - auto& clock = *resources.clock; + if (not resources.status_led && not resources.console) { + // spin here until debugger is connected + while (true) { + continue; + } + } + + // Otherwise, blink the led in a pattern + + auto& led = *resources.status_led.value(); + auto& clock = *resources.clock.value(); while (true) { + using namespace std::chrono_literals; led.level(false); hal::delay(clock, 100ms); led.level(true); @@ -41,17 +51,26 @@ resource_list resources{}; int main() { - // Set terminate routine... - hal::set_terminate(terminate_handler); - try { resources = initialize_platform(); } catch (...) { - // Catch all exceptions preventing terminate from - hal::halt(); + while (true) { + // halt here and wait for a debugger to connect + continue; + } } - application(resources); - resources.reset(); - return 0; + hal::set_terminate(terminate_handler); + + try { + application(resources); + } catch (std::bad_optional_access const& e) { + if (resources.console) { + hal::print(*resources.console.value(), + "A resource required by the application was not available!\n" + "Calling terminate!\n"); + } + } // Allow any other exceptions to terminate the application + + std::terminate(); } diff --git a/demos/platforms/lpc4078.cpp b/demos/platforms/lpc4078.cpp index 720e96b..f8d762c 100644 --- a/demos/platforms/lpc4078.cpp +++ b/demos/platforms/lpc4078.cpp @@ -18,17 +18,18 @@ #include #include #include +#include #include #include -#include "../resource_list.hpp" +#include resource_list initialize_platform() { using namespace hal::literals; // Set the MCU to the maximum clock speed - hal::lpc40::maximum(10.0_MHz); + hal::lpc40::maximum(12.0_MHz); // Create a hardware counter static hal::cortex_m::dwt_counter counter( @@ -43,11 +44,13 @@ resource_list initialize_platform() }); static hal::lpc40::output_pin led(1, 10); + static hal::lpc40::pwm pwm(1, 6); return { + .reset = []() { hal::cortex_m::reset(); }, .console = &uart0, .clock = &counter, - .led = &led, - .reset = []() { hal::cortex_m::reset(); }, + .status_led = &led, + .pwm = &pwm, }; } diff --git a/demos/platforms/stm32f103c8.cpp b/demos/platforms/stm32f103c8.cpp index e8ab5a8..bd8f4ee 100644 --- a/demos/platforms/stm32f103c8.cpp +++ b/demos/platforms/stm32f103c8.cpp @@ -23,7 +23,7 @@ #include #include -#include "../resource_list.hpp" +#include resource_list initialize_platform() { @@ -44,9 +44,9 @@ resource_list initialize_platform() static hal::stm32f1::output_pin led('C', 13); return { + .reset = +[]() { hal::cortex_m::reset(); }, .console = &uart1, .clock = &counter, - .led = &led, - .reset = +[]() { hal::cortex_m::reset(); }, + .status_led = &led, }; } diff --git a/demos/resource_list.hpp b/demos/resource_list.hpp index 13b24f1..38ef1c1 100644 --- a/demos/resource_list.hpp +++ b/demos/resource_list.hpp @@ -14,18 +14,23 @@ #pragma once +#include + +#include #include #include +#include #include #include struct resource_list { - hal::serial* console; - hal::steady_clock* clock; - hal::output_pin* led; hal::callback reset; - // Add more driver interfaces here ... + std::optional console = std::nullopt; + std::optional clock = std::nullopt; + std::optional status_led = std::nullopt; + std::optional can = std::nullopt; + std::optional pwm = std::nullopt; }; // Application function is implemented by one of the .cpp files. diff --git a/include/libhal-actuator/actuator.hpp b/include/libhal-actuator/actuator.hpp deleted file mode 100644 index b655908..0000000 --- a/include/libhal-actuator/actuator.hpp +++ /dev/null @@ -1,20 +0,0 @@ -// 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 - -namespace hal::actuator { -struct actuator_replace_me -{}; -} // namespace hal::actuator diff --git a/include/libhal-actuator/rc_servo.hpp b/include/libhal-actuator/rc_servo.hpp new file mode 100644 index 0000000..a745725 --- /dev/null +++ b/include/libhal-actuator/rc_servo.hpp @@ -0,0 +1,77 @@ +// 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. + +// Use "#pragma once" as an include guard for headers +// This is required because it ensures that the compiler will process this file +// only once, no matter how many times it is included. +#pragma once + +#include +#include + +// Keep drivers within the namespace hal to not pollute global namespace +namespace hal::actuator { +/** + * @brief Generic RC servo driver. + * + */ +class rc_servo : public hal::servo +{ +public: + /** + * @brief Information about the RC servo needed to control it properly + * + */ + struct settings + { + /// @brief PWM signal frequency. Check the documentation for the RC servo to + /// determine what range of frequencies can be used with it. + hal::hertz frequency = 50; + /// @brief The physical minimum angle that the servo shaft can move to + hal::degrees min_angle = 0; + /// @brief The physical maximum angle that the servo shaft can move to + hal::degrees max_angle = 90; + /// @brief The minimum pulse width in microseconds that maps to the minimum + /// angle of the servo + std::uint32_t min_microseconds = 1000; + /// @brief The maximum pulse width in microseconds that maps to the maximum + /// angle of the servo. + std::uint32_t max_microseconds = 2000; + }; + + /** + * @brief Create a rc_servo object. + * + * @param p_pwm - pwm signal connected to the RC servo + * @param p_settings - RC servo settings + */ + rc_servo(hal::pwm& p_pwm, settings const& p_settings); + +private: + struct ranges + { + std::pair percent{}; + std::pair angle{}; + }; + // Use override keyword to make it clear that this api is for virtual + // functions + void driver_position(hal::degrees p_position) override; + + // Use m_ prefix for private/protected class members. + // Use a pointer here rather than a reference, because member references + // implicitly delete move constructors + hal::pwm* m_pwm; + ranges m_ranges{}; +}; +} // namespace hal::actuator diff --git a/include/libhal-actuator/smart_servo/rmd/drc.hpp b/include/libhal-actuator/smart_servo/rmd/drc.hpp new file mode 100644 index 0000000..25abcc7 --- /dev/null +++ b/include/libhal-actuator/smart_servo/rmd/drc.hpp @@ -0,0 +1,370 @@ +// 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 +{ +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 Create a new device driver drc + * + * This factory function will power cycle the motor + * + * @param p_router - can router to use + * @param p_clock - clocked used to determine timeouts + * @param p_gear_ratio - gear ratio of the motor + * @param p_device_id - The CAN ID 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( + hal::can_router& p_router, + hal::steady_clock& p_clock, + float p_gear_ratio, + can::id_t p_device_id, + hal::time_duration p_max_response_time = std::chrono::milliseconds(10)); + + rmd_drc(rmd_drc&) = delete; + rmd_drc& operator=(rmd_drc&) = delete; + rmd_drc(rmd_drc&&) noexcept = delete; + rmd_drc& operator=(rmd_drc&&) noexcept = delete; + + /** + * @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); + + feedback_t const& feedback() const; + + /** + * @brief Handle messages from the canbus with this devices ID + * + * Meant mostly for testing purposes. + * + * @param p_message - message received from the bus + */ + void operator()(can::message_t 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_router* m_router; + hal::can_router::route_item m_route_item; + float m_gear_ratio; + can::id_t m_device_id; + hal::time_duration m_max_response_time; +}; + +/** + * @brief Rotation sensor adaptor for DRC motors + * + */ +class rmd_drc_rotation_sensor : public hal::rotation_sensor +{ +private: + rmd_drc_rotation_sensor(rmd_drc& p_drc); + hal::rotation_sensor::read_t driver_read() override; + friend rmd_drc_rotation_sensor make_rotation_sensor(rmd_drc& p_drc); + rmd_drc* m_drc = nullptr; +}; + +/** + * @brief Create a hal::rotation_sensor driver using the drc driver + * + * @param p_drc - reference to a drc driver. This object's lifetime must + * exceed the lifetime of the returned object. + * @return drc_rotation_sensor - motor implementation based on the drc driver + */ +rmd_drc_rotation_sensor make_rotation_sensor(rmd_drc& p_drc); + +/** + * @brief Temperature sensor adaptor for DRC motors + * + */ +class rmd_drc_temperature_sensor : public hal::temperature_sensor +{ +private: + rmd_drc_temperature_sensor(rmd_drc& p_drc); + celsius driver_read() override; + friend rmd_drc_temperature_sensor make_temperature_sensor(rmd_drc& p_drc); + rmd_drc* m_drc = nullptr; +}; + +/** + * @brief Create a hal::temperature_sensor driver using the drc driver + * + * @param p_drc - reference to a drc driver. This object's lifetime must exceed + * the lifetime of the returned object. + * @return drc_temperature_sensor - temperature sensor implementation based on + * the drc driver. + */ +rmd_drc_temperature_sensor make_temperature_sensor(rmd_drc& p_drc); + +/** + * @brief Motor interface adaptor for DRC + * + */ +class rmd_drc_motor : public hal::motor +{ +private: + rmd_drc_motor(rmd_drc& p_drc, hal::rpm p_max_speed); + void driver_power(float p_power) override; + friend rmd_drc_motor make_motor(rmd_drc& p_drc, hal::rpm p_max_speed); + + rmd_drc* m_drc = nullptr; + hal::rpm m_max_speed; +}; + +/** + * @brief Create a hal::motor implementation from the drc driver + * + * @param p_drc - reference to a drc driver. This object's lifetime must NOT + * exceed the lifetime of the return drc motor. + * @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 + */ +rmd_drc_motor make_motor(rmd_drc& p_drc, hal::rpm p_max_speed); + +/** + * @brief Servo interface adaptor for DRC + * + */ +class rmd_drc_servo : public hal::servo +{ +private: + rmd_drc_servo(rmd_drc& p_drc, hal::rpm p_max_speed); + void driver_position(hal::degrees p_position) override; + friend rmd_drc_servo make_servo(rmd_drc& p_drc, hal::rpm p_max_speed); + rmd_drc* m_drc = nullptr; + hal::rpm m_max_speed; +}; + +/** + * @brief Create a hal::servo driver using the drc driver + * + * @param p_drc - reference to a drc driver. This object's lifetime must + * exceed the lifetime of the returned object. + * @param p_max_speed - maximum speed of the servo when moving to an angle + * @return drc_servo - servo implementation based on the drc driver + */ +rmd_drc_servo make_servo(rmd_drc& p_drc, hal::rpm p_max_speed); + +/** + * @brief angular velocity sensor adaptor for DRC + * + */ +class rmd_drc_angular_velocity_sensor : public hal::angular_velocity_sensor +{ +private: + rmd_drc_angular_velocity_sensor(rmd_drc& p_drc); + hal::rpm driver_read() override; + friend rmd_drc_angular_velocity_sensor make_angular_velocity_sensor( + rmd_drc& p_drc); + rmd_drc* m_drc = nullptr; +}; + +/** + * @brief Create a hal::angular_velocity_sensor driver using the drc driver + * + * @param p_drc - reference to a drc driver. This object's lifetime must exceed + * the lifetime of the returned object + * @return angular_velocity_sensor - angular_velocity_sensor implementation + * based on the drc driver + */ +rmd_drc_angular_velocity_sensor make_angular_velocity_sensor(rmd_drc& p_drc); +} // namespace hal::actuator + +// NOTE: Inject make functions into `hal` namespace +namespace hal { +using actuator::make_angular_velocity_sensor; +using actuator::make_motor; +using actuator::make_rotation_sensor; +using actuator::make_servo; +using actuator::make_temperature_sensor; +} // namespace hal diff --git a/include/libhal-actuator/smart_servo/rmd/mc_x.hpp b/include/libhal-actuator/smart_servo/rmd/mc_x.hpp new file mode 100644 index 0000000..6f2f58b --- /dev/null +++ b/include/libhal-actuator/smart_servo/rmd/mc_x.hpp @@ -0,0 +1,334 @@ +// 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 series motors equip with the MC-X motor driver + * + */ +class rmd_mc_x +{ +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 Create a new mc_x device driver + * + * @param p_router - can router to use + * @param p_clock - clocked used to determine timeouts + * @param p_gear_ratio - gear ratio of the motor + * @param p_device_id - The CAN ID 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_mc_x( + hal::can_router& p_router, + hal::steady_clock& p_clock, + float p_gear_ratio, + can::id_t p_device_id, + hal::time_duration p_max_response_time = std::chrono::milliseconds(10)); + + rmd_mc_x(rmd_mc_x&) = delete; + rmd_mc_x& operator=(rmd_mc_x&) = delete; + rmd_mc_x(rmd_mc_x&&) noexcept; + rmd_mc_x& operator=(rmd_mc_x&&) noexcept; + + /** + * @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 operator()(can::message_t 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_router* m_router; + hal::can_router::route_item m_route_item; + float m_gear_ratio; + can::id_t m_device_id; + hal::time_duration m_max_response_time; +}; + +/** + * @brief Control a mc_x motor driver like a hal::motor + * + */ +class rmd_mc_x_motor : public hal::motor +{ +private: + rmd_mc_x_motor(rmd_mc_x& p_mc_x, hal::rpm p_max_speed); + void driver_power(float p_power) override; + friend rmd_mc_x_motor make_motor(rmd_mc_x& p_mc_x, hal::rpm p_max_speed); + rmd_mc_x* m_mc_x = nullptr; + hal::rpm m_max_speed; +}; + +/** + * @brief Create a hal::motor driver using the MC-X driver + * + * @param p_mc_x - reference to a MC-X driver. This object's lifetime must + * exceed the lifetime of the returned object. + * @param p_max_speed - maximum speed of the motor represented by +1.0 and -1.0 + * @return mc_x_motor - motor implementation using the MC-X driver + */ +rmd_mc_x_motor make_motor(rmd_mc_x& p_mc_x, hal::rpm p_max_speed); + +/** + * @brief Reports the rotation of the DRC motor + * + */ +class rmd_mc_x_rotation : public hal::rotation_sensor +{ +private: + rmd_mc_x_rotation(rmd_mc_x& p_mc_x); + hal::rotation_sensor::read_t driver_read() override; + friend rmd_mc_x_rotation make_rotation_sensor(rmd_mc_x& p_mc_x); + rmd_mc_x* m_mc_x = nullptr; +}; + +/** + * @brief Create a hal::rotation_sensor driver using the MC-X driver + * + * @param p_mc_x - reference to a MC-X driver. This object's lifetime must + * exceed the lifetime of the returned object. + * @return mc_x_rotation - rotation sensor implementation based on the + * MC-X driver + */ +rmd_mc_x_rotation make_rotation_sensor(rmd_mc_x& p_mc_x); + +/** + * @brief Control a mc_x motor driver like a hal::servo + * + */ +class rmd_mc_x_servo : public hal::servo +{ +private: + rmd_mc_x_servo(rmd_mc_x& p_mc_x, hal::rpm p_max_speed); + void driver_position(hal::degrees p_position) override; + friend rmd_mc_x_servo make_servo(rmd_mc_x& p_mc_x, hal::rpm p_max_speed); + rmd_mc_x* m_mc_x = nullptr; + hal::rpm m_max_speed; +}; + +/** + * @brief Create a hal::rotation_sensor driver using the MC-X driver + * + * @param p_mc_x - reference to a MC-X driver. This object's lifetime must + * exceed the lifetime of the returned object. + * @param p_max_speed - maximum speed of the motor when moving to an angle + * @return mc_x_rotation - rotation sensor implementation based on the + * MC-X driver + */ +rmd_mc_x_servo make_servo(rmd_mc_x& p_mc_x, hal::rpm p_max_speed); + +/** + * @brief Reports the temperature of the DRC motor + * + */ +class rmd_mc_x_temperature : public hal::temperature_sensor +{ +private: + rmd_mc_x_temperature(rmd_mc_x& p_mc_x); + hal::celsius driver_read() override; + friend rmd_mc_x_temperature make_temperature_sensor(rmd_mc_x& p_mc_x); + rmd_mc_x* m_mc_x = nullptr; +}; + +/** + * @brief Create a hal::temperature_sensor driver using the MC-X driver + * + * @param p_mc_x - reference to a MC-X driver. This object's lifetime must + * exceed the lifetime of the returned object. + * @return mc_x_temperature - temperature sensor implementation based on + * the MC-X driver + */ +rmd_mc_x_temperature make_temperature_sensor(rmd_mc_x& p_mc_x); + +/** + * @brief current sensor adaptor for mc_x + * + */ +class rmd_mc_x_current_sensor : public hal::current_sensor +{ +private: + rmd_mc_x_current_sensor(rmd_mc_x& p_mc_x); + hal::ampere driver_read() override; + friend rmd_mc_x_current_sensor make_current_sensor(rmd_mc_x& p_mc_x); + rmd_mc_x* m_mc_x = nullptr; +}; + +/** + * @brief Create a hal::current_sensor driver using the mc_x driver + * + * @param p_mc_x - reference to a mc_x driver. This object's lifetime must + * exceed the lifetime of the returned object + * @return mc_x_current_sensor - current_sensor implementation based on the mc_x + * driver + */ +rmd_mc_x_current_sensor make_current_sensor(rmd_mc_x& p_mc_x); +} // namespace hal::actuator + +// NOTE: Inject make functions into `hal` namespace +namespace hal { +using actuator::make_current_sensor; +using actuator::make_motor; +using actuator::make_rotation_sensor; +using actuator::make_servo; +using actuator::make_temperature_sensor; +} // namespace hal diff --git a/src/actuator.cpp b/src/actuator.cpp deleted file mode 100644 index 83c5d40..0000000 --- a/src/actuator.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// 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 "libhal-actuator/actuator.hpp" - -namespace hal::actuator { -} // namespace hal::actuator diff --git a/src/rc_servo.cpp b/src/rc_servo.cpp new file mode 100644 index 0000000..784c35b --- /dev/null +++ b/src/rc_servo.cpp @@ -0,0 +1,85 @@ +// 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 + +namespace hal::actuator { +rc_servo::rc_servo(hal::pwm& p_pwm, settings const& p_settings) + : m_pwm(&p_pwm) +{ + // Check if any errors happened while setting the frequency of the pwm. + // Using HAL_CHECK will return any errors that occur from the factory + // function allowing the caller to choose what to do with the error + // information. + p_pwm.frequency(p_settings.frequency); + + // Calculate the wavelength in microseconds. + auto wavelength = (1.0f / p_settings.frequency) * std::micro::den; + // min_percent represents the minimum float to be used with the pwm + // signal. The float is calculated by using the minimum width of the + // signal in microseconds divided by the wavelength to get the decimal + // representation of the float. + auto min_percent = + static_cast(p_settings.min_microseconds) / wavelength; + // max_percent represents the maximum float to be used with the pwm + // signal. The float is calculated by using the maximum width of the + // signal in microseconds divided by the wavelength to get the decimal + // representation of the float. + auto max_percent = + static_cast(p_settings.max_microseconds) / wavelength; + // percent_range holds float value of min_percent and max_percent for use + // with map function used in position(). + auto percent_range = std::make_pair(min_percent, max_percent); + // angle_range holds degrees values of MinAngle and MaxAngle for use with + // map function and range checking used in postion() + auto angle_range = std::make_pair(static_cast(p_settings.min_angle), + static_cast(p_settings.max_angle)); + // If no errors happen, call the constructor with verified parameters + m_ranges = ranges{ .percent = percent_range, .angle = angle_range }; +} + +// Drivers must implement functions that are listed in interface. +void rc_servo::driver_position(hal::degrees p_position) +{ + // The angle of p_position should be within the min and max + // angles of the servo. If the provided position is out of the + // provided range, an invalid_argument error is thrown. + if (p_position < std::get<0>(m_ranges.angle) || + p_position > std::get<1>(m_ranges.angle)) { + hal::safe_throw(hal::argument_out_of_domain(this)); + } + // The range of p_position should be within the servo's angle + // range that was defined during creation. The value of + // p_position is mapped within the float range of the pwm + // signal to get the decimal value of the scaled_float. + // + // Example: + // pwm min float: float(0.05) + // pwm max float: float(0.25) + // min angle: float(0.0) + // max angle: float(180.0) + // + // float(0.05) = position(float(0.0)) + // float(0.10) = position(float(45.0)) + // float(0.15) = position(float(90.0)) + // float(0.20) = position(float(135.0)) + // float(0.25) = position(float(180.0)) + auto scaled_percent_raw = map(p_position, m_ranges.angle, m_ranges.percent); + auto scaled_percent = float(scaled_percent_raw); + // Set the duty cycle of the pwm with the scaled percent. + m_pwm->duty_cycle(scaled_percent); +} +} // namespace hal::actuator diff --git a/src/smart_servo/rmd/common.hpp b/src/smart_servo/rmd/common.hpp new file mode 100644 index 0000000..d55d681 --- /dev/null +++ b/src/smart_servo/rmd/common.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include + +#include + +namespace hal { +inline constexpr can::message_t message(hal::can::id_t p_device_id, + std::array p_payload) +{ + can::message_t message{ .id = p_device_id, .length = 8 }; + message.payload = p_payload; + return message; +} + +template +T bounds_check(std::floating_point auto p_float) +{ + using float_t = decltype(p_float); + constexpr auto min = static_cast(std::numeric_limits::min()); + constexpr auto max = static_cast(std::numeric_limits::max()); + + return static_cast(std::clamp(p_float, min, max)); +} +} // namespace hal diff --git a/src/smart_servo/rmd/drc.cpp b/src/smart_servo/rmd/drc.cpp new file mode 100644 index 0000000..a0d3d75 --- /dev/null +++ b/src/smart_servo/rmd/drc.cpp @@ -0,0 +1,239 @@ +// 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 "common.hpp" +#include "drc_constants.hpp" + +namespace hal::actuator { +hal::ampere rmd_drc::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::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::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::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::feedback_t::angle() const noexcept +{ + return static_cast(raw_multi_turn_angle) * dps_per_lsb_speed; +} + +bool rmd_drc::feedback_t::over_voltage_protection_tripped() const noexcept +{ + return raw_error_state & over_temperature_protection_tripped_mask; +} + +bool rmd_drc::feedback_t::over_temperature_protection_tripped() const noexcept +{ + return raw_error_state & over_temperature_protection_tripped_mask; +} + +rmd_drc::feedback_t const& rmd_drc::feedback() const +{ + return m_feedback; +} + +rmd_drc::rmd_drc(hal::can_router& p_router, + hal::steady_clock& p_clock, + float p_gear_ratio, // NOLINT + can::id_t p_device_id, + hal::time_duration p_max_response_time) + : m_feedback{} + , m_clock(&p_clock) + , m_router(&p_router) + , m_route_item(p_router.add_message_callback(p_device_id)) + , m_gear_ratio(p_gear_ratio) + , m_device_id(p_device_id) + , m_max_response_time(p_max_response_time) +{ + m_route_item.get().handler = std::ref(*this); + + rmd_drc::system_control(system::off); + 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 + auto original_message_number = feedback().message_number; + + // Send payload + m_router->bus().send(message(m_device_id, p_payload)); + + // Wait for the message number to increment + auto timeout = hal::create_timeout(*m_clock, m_max_response_time); + while (true) { + if (original_message_number != feedback().message_number) { + return; + } + timeout(); + } +} + +void rmd_drc::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::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::feedback_request(read p_command) +{ + send({ + hal::value(p_command), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + }); +} + +void rmd_drc::system_control(system p_system_command) +{ + send({ + hal::value(p_system_command), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + }); +} + +void rmd_drc::operator()(can::message_t const& p_message) +{ + m_feedback.message_number++; + + if (p_message.length != 8 || p_message.id != m_device_id) { + return; + } + + 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; + } +} +} // namespace hal::actuator diff --git a/src/smart_servo/rmd/drc_adaptors.cpp b/src/smart_servo/rmd/drc_adaptors.cpp new file mode 100644 index 0000000..dc0579c --- /dev/null +++ b/src/smart_servo/rmd/drc_adaptors.cpp @@ -0,0 +1,104 @@ +// 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 + +namespace hal::actuator { +rmd_drc_servo::rmd_drc_servo(rmd_drc& p_drc, hal::rpm p_max_speed) + : m_drc(&p_drc) + , m_max_speed(p_max_speed) +{ +} + +void rmd_drc_servo::driver_position(hal::degrees p_position) +{ + m_drc->position_control(p_position, m_max_speed); +} + +rmd_drc_temperature_sensor::rmd_drc_temperature_sensor(rmd_drc& p_drc) + : m_drc(&p_drc) +{ +} + +hal::celsius rmd_drc_temperature_sensor::driver_read() +{ + m_drc->feedback_request(hal::actuator::rmd_drc::read::status_2); + return m_drc->feedback().temperature(); +} + +rmd_drc_rotation_sensor::rmd_drc_rotation_sensor(rmd_drc& p_drc) + : m_drc(&p_drc) +{ +} + +hal::rotation_sensor::read_t rmd_drc_rotation_sensor::driver_read() +{ + m_drc->feedback_request(hal::actuator::rmd_drc::read::multi_turns_angle); + return { .angle = m_drc->feedback().angle() }; +} + +actuator::rmd_drc_rotation_sensor make_rotation_sensor(rmd_drc& p_drc) +{ + return { p_drc }; +} + +actuator::rmd_drc_servo make_servo(rmd_drc& p_drc, hal::rpm p_max_speed) +{ + return { p_drc, std::abs(p_max_speed) }; +} + +actuator::rmd_drc_temperature_sensor make_temperature_sensor(rmd_drc& p_drc) +{ + return { p_drc }; +} + +rmd_drc_motor::rmd_drc_motor(rmd_drc& p_drc, hal::rpm p_max_speed) + : m_drc(&p_drc) + , m_max_speed(p_max_speed) +{ +} + +void rmd_drc_motor::driver_power(float p_power) +{ + m_drc->velocity_control(m_max_speed * p_power); +} + +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) +{ +} + +hal::rpm rmd_drc_angular_velocity_sensor::driver_read() +{ + m_drc->feedback_request(rmd_drc::read::status_2); + return m_drc->feedback().speed(); +} + +rmd_drc_angular_velocity_sensor make_angular_velocity_sensor(rmd_drc& p_drc) +{ + return { p_drc }; +} +} // namespace hal::actuator \ No newline at end of file diff --git a/src/smart_servo/rmd/drc_constants.hpp b/src/smart_servo/rmd/drc_constants.hpp new file mode 100644 index 0000000..9ae6919 --- /dev/null +++ b/src/smart_servo/rmd/drc_constants.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include + +namespace hal::actuator { +/// Operating baudrate of all RMD-X smart servos +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 std::uint8_t over_voltage_protection_tripped_mask = 0b1; +static constexpr std::uint8_t over_temperature_protection_tripped_mask = 0b100; +} // namespace hal::actuator diff --git a/src/smart_servo/rmd/mc_x.cpp b/src/smart_servo/rmd/mc_x.cpp new file mode 100644 index 0000000..c50fe23 --- /dev/null +++ b/src/smart_servo/rmd/mc_x.cpp @@ -0,0 +1,244 @@ +// 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 "common.hpp" +#include "mc_x_constants.hpp" + +namespace hal::actuator { + +hal::ampere rmd_mc_x::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::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::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::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::feedback_t::angle() const noexcept +{ + return static_cast(raw_multi_turn_angle) * dps_per_lsb_speed; +} + +bool rmd_mc_x::feedback_t::motor_stall() const noexcept +{ + return raw_error_state & motor_stall_mask; +} +bool rmd_mc_x::feedback_t::low_pressure() const noexcept +{ + return raw_error_state & low_pressure_mask; +} +bool rmd_mc_x::feedback_t::over_voltage() const noexcept +{ + return raw_error_state & over_voltage_mask; +} +bool rmd_mc_x::feedback_t::over_current() const noexcept +{ + return raw_error_state & over_current_mask; +} +bool rmd_mc_x::feedback_t::power_overrun() const noexcept +{ + return raw_error_state & power_overrun_mask; +} +bool rmd_mc_x::feedback_t::speeding() const noexcept +{ + return raw_error_state & speeding_mask; +} +bool rmd_mc_x::feedback_t::over_temperature() const noexcept +{ + return raw_error_state & over_temperature_mask; +} +bool rmd_mc_x::feedback_t::encoder_calibration_error() const noexcept +{ + return raw_error_state & encoder_calibration_error_mask; +} + +rmd_mc_x::rmd_mc_x(hal::can_router& p_router, + hal::steady_clock& p_clock, + float p_gear_ratio, // NOLINT + can::id_t p_device_id, + hal::time_duration p_max_response_time) + : m_feedback{} + , m_clock(&p_clock) + , m_router(&p_router) + , m_route_item( + p_router.add_message_callback(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) +{ + m_route_item.get().handler = std::ref(*this); + // TODO(#3): determine if the device actually exists before fully constructing + // this object. +} + +void rmd_mc_x::send(std::array p_payload) +{ + // Capture the message number prior to the send command + auto original_message_number = feedback().message_number; + + // Send payload + m_router->bus().send(message(m_device_id, p_payload)); + + // Wait for the message number to increment + auto timeout = hal::create_timeout(*m_clock, m_max_response_time); + while (true) { + if (original_message_number != feedback().message_number) { + return; + } + timeout(); + } +} + +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); +} + +void rmd_mc_x::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::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::feedback_request(read p_command) +{ + send({ + hal::value(p_command), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + }); +} + +void rmd_mc_x::system_control(system p_system_command) +{ + send({ + hal::value(p_system_command), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + }); +} + +rmd_mc_x::feedback_t const& rmd_mc_x::feedback() const +{ + return m_feedback; +} + +void rmd_mc_x::operator()(can::message_t 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; + } +} +} // namespace hal::actuator diff --git a/src/smart_servo/rmd/mc_x_adaptors.cpp b/src/smart_servo/rmd/mc_x_adaptors.cpp new file mode 100644 index 0000000..8859fb4 --- /dev/null +++ b/src/smart_servo/rmd/mc_x_adaptors.cpp @@ -0,0 +1,95 @@ +// 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 + +namespace hal::actuator { +rmd_mc_x_servo::rmd_mc_x_servo(rmd_mc_x& p_mc_x, hal::rpm p_max_speed) + : m_mc_x(&p_mc_x) + , m_max_speed(p_max_speed) +{ +} + +void rmd_mc_x_servo::driver_position(hal::degrees p_position) +{ + m_mc_x->position_control(p_position, m_max_speed); +} + +rmd_mc_x_motor::rmd_mc_x_motor(rmd_mc_x& p_mc_x, hal::rpm p_max_speed) + : m_mc_x(&p_mc_x) + , m_max_speed(p_max_speed) +{ +} + +void rmd_mc_x_motor::driver_power(float p_power) +{ + m_mc_x->velocity_control(m_max_speed * p_power); +} + +rmd_mc_x_temperature::rmd_mc_x_temperature(rmd_mc_x& p_mc_x) + : m_mc_x(&p_mc_x) +{ +} + +hal::celsius rmd_mc_x_temperature::driver_read() +{ + m_mc_x->feedback_request(hal::actuator::rmd_mc_x::read::multi_turns_angle); + return m_mc_x->feedback().temperature(); +} + +rmd_mc_x_rotation::rmd_mc_x_rotation(rmd_mc_x& p_mc_x) + : m_mc_x(&p_mc_x) +{ +} + +hal::rotation_sensor::read_t rmd_mc_x_rotation::driver_read() +{ + m_mc_x->feedback_request(hal::actuator::rmd_mc_x::read::status_2); + return { .angle = m_mc_x->feedback().angle() }; +} + +rmd_mc_x_current_sensor::rmd_mc_x_current_sensor(rmd_mc_x& p_mc_x) + : m_mc_x(&p_mc_x) +{ +} + +hal::ampere rmd_mc_x_current_sensor::driver_read() +{ + m_mc_x->feedback_request(hal::actuator::rmd_mc_x::read::status_2); + + return m_mc_x->feedback().current(); +} + +rmd_mc_x_motor make_motor(rmd_mc_x& p_mc_x, hal::rpm p_max_speed) +{ + return { p_mc_x, p_max_speed }; +} +rmd_mc_x_rotation make_rotation_sensor(rmd_mc_x& p_mc_x) +{ + return { p_mc_x }; +} +rmd_mc_x_servo make_servo(rmd_mc_x& p_mc_x, hal::rpm p_max_speed) +{ + return { p_mc_x, p_max_speed }; +} +rmd_mc_x_temperature make_temperature_sensor(rmd_mc_x& p_mc_x) +{ + return { p_mc_x }; +} + +rmd_mc_x_current_sensor make_current_sensor(rmd_mc_x& p_mc_x) +{ + return { p_mc_x }; +} +} // namespace hal::actuator diff --git a/src/smart_servo/rmd/mc_x_constants.hpp b/src/smart_servo/rmd/mc_x_constants.hpp new file mode 100644 index 0000000..ce8a482 --- /dev/null +++ b/src/smart_servo/rmd/mc_x_constants.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include + +namespace hal::actuator { +/// Operating baudrate of all RMD-X smart servos +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; +/// Messages returned from these motor drivers are the same as motor ID plus +/// this offset. +static constexpr std::uint32_t response_id_offset = 0x100; +/// Error state flag indicating the motor is stalling or has stalled +static constexpr std::uint16_t motor_stall_mask = 0x0002; +/// Error state flag indicating the motor is suffering from low pressure... +/// I'm not sure what that means +static constexpr std::uint16_t low_pressure_mask = 0x0004; +/// Error state flag indicating the motor has seen a voltage spike above its +/// maximum rating. +static constexpr std::uint16_t over_voltage_mask = 0x0008; +/// Error state flag indicating the motor drew more current than it was +/// prepared to handle. +static constexpr std::uint16_t over_current_mask = 0x0010; +/// Error state flag indicating the motor drew more power than it was +/// prepared to handle. +static constexpr std::uint16_t power_overrun_mask = 0x0040; +/// NOTE: I don't know what this means exactly... +static constexpr std::uint16_t speeding_mask = 0x0100; +/// Error state flag indicating the motor temperature has exceeded its +/// rating. +static constexpr std::uint16_t over_temperature_mask = 0x1000; +/// Error state flag indicating the encoder calibration failed. +static constexpr std::uint16_t encoder_calibration_error_mask = 0x2000; +} // namespace hal::actuator diff --git a/test_package/main.cpp b/test_package/main.cpp index 7b77428..5b1f7be 100644 --- a/test_package/main.cpp +++ b/test_package/main.cpp @@ -12,9 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + +bool run = false; int main() { - hal::actuator::actuator_replace_me bar; + if (run) { + // This is fine because we don't actually run this code, we just need to + // construct something to verify that it links. + hal::pwm* null_pwm = nullptr; + hal::actuator::rc_servo bar(*null_pwm, {}); + } } diff --git a/tests/main.test.cpp b/tests/main.test.cpp index 9f4a5d0..e891063 100644 --- a/tests/main.test.cpp +++ b/tests/main.test.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -namespace hal::actuator { -extern void actuator_test(); -} // namespace hal::actuator - int main() { - hal::actuator::actuator_test(); + // NOTE: Keep the comment below this one: + // All tests run and execute before main. If a set of tests must be performed + // in a specific order, then place the tests into a free function and call + // that free function below. + + // NOTE: Remove this line and the line below after the first test is called + // No functions called yet... } \ No newline at end of file diff --git a/tests/rc_servo.test.cpp b/tests/rc_servo.test.cpp new file mode 100644 index 0000000..0a6ccc5 --- /dev/null +++ b/tests/rc_servo.test.cpp @@ -0,0 +1,159 @@ +// 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 + +namespace hal::actuator { +boost::ut::suite test_rc_servo = [] { + using namespace boost::ut; + "hal::actuator::rc_servo()"_test = []() { + // Setup + hal::mock::pwm pwm0; + hal::mock::pwm pwm1; + hal::mock::pwm pwm2; + + // Exercise + // Verify + + // use defaults + [[maybe_unused]] rc_servo servo0(pwm0, {}); + // 100Hz (or 10ms per update) with 500us being max negative start and 2500us + // being max positive. + [[maybe_unused]] rc_servo servo1(pwm1, + { + .frequency = 100, + .min_microseconds = 500, + .max_microseconds = 2500, + }); + + pwm2.spy_frequency.trigger_error_on_call( + 1, []() { hal::safe_throw(hal::operation_not_supported(nullptr)); }); + + expect(throws([&]() { rc_servo servo2(pwm2, {}); })); + }; + + "hal::actuator::rc_servo::position"_test = []() { + // Setup + constexpr auto expected0 = float(0.05); + constexpr auto expected1 = float(0.10); + constexpr auto expected2 = float(0.15); + constexpr auto expected3 = float(0.20); + constexpr auto expected4 = float(0.25); + + constexpr auto angle0 = hal::degrees(0); + constexpr auto angle1 = hal::degrees(45); + constexpr auto angle2 = hal::degrees(90); + constexpr auto angle3 = hal::degrees(135); + constexpr auto angle4 = hal::degrees(180); + + hal::mock::pwm pwm; + rc_servo servo(pwm, + { + .frequency = 100, + .min_angle = 0, + .max_angle = 180, + .min_microseconds = 500, + .max_microseconds = 2500, + }); + + // Exercise + servo.position(angle0); + servo.position(angle1); + servo.position(angle2); + servo.position(angle3); + servo.position(angle4); + + // Verify + expect(that % expected0 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(0))); + expect(that % expected1 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(1))); + expect(that % expected2 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(2))); + expect(that % expected3 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(3))); + expect(that % expected4 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(4))); + }; + + "hal::actuator::rc_servo::position negative angles"_test = []() { + // Setup + constexpr auto expected0 = float(0.05); + constexpr auto expected1 = float(0.10); + constexpr auto expected2 = float(0.15); + constexpr auto expected3 = float(0.20); + constexpr auto expected4 = float(0.25); + + constexpr auto angle0 = hal::degrees(-90); + constexpr auto angle1 = hal::degrees(-45); + constexpr auto angle2 = hal::degrees(0); + constexpr auto angle3 = hal::degrees(45); + constexpr auto angle4 = hal::degrees(90); + + hal::mock::pwm pwm; + rc_servo servo(pwm, + { + .frequency = 100, + .min_angle = -90, + .max_angle = 90, + .min_microseconds = 500, + .max_microseconds = 2500, + }); + + // Exercise + servo.position(angle0); + servo.position(angle1); + servo.position(angle2); + servo.position(angle3); + servo.position(angle4); + + // Verify + expect(that % expected0 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(0))); + expect(that % expected1 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(1))); + expect(that % expected2 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(2))); + expect(that % expected3 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(3))); + expect(that % expected4 == + std::get<0>(pwm.spy_duty_cycle.call_history().at(4))); + }; + + "servo errors test"_test = []() { + // Setup + hal::mock::pwm pwm; + constexpr hal::degrees min_angle = 0.0f; + constexpr hal::degrees max_angle = 90.0f; + rc_servo test(pwm, + { + .frequency = 100, + .min_angle = min_angle, + .max_angle = max_angle, + .min_microseconds = 500, + .max_microseconds = 2500, + }); + + // Exercise + // Verify + expect(throws( + [&]() { test.position(max_angle + 45.0f); })); + }; +}; +} // namespace hal::actuator \ No newline at end of file diff --git a/tests/smart_servo/rmd/drc.test.cpp b/tests/smart_servo/rmd/drc.test.cpp new file mode 100644 index 0000000..994a587 --- /dev/null +++ b/tests/smart_servo/rmd/drc.test.cpp @@ -0,0 +1,371 @@ +// 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 + +namespace hal::actuator { +namespace { +constexpr can::id_t expected_id = 0x140; +constexpr can::id_t 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 +{ + /** + * @brief Reset spy information for functions + * + */ + void reset() + { + spy_configure.reset(); + spy_send.reset(); + spy_bus_on.reset(); + } + + /// Spy handler for hal::can::configure() + spy_handler spy_configure; + /// Spy handler for hal::can::send() + spy_handler spy_send; + /// Spy handler for hal::can::bus_on() will always have content of "true" + spy_handler spy_bus_on; + /// Spy handler for hal::can::on_receive() + hal::callback m_on_receive = [](message_t const&) {}; + +private: + void driver_configure(settings const& p_settings) override + { + return spy_configure.record(p_settings); + } + + void driver_bus_on() override + { + return spy_bus_on.record(true); + } + + void driver_send(message_t const& p_message) override + { + spy_send.record(p_message); + m_on_receive(p_message); + } + + void driver_on_receive(hal::callback p_handler) override + { + m_on_receive = p_handler; + } +}; +} // namespace + +boost::ut::suite test_rmd_drc = [] { + using namespace boost::ut; + using namespace std::literals; + + "rmd_drc::create()"_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::system::off); + expected[1].payload[0] = hal::value(rmd_drc::system::running); + + // Exercise + rmd_drc 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::create() 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::system::off), + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00 }, + .length = 8, + }; + + // Exercise + expect(throws([&]() { + rmd_drc 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::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 driver(router, mock_steady, expected_gear_ratio, expected_id); + mock_can.reset(); + auto expected = prefilled_messages<7>(hal::value(rmd_drc::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::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 driver(router, mock_steady, expected_gear_ratio, expected_id); + mock_can.reset(); + auto expected = + prefilled_messages<6>(hal::value(rmd_drc::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::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 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::read::multi_turns_angle); + expected[1].payload[0] = + hal::value(rmd_drc::read::status_1_and_error_flags); + expected[2].payload[0] = hal::value(rmd_drc::read::status_2); + + // Exercise + driver.feedback_request(rmd_drc::read::multi_turns_angle); + driver.feedback_request(rmd_drc::read::status_1_and_error_flags); + driver.feedback_request(rmd_drc::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::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 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::system::clear_error_flag); + expected[1].payload[0] = hal::value(rmd_drc::system::off); + expected[2].payload[0] = hal::value(rmd_drc::system::stop); + expected[3].payload[0] = hal::value(rmd_drc::system::running); + + // Exercise + driver.system_control(rmd_drc::system::clear_error_flag); + driver.system_control(rmd_drc::system::off); + driver.system_control(rmd_drc::system::stop); + driver.system_control(rmd_drc::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::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 driver(router, mock_steady, expected_gear_ratio, expected_id); + + auto expected = prefilled_messages<1>(); + expected[0].payload[0] = hal::value(rmd_drc::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::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 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 \ No newline at end of file diff --git a/demos/applications/actuator.cpp b/tests/smart_servo/rmd/drc_adaptors.test.cpp similarity index 50% rename from demos/applications/actuator.cpp rename to tests/smart_servo/rmd/drc_adaptors.test.cpp index 5b0374b..614871d 100644 --- a/demos/applications/actuator.cpp +++ b/tests/smart_servo/rmd/drc_adaptors.test.cpp @@ -12,25 +12,40 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include -#include "../resource_list.hpp" +#include +#include +#include -void application(resource_list& p_map) +#include + +namespace hal::rmd { +namespace { +struct drc_inert_can : public hal::can { - using namespace std::chrono_literals; - using namespace hal::literals; +private: + void driver_configure(settings const&) override + { + } - auto& clock = *p_map.clock; - auto& console = *p_map.console; - auto& led = *p_map.led; + void driver_bus_on() override + { + } - hal::print(console, "Demo Application Starting...\n\n"); + void driver_send(message_t const&) override + { + } - while (true) { - hal::print(console, "Hello, world\n"); - led.level(!led.level()); // Toggle LED - hal::delay(clock, 500ms); + void driver_on_receive(hal::callback) override + { } -} +}; +} // namespace + +boost::ut::suite test_rmd_drc_adaptors = [] { + using namespace boost::ut; + using namespace std::literals; + using namespace hal::literals; +}; +} // namespace hal::rmd \ No newline at end of file diff --git a/tests/actuator.test.cpp b/tests/smart_servo/rmd/mc_x.test.cpp similarity index 69% rename from tests/actuator.test.cpp rename to tests/smart_servo/rmd/mc_x.test.cpp index f877281..c66f5ca 100644 --- a/tests/actuator.test.cpp +++ b/tests/smart_servo/rmd/mc_x.test.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + +#include +#include +#include #include namespace hal::actuator { -void actuator_test() -{ +boost::ut::suite test_rmd_mc_x = [] { using namespace boost::ut; using namespace std::literals; + using namespace hal::literals; - "actuator::create()"_test = []() { - // Setup - // Exercise - // Verify - }; + "hal::actuator::rmd_mc_x::rmd_mc_x()"_test = []() {}; }; -} // namespace hal::actuator +} // namespace hal::actuator \ No newline at end of file