diff --git a/MODULE.bazel b/MODULE.bazel index a6420a9..4751ff6 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -17,6 +17,7 @@ bazel_dep(name = "aspect_rules_js", version = "1.41.2") bazel_dep(name = "bazel_skylib", version = "1.4.2") bazel_dep(name = "eigen", version = "3.4.0") bazel_dep(name = "gazelle", version = "0.35.0") +bazel_dep(name = "googletest", version = "1.14.0") bazel_dep(name = "hedron_compile_commands", dev_dependency = True) git_override( module_name = "hedron_compile_commands", diff --git a/MODULE.bazel.lock b/MODULE.bazel.lock index 47eec8f..0d9678b 100644 --- a/MODULE.bazel.lock +++ b/MODULE.bazel.lock @@ -4,7 +4,8 @@ "https://bcr.bazel.build/bazel_registry.json": "8a28e4aff06ee60aed2a8c281907fb8bcbf3b753c91fb5a5c57da3215d5b3497", "https://bcr.bazel.build/modules/abseil-cpp/20210324.2/MODULE.bazel": "7cd0312e064fde87c8d1cd79ba06c876bd23630c83466e9500321be55c96ace2", "https://bcr.bazel.build/modules/abseil-cpp/20211102.0/MODULE.bazel": "70390338f7a5106231d20620712f7cccb659cd0e9d073d1991c038eb9fc57589", - "https://bcr.bazel.build/modules/abseil-cpp/20211102.0/source.json": "7e3a9adf473e9af076ae485ed649d5641ad50ec5c11718103f34de03170d94ad", + "https://bcr.bazel.build/modules/abseil-cpp/20230125.1/MODULE.bazel": "89047429cb0207707b2dface14ba7f8df85273d484c2572755be4bab7ce9c3a0", + "https://bcr.bazel.build/modules/abseil-cpp/20230125.1/source.json": "06cc0842d241da0c5edc755edb3c7d0d008d304330e57ecf2d6449fb0b633a82", "https://bcr.bazel.build/modules/apple_support/1.5.0/MODULE.bazel": "50341a62efbc483e8a2a6aec30994a58749bd7b885e18dd96aa8c33031e558ef", "https://bcr.bazel.build/modules/apple_support/1.5.0/source.json": "eb98a7627c0bc486b57f598ad8da50f6625d974c8f723e9ea71bd39f709c9862", "https://bcr.bazel.build/modules/aspect_bazel_lib/1.31.2/MODULE.bazel": "7bee702b4862612f29333590f4b658a5832d433d6f8e4395f090e8f4e85d442f", @@ -49,7 +50,8 @@ "https://bcr.bazel.build/modules/gazelle/0.35.0/MODULE.bazel": "bda67986233654255d52d56c2e8d8ce5649fdcf0acd96b1fdd04af4d7e038c36", "https://bcr.bazel.build/modules/gazelle/0.35.0/source.json": "121e19120e03aa1ad21b0adfb2262d12d7cc907d447962d076951179ea6b6c51", "https://bcr.bazel.build/modules/googletest/1.11.0/MODULE.bazel": "3a83f095183f66345ca86aa13c58b59f9f94a2f81999c093d4eeaa2d262d12f4", - "https://bcr.bazel.build/modules/googletest/1.11.0/source.json": "c73d9ef4268c91bd0c1cd88f1f9dfa08e814b1dbe89b5f594a9f08ba0244d206", + "https://bcr.bazel.build/modules/googletest/1.14.0/MODULE.bazel": "cfbcbf3e6eac06ef9d85900f64424708cc08687d1b527f0ef65aa7517af8118f", + "https://bcr.bazel.build/modules/googletest/1.14.0/source.json": "2478949479000fdd7de9a3d0107ba2c85bb5f961c3ecb1aa448f52549ce310b5", "https://bcr.bazel.build/modules/platforms/0.0.4/MODULE.bazel": "9b328e31ee156f53f3c416a64f8491f7eb731742655a47c9eec4703a71644aee", "https://bcr.bazel.build/modules/platforms/0.0.5/MODULE.bazel": "5733b54ea419d5eaf7997054bb55f6a1d0b5ff8aedf0176fef9eea44f3acda37", "https://bcr.bazel.build/modules/platforms/0.0.6/MODULE.bazel": "ad6eeef431dc52aefd2d77ed20a4b353f8ebf0f4ecdd26a807d2da5aa8cd0615", @@ -66,6 +68,7 @@ "https://bcr.bazel.build/modules/rules_buf/0.1.1/source.json": "021363d254f7438f3f10725355969c974bb2c67e0c28667782ade31a9cdb747f", "https://bcr.bazel.build/modules/rules_cc/0.0.1/MODULE.bazel": "cb2aa0747f84c6c3a78dad4e2049c154f08ab9d166b1273835a8174940365647", "https://bcr.bazel.build/modules/rules_cc/0.0.2/MODULE.bazel": "6915987c90970493ab97393024c156ea8fb9f3bea953b2f3ec05c34f19b5695c", + "https://bcr.bazel.build/modules/rules_cc/0.0.6/MODULE.bazel": "abf360251023dfe3efcef65ab9d56beefa8394d4176dd29529750e1c57eaa33f", "https://bcr.bazel.build/modules/rules_cc/0.0.8/MODULE.bazel": "964c85c82cfeb6f3855e6a07054fdb159aced38e99a5eecf7bce9d53990afa3e", "https://bcr.bazel.build/modules/rules_cc/0.0.9/MODULE.bazel": "836e76439f354b89afe6a911a7adf59a6b2518fafb174483ad78a2a2fde7b1c5", "https://bcr.bazel.build/modules/rules_cc/0.0.9/source.json": "1f1ba6fea244b616de4a554a0f4983c91a9301640c8fe0dd1d410254115c8430", diff --git a/src/math/BUILD.bazel b/src/math/BUILD.bazel new file mode 100644 index 0000000..f5e8ff2 --- /dev/null +++ b/src/math/BUILD.bazel @@ -0,0 +1,18 @@ +cc_library( + name = "math_lib", + hdrs = [ + "pid.h", + ], + visibility = ["//visibility:public"], +) + +cc_library( + name = "vector_lib", + hdrs = [ + "vector_math.h", + ], + visibility = ["//visibility:public"], + deps = [ + "@eigen", + ], +) diff --git a/src/math/pid.h b/src/math/pid.h new file mode 100644 index 0000000..724c989 --- /dev/null +++ b/src/math/pid.h @@ -0,0 +1,5 @@ +struct PIDGain { + float kP; + float kI; + float kD; +}; \ No newline at end of file diff --git a/src/math/vector_math.h b/src/math/vector_math.h new file mode 100644 index 0000000..b44c1dd --- /dev/null +++ b/src/math/vector_math.h @@ -0,0 +1,6 @@ +#include + +struct Location { + Eigen::Vector2d position; + Eigen::Rotation2Df rotation; +}; \ No newline at end of file diff --git a/src/robot/BUILD.bazel b/src/robot/BUILD.bazel index 14f840a..8b645a0 100644 --- a/src/robot/BUILD.bazel +++ b/src/robot/BUILD.bazel @@ -17,13 +17,36 @@ py_library( ], ) +cc_library( + name = "robot_lib", + srcs = [ + "robot.cpp", + ], + hdrs = [ + "motor.h", + "robot.h", + ], + deps = ["@eigen"], +) + cc_binary( name = "robot", srcs = [ "robot.cpp", - "robot.h", ], deps = [ + "robot_lib", + "@eigen", + ], +) + +cc_binary( + name = "main", + srcs = [ + "main.cpp", + ], + deps = [ + "robot_lib", "@eigen", ], ) diff --git a/src/robot/actuator/BUILD.bazel b/src/robot/actuator/BUILD.bazel new file mode 100644 index 0000000..ab9525e --- /dev/null +++ b/src/robot/actuator/BUILD.bazel @@ -0,0 +1,67 @@ +cc_library( + name = "generic_motor_lib", + hdrs = [ + "motor.h", + ], + deps = [ + "//src/math:math_lib", + "//src/math:vector_lib", + "@eigen", + ], +) + +cc_library( + name = "dynamixel_motor_lib", + srcs = [ + "dynamixel_motor.cpp", + ], + hdrs = [ + "dynamixel_motor.h", + ], + deps = [ + ":generic_motor_lib", + "//src/math:math_lib", + "@eigen", + ], +) + +cc_test( + name = "dynamixel_motor_test", + size = "small", + srcs = [ + "dynamixel_motor_test.cpp", + ], + deps = [ + ":dynamixel_motor_lib", + "@eigen", + "@googletest//:gtest_main", + ], +) + +cc_library( + name = "mock_motor_lib", + srcs = [ + "mock_motor.cpp", + ], + hdrs = [ + "mock_motor.h", + ], + deps = [ + ":generic_motor_lib", + "//src/math:math_lib", + "@eigen", + ], +) + +cc_test( + name = "mock_motor_test", + size = "small", + srcs = [ + "mock_motor_test.cpp", + ], + deps = [ + ":mock_motor_lib", + "@eigen", + "@googletest//:gtest_main", + ], +) diff --git a/src/robot/actuator/dynamixel_motor.cpp b/src/robot/actuator/dynamixel_motor.cpp new file mode 100644 index 0000000..7010df0 --- /dev/null +++ b/src/robot/actuator/dynamixel_motor.cpp @@ -0,0 +1,304 @@ +#include "dynamixel_motor.h" + +DynamixelMotor::DynamixelMotor(int id, Location location, bool inverted) + : IMotor(id, location, inverted) { + DynamixelMemoryLayout offsets = { + 24, // torque_enable + 25, // led_enable + 26, // d_gain + 27, // i_gain + 28, // p_gain + 30, // goal_position + 32, // moving_speed + 34, // torque_limit + 36, // present_position + 38, // present_speed + 40, // present_load + 42, // present_input_voltage + 43, // present_temperature + 44, // registered + 46, // moving + 47, // lock + 48, // punch + 50, // realtime_tick + 73, // goal_acceleration + }; + + DynamixelMemoryLayout sizes = { + 1, // torque_enable + 1, // led_enable + 1, // d_gain + 1, // i_gain + 1, // p_gain + 2, // goal_position + 2, // moving_speed + 2, // torque_limit + 2, // present_position + 2, // present_speed + 2, // present_load + 1, // present_input_voltage + 1, // present_temperature + 1, // registered + 1, // moving + 1, // lock + 2, // punch + 2, // realtime_tick + 1, // goal_acceleration + }; + + config_ = {offsets, sizes}; +}; + +bool DynamixelMotor::Initialize_() { return true; } + +bool DynamixelMotor::ValidateCommand_() { return true; } + +bool DynamixelMotor::ReadBytes_(int offset, int size, int *out_bytes) { + return true; +} + +bool DynamixelMotor::WriteBytes_(int offset, int size, int value) { + return true; +} + +bool DynamixelMotor::GetMemoryProperty_(DynamixelPropertyType property, + int *out_value) { + return true; +} + +bool DynamixelMotor::SetMemoryProperty_(DynamixelPropertyType property, + int value) { + return true; +} + +bool DynamixelMotor::RawSpeedToNormalizedSpeed(float raw_speed, + float *out_speed) { + /* + Speed is laid out in memory like this: + 0 0000000000 + | |--------| + | | + | -- Velocity + -- Polarity + + Speed is returned as a value between 0-2048, where: + Stopped: [0, 1024] + CCW: [1-1023] + CW: [1025-2047] + + We want to adjust the ranges to match the following: + (TODO CHECK) CCW Full (-1023) <--- Stopped (0) ---> CW Full (1023) + */ + float adjusted_speed; + + if (raw_speed < 1024) { + // If we are less than the range midpoint, invert the range. + adjusted_speed = -raw_speed; + } else { + // If we are in the second half of the motor's range, shift + // the values back + adjusted_speed = raw_speed - 1024; + } + + /* + Now, we want to scale the speed from this range: + (TODO CHECK) CCW (-1023) <--- Stopped (0) ---> CW (1023) + + To match this range: + (TODO CHECK) CCW (-100) <--- Stopped (0) ---> CW (100) + */ + float scaled_speed = (adjusted_speed / 1023.) * MAX_SPEED; + + /* + The last adjustment we make is to invert the speed, if requested. + This allows for easily handling reversed motors: + (TODO CHECK) CCW (-100) <--- Stopped (0) ---> CW (100) + + If we need to invert, will produce this range: + (TODO CHECK) CW (-100) <--- Stopped (0) ---> CCW (100) + */ + float inverted_speed = this->inverted_ ? -scaled_speed : scaled_speed; + + *out_speed = inverted_speed; + + return true; +} + +bool DynamixelMotor::NormalizedSpeedToRawSpeed(float normalized_speed, + float *out_speed) { + /* + The first adjustment we make is to invert the speed, if requested. + This allows for easily handling reversed motors: + CW (check) (-100) <--- Stopped (0) ---> CCW (100) + + If we need to invert, will produce this range: + (TODO CHECK) CCW (-100) <--- Stopped (0) ---> CW (100) + */ + float non_inverted_speed = + this->inverted_ ? -normalized_speed : normalized_speed; + + /* + Now, we want to scale the speed from this range: + (TODO CHECK) CCW (-100) <--- Stopped (0) ---> CW (100) + + To match this range: + (TODO CHECK) CCW (-1023) <--- Stopped (0) ---> CW (1023) + */ + float scaled_speed = (non_inverted_speed / MAX_SPEED) * 1023.; + + /* + The last adjustment is to convert speed from normalized form, + to raw form. + + Normalized form (right now): + (TODO CHECK) CCW (-1023) <--- Stopped (0) ---> CW (1023) + + Speed is laid out in memory like this: + 0 0000000000 + | |--------| + | | + | -- Velocity + -- Polarity + + We want to adjust speed to match the ranges: + Stopped: [0, 1024] + CCW: [1-1023] + CW: [1025-2047] + */ + float adjusted_speed; + + if (scaled_speed <= 0) { + // If we are less than or at the range midpoint, invert the range. + // If we are at 0, we do not want to offset by 1024. + adjusted_speed = -scaled_speed; + } else { + // If we are in the second half of the motor's range, shift + // the values back + adjusted_speed = scaled_speed + 1024; + } + + *out_speed = adjusted_speed; + + return true; +} + +bool DynamixelMotor::GetPosition(int *out_position) { + return GetMemoryProperty_(DynamixelPropertyType::PRESENT_POSITION, + out_position); +} + +bool DynamixelMotor::SetPosition(int position) { + return SetMemoryProperty_(DynamixelPropertyType::GOAL_POSITION, position); +} + +bool DynamixelMotor::GetSpeed(float *out_speed) { + int raw_speed; + + if (!GetMemoryProperty_(DynamixelPropertyType::PRESENT_SPEED, &raw_speed)) { + // Fail early if we can't read data from the motor's memory. + return false; + } + + return this->RawSpeedToNormalizedSpeed(static_cast(raw_speed), + out_speed); +} + +bool DynamixelMotor::SetSpeed(float speed) { + float raw_speed; + + if (!NormalizedSpeedToRawSpeed(speed, &raw_speed)) { + // Fail early if we don't convert the speed correctly + return false; + } + + int casted_value = static_cast(floor(raw_speed)); + + return SetMemoryProperty_(DynamixelPropertyType::MOVING_SPEED, casted_value); +} + +bool DynamixelMotor::GetAcceleration(float *out_acceleration) { + int raw_acceleration; + + if (!GetMemoryProperty_(DynamixelPropertyType::GOAL_ACCELERATION, + &raw_acceleration)) { + return false; + } + + *out_acceleration = static_cast(raw_acceleration); + + return true; +} + +bool DynamixelMotor::SetAcceleration(float acceleration) { return true; } + +bool DynamixelMotor::GetPIDGain(PIDGain *out_pid_gain) { + int kP, kI, kD; + + if (GetMemoryProperty_(DynamixelPropertyType::P_GAIN, &kP) || + GetMemoryProperty_(DynamixelPropertyType::I_GAIN, &kI) || + GetMemoryProperty_(DynamixelPropertyType::D_GAIN, &kD)) { + return false; + } + + *out_pid_gain = {static_cast(kP), static_cast(kI), + static_cast(kD)}; + return true; +} + +bool DynamixelMotor::SetPIDGain(PIDGain pid_gain) { + int kP = static_cast(floor(pid_gain.kP)); + int kI = static_cast(floor(pid_gain.kI)); + int kD = static_cast(floor(pid_gain.kD)); + + if (SetMemoryProperty_(DynamixelPropertyType::P_GAIN, kP) || + SetMemoryProperty_(DynamixelPropertyType::I_GAIN, kI) || + SetMemoryProperty_(DynamixelPropertyType::D_GAIN, kD)) { + return false; + } + + return true; +} + +bool DynamixelMotor::GetLED(bool *out_enabled) { + int led_enabled; + + if (!GetMemoryProperty_(DynamixelPropertyType::LED_ENABLE, &led_enabled)) { + return false; + } + + *out_enabled = led_enabled != 0; + + return true; +} + +bool DynamixelMotor::SetLED(bool enabled) { + return SetMemoryProperty_(DynamixelPropertyType::LED_ENABLE, enabled != 0); +} + +bool DynamixelMotor::GetEnabled(bool *out_enabled) { + int torque_enable; + + if (!GetMemoryProperty_(DynamixelPropertyType::TORQUE_ENABLE, + &torque_enable)) { + return false; + } + + *out_enabled = torque_enable != 0; + + return true; +} + +bool DynamixelMotor::SetEnabled(bool enabled) { + return SetMemoryProperty_(DynamixelPropertyType::TORQUE_ENABLE, enabled != 0); + return true; +} + +bool DynamixelMotor::GetMaxSpeed(float *out_speed) { + *out_speed = this->MAX_SPEED; + return true; +} + +bool DynamixelMotor::GetMotorLocation(Location *out_location) { + *out_location = this->location_; + return true; +} \ No newline at end of file diff --git a/src/robot/actuator/dynamixel_motor.h b/src/robot/actuator/dynamixel_motor.h new file mode 100644 index 0000000..7d12ef9 --- /dev/null +++ b/src/robot/actuator/dynamixel_motor.h @@ -0,0 +1,104 @@ +#include "motor.h" +#include + +enum DynamixelPropertyType { + TORQUE_ENABLE = 0, + LED_ENABLE, + P_GAIN, + I_GAIN, + D_GAIN, + GOAL_POSITION, + MOVING_SPEED, + TORQUE_LIMIT, + PRESENT_POSITION, + PRESENT_SPEED, + PRESENT_LOAD, + PRESENT_INPUT_VOLTAGE, + PRESENT_TEMPERATURE, + REGISTERED, + MOVING, + LOCK, + PUNCH, + REALTIME_TICK, + GOAL_ACCELERATION, +}; + +struct DynamixelMemoryLayout { + int torque_enable; + int led_enable; + int d_gain; + int i_gain; + int p_gain; + int goal_position; + int moving_speed; + int torque_limit; + int present_position; + int present_speed; + int present_load; + int present_input_voltage; + int present_temperature; + int registered; + int moving; + int lock; + int punch; + int realtime_tick; + int goal_acceleration; +}; + +struct DynamixelConfig { + DynamixelMemoryLayout offsets; + DynamixelMemoryLayout sizes; +}; + +class DynamixelMotor : IMotor { +private: + DynamixelConfig config_; + bool initialized_; + + bool Initialize_(); + + bool ValidateCommand_(); + + bool ReadBytes_(int offset, int size, int *out_bytes); + + bool WriteBytes_(int offset, int size, int value); + + bool GetMemoryProperty_(DynamixelPropertyType property, int *out_value); + + bool SetMemoryProperty_(DynamixelPropertyType property, int value); + +public: + DynamixelMotor(int id, Location location, bool inverted = false); + + bool GetPosition(int *out_position); + + bool SetPosition(int position); + + bool GetSpeed(float *out_speed); + + bool SetSpeed(float speed); + + bool GetAcceleration(float *out_acceleration); + + bool SetAcceleration(float acceleration); + + bool GetPIDGain(PIDGain *out_pid_gain); + + bool SetPIDGain(PIDGain pid_gain); + + bool GetLED(bool *out_enabled); + + bool SetLED(bool enabled); + + bool GetEnabled(bool *out_enabled); + + bool SetEnabled(bool enabled); + + bool GetMaxSpeed(float *out_speed); + + bool GetMotorLocation(Location *out_location); + + bool RawSpeedToNormalizedSpeed(float speed, float *out_speed); + + bool NormalizedSpeedToRawSpeed(float speed, float *out_speed); +}; \ No newline at end of file diff --git a/src/robot/actuator/dynamixel_motor_test.cpp b/src/robot/actuator/dynamixel_motor_test.cpp new file mode 100644 index 0000000..774a6e9 --- /dev/null +++ b/src/robot/actuator/dynamixel_motor_test.cpp @@ -0,0 +1,245 @@ +#include "dynamixel_motor.h" +#include "gtest/gtest.h" +#include + +namespace { + +const float FLOAT_EQUALITY_TOLERANCE = 1e-4; + +bool GetZeroTestPairs(std::vector> *out_test_pairs) { + *out_test_pairs = { + // Confirm that 0 and 1024 raw speeds map to 0 normalized speed + std::make_pair(0, 0), + std::make_pair(1024, 0), + }; + + return true; +}; + +bool GetNonZeroTestPairs(float max_speed, bool inverted, + std::vector> *out_test_pairs) { + int inverted_multiplier = inverted ? -1 : 1; + + *out_test_pairs = { + // Confirm that speeds between 0 and 1024 exclusive map to + // appropriate normalized speeds. Non-inverted motors expect + // raw speeds less than 1024 to result in negative normalized + // speeds, and inverted motors expect positive speeds. + std::make_pair(100, inverted_multiplier * -(100. / 1023) * max_speed), + std::make_pair(256, inverted_multiplier * -(256. / 1023) * max_speed), + std::make_pair(350, inverted_multiplier * -(350. / 1023) * max_speed), + std::make_pair(512, inverted_multiplier * -(512. / 1023) * max_speed), + std::make_pair(600, inverted_multiplier * -(600. / 1023) * max_speed), + std::make_pair(768, inverted_multiplier * -(768. / 1023) * max_speed), + std::make_pair(1023, inverted_multiplier * -max_speed), + // Also confirm that speeds between 1025 and 2047 inclusive + // map to appropriate normalized speeds. Non-inverted motors + // expect raw speeds greater than 1024 to result in positive + // normalized speeds, and inverted motors expect negative speeds. + std::make_pair(1024 + 100, + inverted_multiplier * (100. / 1023) * max_speed), + std::make_pair(1024 + 256, + inverted_multiplier * (256. / 1023) * max_speed), + std::make_pair(1024 + 350, + inverted_multiplier * (350. / 1023) * max_speed), + std::make_pair(1024 + 512, + inverted_multiplier * (512. / 1023) * max_speed), + std::make_pair(1024 + 600, + inverted_multiplier * (600. / 1023) * max_speed), + std::make_pair(1024 + 768, + inverted_multiplier * (768. / 1023) * max_speed), + std::make_pair(1024 + 1023, inverted_multiplier * max_speed)}; + + return true; +}; + +TEST(DynamixelMotorTest, TestZeroRawSpeedToNormalizedSpeed) { + DynamixelMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + { + // Confirm that 0 and 1024 raw speeds map to 0 normalized speed. + std::vector> test_pairs; + ASSERT_TRUE(GetZeroTestPairs(&test_pairs)); + + for (std::pair test_pair : test_pairs) { + float raw_speed = test_pair.first; + float expected_normalized_speed = test_pair.second; + float actual_normalized_speed; + + ASSERT_TRUE( + motor.RawSpeedToNormalizedSpeed(raw_speed, &actual_normalized_speed)); + ASSERT_EQ(expected_normalized_speed, actual_normalized_speed); + } + } +} + +TEST(DynamixelMotorTest, TestNonZeroRawSpeedToNormalizedSpeed) { + DynamixelMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + float motor_max_speed; + ASSERT_TRUE(motor.GetMaxSpeed(&motor_max_speed)); + + { + // Confirm that various raw speeds correctly map to their + // corresponding normalized speed + std::vector> test_pairs; + ASSERT_TRUE(GetNonZeroTestPairs(motor_max_speed, false, &test_pairs)); + + for (std::pair test_pair : test_pairs) { + float raw_speed = test_pair.first; + float expected_normalized_speed = test_pair.second; + float actual_normalized_speed; + + ASSERT_TRUE( + motor.RawSpeedToNormalizedSpeed(raw_speed, &actual_normalized_speed)); + ASSERT_NEAR(expected_normalized_speed, actual_normalized_speed, + FLOAT_EQUALITY_TOLERANCE); + } + } +} + +TEST(DynamixelMotorTest, TestZeroRawSpeedToNormalizedSpeedWithInvertedMotor) { + DynamixelMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}, true); + + { + // Confirm that 0 and 1024 raw speeds map to 0 normalized speed. + std::vector> test_pairs; + ASSERT_TRUE(GetZeroTestPairs(&test_pairs)); + + for (std::pair test_pair : test_pairs) { + float raw_speed = test_pair.first; + float expected_normalized_speed = test_pair.second; + float actual_normalized_speed; + + ASSERT_TRUE( + motor.RawSpeedToNormalizedSpeed(raw_speed, &actual_normalized_speed)); + ASSERT_EQ(expected_normalized_speed, actual_normalized_speed); + } + } +} + +TEST(DynamixelMotorTest, + TestNonZeroRawSpeedToNormalizedSpeedWithInvertedMotor) { + DynamixelMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}, true); + + float motor_max_speed; + ASSERT_TRUE(motor.GetMaxSpeed(&motor_max_speed)); + + { + // Confirm that various raw speeds correctly map to their + // corresponding normalized speed + std::vector> test_pairs; + ASSERT_TRUE(GetNonZeroTestPairs(motor_max_speed, true, &test_pairs)); + + for (std::pair test_pair : test_pairs) { + float raw_speed = test_pair.first; + float expected_normalized_speed = test_pair.second; + float actual_normalized_speed; + + ASSERT_TRUE( + motor.RawSpeedToNormalizedSpeed(raw_speed, &actual_normalized_speed)); + ASSERT_NEAR(expected_normalized_speed, actual_normalized_speed, + FLOAT_EQUALITY_TOLERANCE); + } + } +} + +TEST(DynamixelMotorTest, TestZeroNormalizedSpeedToRawSpeed) { + DynamixelMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + { + // Confirm that a normalized speed of 0 maps to 0 raw speed. + // We will never send a computed raw speed of 1024 to the + // motor, as there is a one-to-multiple mapping for normalized + // speeds of 0. + std::vector> test_pairs = { + std::make_pair(0, 0), + }; + + for (std::pair test_pair : test_pairs) { + float normalized_speed = test_pair.second; + float expected_raw_speed = test_pair.first; + float actual_raw_speed; + + ASSERT_TRUE( + motor.NormalizedSpeedToRawSpeed(normalized_speed, &actual_raw_speed)); + ASSERT_EQ(expected_raw_speed, actual_raw_speed); + } + } +} + +TEST(DynamixelMotorTest, TestNonZeroNormalizedSpeedToRawSpeed) { + DynamixelMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + float motor_max_speed; + ASSERT_TRUE(motor.GetMaxSpeed(&motor_max_speed)); + + { + // Confirm that various normalized speeds correctly map to their + // corresponding raw speed + std::vector> test_pairs; + ASSERT_TRUE(GetNonZeroTestPairs(motor_max_speed, false, &test_pairs)); + + for (std::pair test_pair : test_pairs) { + float normalized_speed = test_pair.second; + float expected_raw_speed = test_pair.first; + float actual_raw_speed; + + ASSERT_TRUE( + motor.NormalizedSpeedToRawSpeed(normalized_speed, &actual_raw_speed)); + ASSERT_NEAR(expected_raw_speed, actual_raw_speed, + FLOAT_EQUALITY_TOLERANCE); + } + } +} + +TEST(DynamixelMotorTest, TestZeroNormalizedSpeedToRawSpeedWithInvertedMotor) { + DynamixelMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}, true); + + { + // Confirm that a normalized speed of 0 maps to 0 raw speed. + // We will never send a computed raw speed of 1024 to the + // motor, as there is a one-to-multiple mapping for normalized + // speeds of 0. + std::vector> test_pairs = { + std::make_pair(0, 0), + }; + + for (std::pair test_pair : test_pairs) { + float normalized_speed = test_pair.second; + float expected_raw_speed = test_pair.first; + float actual_raw_speed; + + ASSERT_TRUE( + motor.NormalizedSpeedToRawSpeed(normalized_speed, &actual_raw_speed)); + ASSERT_EQ(expected_raw_speed, actual_raw_speed); + } + } +} + +TEST(DynamixelMotorTest, + TestNonZeroNormalizedSpeedToRawSpeedWithInvertedMotor) { + DynamixelMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}, true); + + float motor_max_speed; + ASSERT_TRUE(motor.GetMaxSpeed(&motor_max_speed)); + + { + // Confirm that various normalized speeds correctly map to their + // corresponding raw speed + std::vector> test_pairs; + ASSERT_TRUE(GetNonZeroTestPairs(motor_max_speed, true, &test_pairs)); + + for (std::pair test_pair : test_pairs) { + float normalized_speed = test_pair.second; + float expected_raw_speed = test_pair.first; + float actual_raw_speed; + + ASSERT_TRUE( + motor.NormalizedSpeedToRawSpeed(normalized_speed, &actual_raw_speed)); + ASSERT_NEAR(expected_raw_speed, actual_raw_speed, + FLOAT_EQUALITY_TOLERANCE); + } + } +} +} // namespace diff --git a/src/robot/actuator/mock_motor.cpp b/src/robot/actuator/mock_motor.cpp new file mode 100644 index 0000000..ea3c9ef --- /dev/null +++ b/src/robot/actuator/mock_motor.cpp @@ -0,0 +1,74 @@ +#include "mock_motor.h" + +MockMotor::MockMotor(int id, Location location, bool inverted) + : IMotor(id, location, inverted){}; + +bool MockMotor::GetPosition(int *out_position) { + *out_position = this->position_; + return true; +} + +bool MockMotor::SetPosition(int position) { + this->position_ = position; + return true; +} + +bool MockMotor::GetSpeed(float *out_speed) { + *out_speed = this->speed_; + return true; +} + +bool MockMotor::SetSpeed(float speed) { + this->speed_ = speed; + return true; +} + +bool MockMotor::GetAcceleration(float *out_acceleration) { + *out_acceleration = this->acceleration_; + return true; +} + +bool MockMotor::SetAcceleration(float acceleration) { + this->acceleration_ = acceleration; + return true; +} + +bool MockMotor::GetPIDGain(PIDGain *out_pid_gain) { + *out_pid_gain = this->pid_gain_; + return true; +} + +bool MockMotor::SetPIDGain(PIDGain pid_gain) { + this->pid_gain_ = pid_gain; + return true; +} + +bool MockMotor::GetLED(bool *out_enabled) { + *out_enabled = this->led_enable_; + return true; +} + +bool MockMotor::SetLED(bool enabled) { + this->led_enable_ = enabled; + return true; +} + +bool MockMotor::GetEnabled(bool *out_enabled) { + *out_enabled = this->motor_enable_; + return true; +} + +bool MockMotor::SetEnabled(bool enabled) { + this->motor_enable_ = enabled; + return true; +} + +bool MockMotor::GetMaxSpeed(float *out_speed) { + *out_speed = this->MAX_SPEED; + return true; +} + +bool MockMotor::GetMotorLocation(Location *out_location) { + *out_location = this->location_; + return true; +} \ No newline at end of file diff --git a/src/robot/actuator/mock_motor.h b/src/robot/actuator/mock_motor.h new file mode 100644 index 0000000..35b92cd --- /dev/null +++ b/src/robot/actuator/mock_motor.h @@ -0,0 +1,50 @@ +#include "motor.h" +#include + +class MockMotor : IMotor { +private: + /* + Since we're mocking a motor, we don't have a hardware input + that provides us values for location and status. We mock those + out to reasonable values here. + + Right now, I don't simulate motion because that's hard. + */ + int position_ = 0; + float speed_ = 0; + float acceleration_ = 0; + PIDGain pid_gain_ = {0, 0, 0}; + bool led_enable_ = false; + bool motor_enable_ = false; + +public: + MockMotor(int id, Location location, bool inverted = false); + + bool GetPosition(int *out_position); + + bool SetPosition(int position); + + bool GetSpeed(float *out_speed); + + bool SetSpeed(float speed); + + bool GetAcceleration(float *out_acceleration); + + bool SetAcceleration(float acceleration); + + bool GetPIDGain(PIDGain *out_pid_gain); + + bool SetPIDGain(PIDGain pid_gain); + + bool GetLED(bool *out_enabled); + + bool SetLED(bool enabled); + + bool GetEnabled(bool *out_enabled); + + bool SetEnabled(bool enabled); + + bool GetMaxSpeed(float *out_speed); + + bool GetMotorLocation(Location *out_location); +}; \ No newline at end of file diff --git a/src/robot/actuator/mock_motor_test.cpp b/src/robot/actuator/mock_motor_test.cpp new file mode 100644 index 0000000..47f69f0 --- /dev/null +++ b/src/robot/actuator/mock_motor_test.cpp @@ -0,0 +1,115 @@ +#include "mock_motor.h" +#include "gtest/gtest.h" +#include + +namespace { +TEST(MockMotorTest, TestPositionConfiguration) { + /// Confirm we can write and read position values. + MockMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + // Set and read back position values + EXPECT_TRUE(motor.SetPosition(10)); + + int out; + EXPECT_TRUE(motor.GetPosition(&out)); + + EXPECT_EQ(out, 10); +} + +TEST(MockMotorTest, TestSpeedConfiguration) { + /// Confirm we can write and read speed values. + MockMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + // Set and read back speed values + EXPECT_TRUE(motor.SetSpeed(20)); + + float out; + EXPECT_TRUE(motor.GetSpeed(&out)); + + EXPECT_EQ(out, 20); +} + +TEST(MockMotorTest, TestAccelerationConfiguration) { + /// Confirm we can write and read acceleration values. + MockMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + // Set and read back acceleration values + EXPECT_TRUE(motor.SetAcceleration(30)); + + float out; + EXPECT_TRUE(motor.GetAcceleration(&out)); + + EXPECT_EQ(out, 30); +} + +TEST(MockMotorTest, TestPIDGainConfiguration) { + /// Confirm we can write and read PID gain values. + MockMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + // Set and read back PID gain values + EXPECT_TRUE(motor.SetPIDGain({1, 2, 3})); + + PIDGain out; + EXPECT_TRUE(motor.GetPIDGain(&out)); + + EXPECT_EQ(out.kP, 1); + EXPECT_EQ(out.kI, 2); + EXPECT_EQ(out.kD, 3); +} + +TEST(MockMotorTest, TestLEDConfiguration) { + /// Confirm we can write and read LED configuration values. + MockMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + { + bool out; + EXPECT_TRUE(motor.GetLED(&out)); + + EXPECT_FALSE(out); + } + + // Set and read back LED configuration values + EXPECT_TRUE(motor.SetLED(true)); + + { + bool out; + EXPECT_TRUE(motor.GetLED(&out)); + + EXPECT_TRUE(out); + } +} + +TEST(MockMotorTest, TestMotorConfiguration) { + /// Confirm we can write and read motor configuration values. + MockMotor motor(0, {Eigen::Vector2d(0, 0), Eigen::Rotation2Df(0)}); + + { + bool out; + EXPECT_TRUE(motor.GetEnabled(&out)); + + EXPECT_FALSE(out); + } + + // Set and read back motor configuration values + EXPECT_TRUE(motor.SetEnabled(true)); + + { + bool out; + EXPECT_TRUE(motor.GetEnabled(&out)); + + EXPECT_TRUE(out); + } +} + +TEST(MockMotorTest, TestLocationConfiguration) { + /// Confirm we can write and read motor configuration values. + MockMotor motor(0, {Eigen::Vector2d(1, 2), Eigen::Rotation2Df(M_PI)}); + + Location out; + EXPECT_TRUE(motor.GetMotorLocation(&out)); + + EXPECT_EQ(out.position.x(), 1); + EXPECT_EQ(out.position.y(), 2); + EXPECT_NEAR(out.rotation.angle(), M_PI, 1e-5); +} +} // namespace diff --git a/src/robot/actuator/motor.h b/src/robot/actuator/motor.h new file mode 100644 index 0000000..67056ee --- /dev/null +++ b/src/robot/actuator/motor.h @@ -0,0 +1,30 @@ +#include "src/math/pid.h" +#include "src/math/vector_math.h" +#include + +class IMotor { +protected: + int id_; + Location location_; + bool inverted_; + + const float MAX_SPEED = 100.; + +public: + IMotor(int id, Location location, bool inverted) + : id_(id), location_(location), inverted_(inverted){}; + virtual bool GetPosition(int *out_position) = 0; + virtual bool SetPosition(int position) = 0; + virtual bool GetSpeed(float *out_speed) = 0; + virtual bool SetSpeed(float speed) = 0; + virtual bool GetAcceleration(float *out_acceleration) = 0; + virtual bool SetAcceleration(float acceleration) = 0; + virtual bool GetPIDGain(PIDGain *out_pid_gain) = 0; + virtual bool SetPIDGain(PIDGain pid_gain) = 0; + virtual bool GetLED(bool *out_enabled) = 0; + virtual bool SetLED(bool enabled) = 0; + virtual bool GetEnabled(bool *out_enabled) = 0; + virtual bool SetEnabled(bool enabled) = 0; + virtual bool GetMaxSpeed(float *out_speed) = 0; + virtual bool GetMotorLocation(Location *out_location) = 0; +}; \ No newline at end of file diff --git a/src/robot/dynamixel_motor.cpp b/src/robot/dynamixel_motor.cpp deleted file mode 100644 index 54bb080..0000000 --- a/src/robot/dynamixel_motor.cpp +++ /dev/null @@ -1,50 +0,0 @@ -#include "dynamixel_motor.h" - -DynamixelMotor::DynamixelMotor(int id, Eigen::Vector2d position, bool inverted) - : id_(id), position_(position), inverted_(inverted) { - DynamixelMemoryLayout offsets = { - 24, // torque_enable - 25, // led_enable - 26, // d_gain - 27, // i_gain - 28, // p_gain - 30, // goal_position - 32, // moving_speed - 34, // torque_limit - 36, // present_position - 38, // present_speed - 40, // present_load - 42, // present_input_voltage - 43, // present_temperature - 44, // registered - 46, // moving - 47, // lock - 48, // punch - 50, // realtime_tick - 73, // goal_acceleration - }; - - DynamixelMemoryLayout sizes = { - 1, // torque_enable - 1, // led_enable - 1, // d_gain - 1, // i_gain - 1, // p_gain - 2, // goal_position - 2, // moving_speed - 2, // torque_limit - 2, // present_position - 2, // present_speed - 2, // present_load - 1, // present_input_voltage - 1, // present_temperature - 1, // registered - 1, // moving - 1, // lock - 2, // punch - 2, // realtime_tick - 1, // goal_acceleration - }; - - config_ = {offsets, sizes}; -}; diff --git a/src/robot/dynamixel_motor.h b/src/robot/dynamixel_motor.h deleted file mode 100644 index f1f0a10..0000000 --- a/src/robot/dynamixel_motor.h +++ /dev/null @@ -1,81 +0,0 @@ -#include "motor.h" -#include -#include - -struct DynamixelMemoryLayout { - int torque_enable; - int led_enable; - int d_gain; - int i_gain; - int p_gain; - int goal_position; - int moving_speed; - int torque_limit; - int present_position; - int present_speed; - int present_load; - int present_input_voltage; - int present_temperature; - int registered; - int moving; - int lock; - int punch; - int realtime_tick; - int goal_acceleration; -}; - -struct DynamixelConfig { - DynamixelMemoryLayout offsets; - DynamixelMemoryLayout sizes; -}; - -class DynamixelMotor : IMotor { -private: - int id_; - Eigen::Vector2d position_; - bool inverted_; - - DynamixelConfig config_; - bool initialized_; - - bool ValidateCommand_(); - - bool WriteBytes_(int offset, int size, int value); - - int ReadBytes_(int offset, int size); - - int GetMemoryProperty_(std::string property); - - void SetMemoryProperty_(std::string property, int value); - -public: - DynamixelMotor(int id, Eigen::Vector2d position, bool inverted = false); - - bool Initialize(); - - bool Configure(); - - int GetSpeed(); - - void SetSpeed(int speed); - - int GetPosition(); - - void SetPosition(int position); - - int GetAcceleration(); - - void SetAcceleration(int acceleration); - - PIDGain GetPIDGain(); - - void SetPIDGain(PIDGain pid_gain); - - bool GetLED(); - - void SetLED(bool enabled); - - bool GetEnabled(); - - void SetEnabled(bool enabled); -}; \ No newline at end of file diff --git a/src/robot/main.cpp b/src/robot/main.cpp new file mode 100644 index 0000000..c3faada --- /dev/null +++ b/src/robot/main.cpp @@ -0,0 +1,13 @@ +#include +#include + +using Eigen::MatrixXd; + +int main() { + MatrixXd m(2, 2); + m(0, 0) = 3; + m(1, 0) = 2.5; + m(0, 1) = -1; + m(1, 1) = m(1, 0) + m(0, 1); + std::cout << m << std::endl; +} \ No newline at end of file diff --git a/src/robot/motor.h b/src/robot/motor.h deleted file mode 100644 index 4b791ad..0000000 --- a/src/robot/motor.h +++ /dev/null @@ -1,20 +0,0 @@ -#include "pid.h" - -class IMotor { -public: - IMotor(){}; - virtual bool Initialize() = 0; - virtual bool Configure() = 0; - virtual int GetSpeed() = 0; - virtual void SetSpeed(int speed) = 0; - virtual int GetPosition() = 0; - virtual void SetPosition(int position) = 0; - virtual int GetAcceleration() = 0; - virtual void SetAcceleration(int acceleration) = 0; - virtual PIDGain GetPIDGain() = 0; - virtual void SetPIDGain(PIDGain pid_gain) = 0; - virtual bool GetLED() = 0; - virtual void SetLED(bool enabled) = 0; - virtual bool GetEnabled() = 0; - virtual void SetEnabled(bool enabled) = 0; -}; \ No newline at end of file diff --git a/src/robot/pid.h b/src/robot/pid.h deleted file mode 100644 index ea0cad0..0000000 --- a/src/robot/pid.h +++ /dev/null @@ -1,5 +0,0 @@ -struct PIDGain { - int kP; - int kI; - int kD; -}; \ No newline at end of file diff --git a/src/robot/robot.cpp b/src/robot/robot.cpp index c3faada..ed1d22c 100644 --- a/src/robot/robot.cpp +++ b/src/robot/robot.cpp @@ -1,13 +1,29 @@ -#include -#include - -using Eigen::MatrixXd; - -int main() { - MatrixXd m(2, 2); - m(0, 0) = 3; - m(1, 0) = 2.5; - m(0, 1) = -1; - m(1, 1) = m(1, 0) + m(0, 1); - std::cout << m << std::endl; +#include "robot.h" + +Robot::Robot(std::vector motor_list) + : motor_list_(motor_list) {} + +bool Robot::SetSpeed(Eigen::Vector2d desired_speed, + Eigen::Rotation2Df rotation) { + // We are provided a unit vector for the direction and speed, + // and rotation to perform at the same time. We need to perform + // the kinematics to determine what the motor speeds for each motor + // need to be + for (auto motor : motor_list_) { + // Get locations for each motor + Location motor_location; + if (!motor.GetMotorLocation(&motor_location)) { + return false; + } + + // Take the dot product between where we want to go (desired_speed) + // and the vector between the center of the robot and the wheel + float dot_prod = motor_location.position.dot(desired_speed); + + // We then scale our desired speed vector, and rotate it to be + // parallel with the wheel + Eigen::Vector2d scaled_speed = desired_speed * dot_prod; + } + + return true; } \ No newline at end of file diff --git a/src/robot/robot.h b/src/robot/robot.h index b06b06b..1e9b42e 100644 --- a/src/robot/robot.h +++ b/src/robot/robot.h @@ -1,4 +1,12 @@ -#include -#include +#include "src/robot/actuator/dynamixel_motor.h" +#include -class \ No newline at end of file +class Robot { +private: + std::vector motor_list_; + +public: + Robot(std::vector motor_list); + + bool SetSpeed(Eigen::Vector2d unit_speed, Eigen::Rotation2Df rotation); +}; \ No newline at end of file