diff --git a/01_Libraries/ghost_control/CMakeLists.txt b/01_Libraries/ghost_control/CMakeLists.txt index 1c7bd709..99b35405 100644 --- a/01_Libraries/ghost_control/CMakeLists.txt +++ b/01_Libraries/ghost_control/CMakeLists.txt @@ -68,6 +68,30 @@ install( INCLUDES DESTINATION include ) +# Current Limiting +add_library(v5_current_limiting SHARED src/models/v5_current_limiting.cpp) +ament_target_dependencies(v5_current_limiting +${DEPENDENCIES} +) +target_include_directories(v5_current_limiting +PUBLIC +$ +$) +ament_export_targets(v5_current_limiting HAS_LIBRARY_TARGET) +install( + TARGETS v5_current_limiting + EXPORT v5_current_limiting + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +add_executable(current_limit_sandbox test/current_limit_sandbox.cpp) +target_link_libraries(current_limit_sandbox + v5_current_limiting +) + ################# #### Install #### ################# @@ -85,4 +109,10 @@ target_link_libraries(test_dc_motor_model gtest ) +ament_add_gtest(test_v5_current_limiting test/test_v5_current_limiting.cpp) +target_link_libraries(test_v5_current_limiting + v5_current_limiting + gtest_main +) + ament_package() diff --git a/01_Libraries/ghost_control/include/ghost_control/models/v5_current_limiting.hpp b/01_Libraries/ghost_control/include/ghost_control/models/v5_current_limiting.hpp new file mode 100644 index 00000000..efba78bb --- /dev/null +++ b/01_Libraries/ghost_control/include/ghost_control/models/v5_current_limiting.hpp @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2025 Maxx Wilson + * All rights reserved. + + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once +#include +#include +#include +#include + +namespace ghost_control +{ + +namespace v5_current_limiting +{ +constexpr double MAX_CURRENT = 12800.0; + +double convertMotorToBatteryCurrent(double current); + +double convertBatteryToMotorCurrent(double current); + +/** + * @brief This replicates the current limiting logic implemented in VEX OS, as described on this page: https://wiki.purduesigbots.com/vex-electronics/vex-electronics/motors. + * + * This is a weird black-box function, and is not user friendly at all, so we include other functions in this module to assist. + * This is intended just to replicate the existing logic upstream. + * + * + * Observations/Notes: + * If there are less than 9 motors plugged in, they all get the full 2500mA. + * If there are nine or more, the limits decrease. + * If there are 8 motors, and N additional motors with current limits set to zero, THEY STILL DECREASE, albiet by a very small amount. + * If there are nine or more motors plugged in, and a subset are limited to a value higher thatn the nominal decreased limit, then they are throttled to the adjusted limit. + * + * @param active_current_limits_ma Vector containing any active current limits + * @param num_motors Number of motors plugged in to the brain + * @return std::vector vector of all current limits, where first N are equal to the active current limits passed in (IF the active limits are less than the adjusted current limit) + */ +std::vector calculateAllCurrentLimits(std::vector active_current_limits_ma, int num_motors); + +/** + * @brief Given N motors plugged into the brain, and M current limits in milliAmps, this method returns what value we should limit all other motors to such that VEX OS will not throttle + * the desired/"active" motors. + * + * Issue: + * We have 20 motors plugged in. The following is completely ignored, because no motor can exceed the adjusted current limit without first lowering other motor values. + * ``` + * motor_1.setCurrentLimit(2500.0); + * ``` + * + * Solution: + * ``` + * + * lim = getRemainingCurrentDistributed(std::vector(2500.0), 20); + * motor2.setCurrentLimit(lim); + * ... + * motorN.setCurrentLimit(lim); + * motor1.setCurrentLimit(2500.0); + * ``` + * + * Now all motors are properly throttled such that motor 1 can run at full power. + * If some motors are disabled, they can also be added to the input vector to increase the calculated value. + * + * @param active_current_limits_ma Vector containing any active current limits (either low or high) + * @param num_motors Number of motors plugged in to the brain + * @return double Single value which can be set for all other motors to avoid throttling active current limits + */ +double getRemainingCurrentDistributed(std::vector active_current_limits_ma, int num_motors); + +} // namespace v5_current_limiting +} // namespace ghost_control diff --git a/01_Libraries/ghost_control/src/models/v5_current_limiting.cpp b/01_Libraries/ghost_control/src/models/v5_current_limiting.cpp new file mode 100644 index 00000000..8b4c8e71 --- /dev/null +++ b/01_Libraries/ghost_control/src/models/v5_current_limiting.cpp @@ -0,0 +1,134 @@ +/* + * Copyright (c) 2025 Maxx Wilson + * All rights reserved. + + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ +#include +#include + +namespace ghost_control +{ + +namespace v5_current_limiting +{ + +double convertMotorToBatteryCurrent(double current) +{ + return exp(((current / 1000) + 4.324) / 0.925); +} + +double convertBatteryToMotorCurrent(double current) +{ + return (0.925 * log(current) - 4.324) * 1000; +} + +std::vector calculateAllCurrentLimits(std::vector active_current_limits_ma, int num_motors) +{ + double default_current_limit_milliamps = convertBatteryToMotorCurrent(MAX_CURRENT / num_motors); + default_current_limit_milliamps = ghost_util::clamp(default_current_limit_milliamps, 0.0, 2500.0); + + // Sum Current Limits + double limited_sum = 0.0; + int num_limited = 0; + for (const auto & lim_milliamps : active_current_limits_ma) { + if (lim_milliamps < default_current_limit_milliamps) { + limited_sum += convertMotorToBatteryCurrent(lim_milliamps); + num_limited++; + } + } + + int num_unlimited = num_motors - num_limited; + double battery_current_per_motor = (MAX_CURRENT - limited_sum) / num_unlimited; + + double adjusted_current_limit = convertBatteryToMotorCurrent(battery_current_per_motor); + adjusted_current_limit = ghost_util::clamp(adjusted_current_limit, 0.0, 2500.0); + + std::vector final_current_limits(num_motors, adjusted_current_limit); + + for (int i = 0; i < active_current_limits_ma.size(); i++) { + if (active_current_limits_ma[i] < adjusted_current_limit) { + final_current_limits[i] = active_current_limits_ma[i]; + } + } + return final_current_limits; +} + +double getRemainingCurrentDistributed(std::vector active_current_limits_ma, int num_motors) +{ + // Calculate the naive default limit + double default_current_limit_milliamps = convertBatteryToMotorCurrent(MAX_CURRENT / num_motors); + default_current_limit_milliamps = ghost_util::clamp(default_current_limit_milliamps, 0.0, 2500.0); + + int num_distributed = num_motors - active_current_limits_ma.size(); + + // Gather necessary metrics on the active current limits + int num_over = 0; + double max_over_current = 0.0; + std::vector under_currents; + for (const auto & lim : active_current_limits_ma) { + if (lim < default_current_limit_milliamps) { + under_currents.push_back(lim); + } else { + num_over++; + max_over_current = std::max(max_over_current, lim); + } + } + + // Get the battery current per unregulated motor + // Note: motors are divided based on whether they exceed the naive threshold or not, so if the threshold is 1500, then there is + // no difference at the motor if we were to request 2000mA or 2500mA. This is why we take the max instead of an average. + auto battery_current_per_motor = convertMotorToBatteryCurrent(max_over_current); + + // Invert the distributing operation and determine what all the limited motors need to add up to. + auto lim_current_sum = MAX_CURRENT - battery_current_per_motor * num_over; + + // Remove user-specified under currents individually after transforming through exponential to battery. + for (const auto & current : under_currents) { + lim_current_sum -= convertMotorToBatteryCurrent(current); + } + + // Finally, we have the sum of all the unspecified limited motors. + // We can divide to get the individual currents at the battery, and then convert to the requested motor limits. + lim_current_sum /= num_distributed; + auto distributed_current_limit = convertBatteryToMotorCurrent(lim_current_sum); + + // Clamp to max current + distributed_current_limit = std::min(2500.0, distributed_current_limit); + + // This is a patch for the corner case where we call this without any over current motors (which is non-sensical). + // Regardless, in that case, we want to return the correct answer, which ignores the default current limiting below. + if(num_over == 0){ + return distributed_current_limit; + } + + // For the individual limits to be valid, they must be below the nominal limit, otherwise they won't trigger the algorithm. + // We drop 1mA to satisfy the strict inequality. + distributed_current_limit = std::min(default_current_limit_milliamps-1, distributed_current_limit); + + // If we ended up with 1mA below 2500.0, we aren't actually doing any current limiting, so just return full power. + if(std::fabs(distributed_current_limit - 2499.0) < std::numeric_limits::epsilon()){ + return 2500.0; + } + + return distributed_current_limit; +} + +} // namespace v5_current_limiting +} // namespace ghost_control diff --git a/01_Libraries/ghost_control/test/current_limit_sandbox.cpp b/01_Libraries/ghost_control/test/current_limit_sandbox.cpp new file mode 100644 index 00000000..179c522a --- /dev/null +++ b/01_Libraries/ghost_control/test/current_limit_sandbox.cpp @@ -0,0 +1,50 @@ +/* + * Copyright (c) 2025 Maxx Wilson + * All rights reserved. + + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +using namespace ghost_control::v5_current_limiting; +#include + +int main(int argc, char * argv[]) +{ + int num_motors = 9; + std::vector active_current_limits{0.0}; + auto throttled = getRemainingCurrentDistributed(active_current_limits, num_motors); + std::cout << "Remaining Unthrottled: " << throttled << std::endl; + + std::vector custom_current_limits; + custom_current_limits.insert(custom_current_limits.end(), active_current_limits.begin(), active_current_limits.end()); + for (int i = 0; i < num_motors - active_current_limits.size(); i++) { + custom_current_limits.push_back(throttled); + } + auto result = calculateAllCurrentLimits(custom_current_limits, num_motors); + + + double sum = 0.0; + for (const auto & lim : result) { + sum += lim; + std::cout << lim << std::endl; + } + std::cout << "Sum: " << sum << std::endl; +} diff --git a/01_Libraries/ghost_control/test/test_v5_current_limiting.cpp b/01_Libraries/ghost_control/test/test_v5_current_limiting.cpp new file mode 100644 index 00000000..d6cafcda --- /dev/null +++ b/01_Libraries/ghost_control/test/test_v5_current_limiting.cpp @@ -0,0 +1,238 @@ +/* + * Copyright (c) 2025 Maxx Wilson + * All rights reserved. + + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "gtest/gtest.h" + +using namespace ghost_control::v5_current_limiting; + +TEST(testV5CurrentLimiting, testLessThanEightMotorsAreFullPower) { + std::vector active_current_limits; + for (int i = 1; i < 9; i++) { + auto result = calculateAllCurrentLimits(active_current_limits, i); + EXPECT_EQ(result.size(), i); + for (const auto & lim : result) { + EXPECT_FLOAT_EQ(lim, 2500.0); + } + } +} + +TEST(testV5CurrentLimiting, testMoreThanEightMotorsAreLimitedEqually) { + std::vector active_current_limits; + for (int i = 9; i < 21; i++) { + auto result = calculateAllCurrentLimits(active_current_limits, i); + EXPECT_EQ(result.size(), i); + for (const auto & lim : result) { + EXPECT_FLOAT_EQ(lim, convertBatteryToMotorCurrent(MAX_CURRENT / i)); + } + } +} + +TEST(testV5CurrentLimiting, testMotorsLimitedToZero) { + // Sweep all possible numbers of motors + for (int num_motors = 1; num_motors < 21; num_motors++) { + // For each number of motors, limit a subset + for (int num_limited = 1; num_limited < num_motors; num_limited++) { + double num_unlimited = num_motors - num_limited; + + std::vector active_current_limits(num_limited, 0.0); + auto result = calculateAllCurrentLimits(active_current_limits, num_motors); + + for (int k = 0; k < result.size(); k++) { + if (k < num_limited) { + // Limited Subset are zero + EXPECT_EQ(result[k], 0.0); + } else { + // So this part is odd. A motor that is plugged in but set to zero still lowers the overall current capacity because of the log/exp transform used... + double limited_battery_current = num_limited * convertMotorToBatteryCurrent(0.0); + double adjusted_current_limit = convertBatteryToMotorCurrent((MAX_CURRENT - limited_battery_current) / (num_unlimited)); + + // Cap at 2.5A + adjusted_current_limit = std::min(2500.0, adjusted_current_limit); + EXPECT_FLOAT_EQ(result[k], adjusted_current_limit); + } + } + } + } +} + +TEST(testV5CurrentLimiting, testSubsetOfMotorsLimitedToNonZeroValue) { + int num_motors = 16; + int num_limited = 2; + double current_limit = 250.0; + std::vector active_current_limits(num_limited, current_limit); + auto result = calculateAllCurrentLimits(active_current_limits, num_motors); + + for (int i = 0; i < num_motors; i++) { + if (i < num_limited) { + EXPECT_FLOAT_EQ(result[i], current_limit); + } else { + EXPECT_FLOAT_EQ(result[i], 1962.257); + } + } +} + +TEST(testV5CurrentLimiting, testCurrentLimitsOverRegulatedAreIgnored) { + int num_motors = 16; + int num_limited = 2; + double current_limit = 1870; + std::vector active_current_limits(num_limited, current_limit); + auto result = calculateAllCurrentLimits(active_current_limits, num_motors); + + for (int i = 0; i < num_motors; i++) { + EXPECT_FLOAT_EQ(result[i], 1859.266); + } +} + +TEST(testV5CurrentLimiting, testgetRemainingCurrentDistributed) { + int num_motors = 16; + std::vector active_current_limits{2500.0, 2500.0}; + EXPECT_FLOAT_EQ(1716.8187, getRemainingCurrentDistributed(active_current_limits, num_motors)); +} + +TEST(testV5CurrentLimiting, testgetRemainingCurrentDistributedUneven) { + int num_motors = 20; + std::vector active_current_limits{1450.0, 118.0, 2485.0}; + EXPECT_FLOAT_EQ(1627.9312, getRemainingCurrentDistributed(active_current_limits, num_motors)); +} + +TEST(testV5CurrentLimiting, testBelowCurrentLimits) { + int num_motors = 6; + std::vector active_current_limits{2500.0}; + EXPECT_FLOAT_EQ(2500.0, getRemainingCurrentDistributed(active_current_limits, num_motors)); +} + +TEST(testV5CurrentLimiting, testAtCurrentLimits) { + int num_motors = 8; + std::vector active_current_limits{2500.0}; + EXPECT_FLOAT_EQ(2500.0, getRemainingCurrentDistributed(active_current_limits, num_motors)); +} + +TEST(testV5CurrentLimiting, testBelowCurrentLimitsZeroed) { + int num_motors = 6; + std::vector active_current_limits{0.0}; + EXPECT_FLOAT_EQ(2500.0, getRemainingCurrentDistributed(active_current_limits, num_motors)); +} + +TEST(testV5CurrentLimiting, testAtCurrentLimitsZeroed) { + int num_motors = 8; + std::vector active_current_limits{0.0}; + EXPECT_FLOAT_EQ(2500.0, getRemainingCurrentDistributed(active_current_limits, num_motors)); +} + +TEST(testV5CurrentLimiting, testAboveCurrentLimitZeroed) { + int num_motors = 9; + std::vector active_current_limits{0.0}; + EXPECT_FLOAT_EQ(2492.6480, getRemainingCurrentDistributed(active_current_limits, num_motors)); +} + +TEST(testV5CurrentLimiting, testAboveCurrentLimitZeroed2) { + int num_motors = 14; + std::vector active_current_limits{0.0}; + EXPECT_FLOAT_EQ(2043.554, getRemainingCurrentDistributed(active_current_limits, num_motors)); +} + +TEST(testV5CurrentLimiting, testCurrentLimitInversion1) { + int num_motors = 20; + std::vector active_current_limits{2500.0, 2250.0, 0.0, 0.0}; + auto throttled_current = getRemainingCurrentDistributed(active_current_limits, num_motors); + + std::vector throttled_current_limits; + throttled_current_limits.insert(throttled_current_limits.end(), active_current_limits.begin(), active_current_limits.end()); + for (int i = throttled_current_limits.size(); i < num_motors; i++) { + throttled_current_limits.push_back(throttled_current); + } + auto result = calculateAllCurrentLimits(throttled_current_limits, num_motors); + + for (int i = 0; i < num_motors; i++) { + if (i < active_current_limits.size()) { + EXPECT_FLOAT_EQ(result[i], active_current_limits[i]); + } else { + EXPECT_FLOAT_EQ(result[i], throttled_current); + } + } +} + +TEST(testV5CurrentLimiting, testCurrentLimitInversion2) { + int num_motors = 15; + std::vector active_current_limits{2500.0, 2004.0, 100.0, 15.0, 0.0, 2450.0}; + auto throttled_current = getRemainingCurrentDistributed(active_current_limits, num_motors); + + std::vector throttled_current_limits; + throttled_current_limits.insert(throttled_current_limits.end(), active_current_limits.begin(), active_current_limits.end()); + for (int i = throttled_current_limits.size(); i < num_motors; i++) { + throttled_current_limits.push_back(throttled_current); + } + auto result = calculateAllCurrentLimits(throttled_current_limits, num_motors); + + for (int i = 0; i < num_motors; i++) { + if (i < active_current_limits.size()) { + EXPECT_FLOAT_EQ(result[i], active_current_limits[i]); + } else { + EXPECT_FLOAT_EQ(result[i], throttled_current); + } + } +} + +TEST(testV5CurrentLimiting, testCurrentLimitInversion3) { + int num_motors = 16; + std::vector active_current_limits{2500.0, 0.0, 0.0, 0.0}; + auto throttled_current = getRemainingCurrentDistributed(active_current_limits, num_motors); + + std::vector throttled_current_limits; + throttled_current_limits.insert(throttled_current_limits.end(), active_current_limits.begin(), active_current_limits.end()); + for (int i = throttled_current_limits.size(); i < num_motors; i++) { + throttled_current_limits.push_back(throttled_current); + } + auto result = calculateAllCurrentLimits(throttled_current_limits, num_motors); + + for (int i = 0; i < num_motors; i++) { + if (i < active_current_limits.size()) { + EXPECT_FLOAT_EQ(result[i], active_current_limits[i]); + } else { + EXPECT_FLOAT_EQ(result[i], convertBatteryToMotorCurrent(MAX_CURRENT / num_motors) - 1); + } + } +} + +TEST(testV5CurrentLimiting, testCurrentLimitInversionNoOverMotors) { + int num_motors = 16; + std::vector active_current_limits{0.0, 0.0, 0.0, 0.0}; + auto throttled_current = getRemainingCurrentDistributed(active_current_limits, num_motors); + + std::vector throttled_current_limits; + throttled_current_limits.insert(throttled_current_limits.end(), active_current_limits.begin(), active_current_limits.end()); + for (int i = throttled_current_limits.size(); i < num_motors; i++) { + throttled_current_limits.push_back(throttled_current); + } + auto result = calculateAllCurrentLimits(throttled_current_limits, num_motors); + + for (int i = 0; i < num_motors; i++) { + if (i < active_current_limits.size()) { + EXPECT_FLOAT_EQ(result[i], active_current_limits[i]); + } else { + EXPECT_FLOAT_EQ(result[i], 2093.857); // Grabbed from Purdue Wiki + } + } +} \ No newline at end of file diff --git a/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp b/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp index fdff4981..d0c51609 100644 --- a/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp +++ b/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp @@ -90,6 +90,7 @@ void V5SerialNode::updateActuatorCommands(std::vector & buffer) { hardware_interface_ptr_->deserialize(buffer); + std::vector> current_limits; for (const auto & name : *hardware_interface_ptr_) { auto device_data_ptr = hardware_interface_ptr_->getDeviceData(name); auto device_config_ptr = hardware_interface_ptr_->getDeviceConfig(name); @@ -98,7 +99,8 @@ void V5SerialNode::updateActuatorCommands(std::vector & buffer) case device_type_e::MOTOR: { auto motor_device_data_ptr = device_data_ptr->as(); - v5_globals::motor_interfaces.at(name)->setCurrentLimit(motor_device_data_ptr->current_limit); + v5_globals::motor_interfaces.at(name)->setCurrentLimit(0); + current_limits.push_back(std::pair(name, motor_device_data_ptr->current_limit)); v5_globals::motor_interfaces.at(name)->setMotorCommand( motor_device_data_ptr->position_command, @@ -160,6 +162,20 @@ void V5SerialNode::updateActuatorCommands(std::vector & buffer) break; } } + + // Sort current limits smallest to largest + std::sort( + current_limits.begin(), + current_limits.end(), + [](std::pair a, std::pair b) { + return a.second > b.second; + }); + + // Set current limits from smallest to largest to avoid built-in current regulation overriding requested values + for (int i = 0; i < current_limits.size(); i++) { + v5_globals::motor_interfaces.at(current_limits[i].first)->setCurrentLimit(current_limits[i].second); + } + } void V5SerialNode::writeV5StateUpdate() diff --git a/11_Robots/ghost_high_stakes/config/robot_hardware_config_tank.yaml b/11_Robots/ghost_high_stakes/config/robot_hardware_config_tank.yaml index c975d2da..9dc3b1d0 100644 --- a/11_Robots/ghost_high_stakes/config/robot_hardware_config_tank.yaml +++ b/11_Robots/ghost_high_stakes/config/robot_hardware_config_tank.yaml @@ -15,16 +15,16 @@ port_configuration: type: MOTOR reversed: false config: intake_motor_config - neutral_stake_motor_l: + neutral_stake_r: port: 9 type: MOTOR - reversed: false - config: intake_motor_config - neutral_stake_motor_r: + reversed: true + config: neutral_stake_motor_config + neutral_stake_l: port: 10 type: MOTOR - reversed: true - config: intake_motor_config + reversed: false + config: neutral_stake_motor_config imu: port: 14 type: INERTIAL_SENSOR @@ -131,6 +131,19 @@ port_configuration: ff_vel_gain: 0.0 ff_torque_gain: 0.0 serial: - send_position_command: true + send_position_command: false send_velocity_command: false - send_voltage_command: true \ No newline at end of file + send_voltage_command: true + neutral_stake_motor_config: + gearset: 200 + brake_mode: BRAKE + encoder_units: DEGREES + controller: + pos_gain: 0.0 + vel_gain: 0.0 + ff_vel_gain: 0.0 + ff_torque_gain: 0.0 + serial: + send_position_command: false + send_velocity_command: false + send_voltage_command: true diff --git a/11_Robots/ghost_high_stakes/config/ros_config.yaml b/11_Robots/ghost_high_stakes/config/ros_config.yaml index 12390bf4..c96ef3b5 100644 --- a/11_Robots/ghost_high_stakes/config/ros_config.yaml +++ b/11_Robots/ghost_high_stakes/config/ros_config.yaml @@ -69,6 +69,15 @@ competition_state_machine_node: conveyor_hook_throw_fraction: 0.5 conveyor_hook_throw_duration: 0.25 + neutral_stake_arm_kp: 0.04 + neutral_stake_arm_gear_ratio: 3.0 + neutral_stake_arm_rest_pos_deg: 0.0 + neutral_stake_arm_loading_pos_deg: 25.0 + neutral_stake_arm_loaded_pos_deg: 70.0 + neutral_stake_arm_score_neutral_pos_deg: 110.0 + neutral_stake_arm_score_alliance_pos_deg: 170.0 + neutral_stake_arm_down_pos_deg: 220.0 + # motion planning move_to_pose_kp_xy: 2.2 move_to_pose_kd_xy: 0.2 diff --git a/11_Robots/ghost_tank/include/ghost_tank/tank_robot_plugin.hpp b/11_Robots/ghost_tank/include/ghost_tank/tank_robot_plugin.hpp index 34577730..a44b8f38 100644 --- a/11_Robots/ghost_tank/include/ghost_tank/tank_robot_plugin.hpp +++ b/11_Robots/ghost_tank/include/ghost_tank/tank_robot_plugin.hpp @@ -68,6 +68,7 @@ class TankRobotPlugin : public ghost_ros_interfaces::V5RobotBase void initROSComms(); void initEstimation(); void initIntake(); + void initNeutralStakeArm(); void initTankModel(); void initAutonomy(); @@ -102,7 +103,7 @@ class TankRobotPlugin : public ghost_ros_interfaces::V5RobotBase void updateDrivetrain(std::shared_ptr joy_data); void updateBite(std::shared_ptr joy_data); void updateGoalRush(std::shared_ptr joy_data); - void updateNeutralStake(std::shared_ptr joy_data); + void updateNeutralStakeArm(std::shared_ptr joy_data); void resetWorldPose(); @@ -217,13 +218,24 @@ class TankRobotPlugin : public ghost_ros_interfaces::V5RobotBase double m_conveyor_last_aligned_position{0.0}; bool m_conveyor_hook_is_aligned{false}; - double m_conveyor_hook_throw_fraction{0.0}; double m_conveyor_hook_throw_duration{0.0}; double m_conveyor_throw_start_time{0.0}; bool m_conveyor_hook_is_ejecting{false}; bool m_conveyor_is_throwing{false}; + // Neutral Stake Arm + double m_neutral_stake_arm_kp{0.0}; + double m_neutral_stake_arm_gear_ratio{0.0}; + double m_neutral_stake_arm_rest_pos_deg{0.0}; + double m_neutral_stake_arm_loading_pos_deg{0.0}; + double m_neutral_stake_arm_loaded_pos_deg{0.0}; + double m_neutral_stake_arm_score_neutral_pos_deg{0.0}; + double m_neutral_stake_arm_score_alliance_pos_deg{0.0}; + double m_neutral_stake_arm_down_pos_deg{0.0}; + double m_neutral_stake_arm_des_pos{0.0}; + int m_arm_mode{0}; + // Digital IO std::vector m_digital_io; std::unordered_map m_digital_io_name_map; @@ -270,9 +282,13 @@ class TankRobotPlugin : public ghost_ros_interfaces::V5RobotBase std::vector m_right_drive_motor_names; std::vector m_left_drive_motor_names; - std::vector m_all_motor_names; + std::vector m_all_drive_motor_names; std::unordered_map digital_io_port_map; + + // Current Limiting + std::vector m_loop_current_limits; + int m_num_motors{16}; }; } // namespace ghost_tank diff --git a/11_Robots/ghost_tank/src/tank_robot_plugin.cpp b/11_Robots/ghost_tank/src/tank_robot_plugin.cpp index d13eb0c0..a0dc7a3a 100644 --- a/11_Robots/ghost_tank/src/tank_robot_plugin.cpp +++ b/11_Robots/ghost_tank/src/tank_robot_plugin.cpp @@ -31,6 +31,7 @@ #include #include #include +#include using ghost_planners::RobotTrajectory; using ghost_ros_interfaces::msg_helpers::fromROSMsg; @@ -47,7 +48,6 @@ TankRobotPlugin::TankRobotPlugin() { populateMotorNames(); populateDigitalIONames(); - } void TankRobotPlugin::populateMotorNames() @@ -69,18 +69,15 @@ void TankRobotPlugin::populateMotorNames() "drive_l6", }; - m_all_motor_names.insert( - m_all_motor_names.end(), + m_all_drive_motor_names.insert( + m_all_drive_motor_names.end(), m_left_drive_motor_names.begin(), m_left_drive_motor_names.end()); - m_all_motor_names.insert( - m_all_motor_names.end(), + m_all_drive_motor_names.insert( + m_all_drive_motor_names.end(), m_right_drive_motor_names.begin(), m_right_drive_motor_names.end()); - - m_all_motor_names.push_back("ground_pickup_motor"); - m_all_motor_names.push_back("conveyor_motor"); } void TankRobotPlugin::populateDigitalIONames() @@ -100,6 +97,7 @@ void TankRobotPlugin::initialize() initROSComms(); initEstimation(); initIntake(); + initNeutralStakeArm(); initTankModel(); initAutonomy(); } @@ -247,6 +245,32 @@ void TankRobotPlugin::initIntake() m_conveyor_hook_throw_duration = node_ptr_->get_parameter("tank_robot_plugin.conveyor_hook_throw_duration").as_double(); } +void TankRobotPlugin::initNeutralStakeArm() +{ + std::cout << "[TankRobotPlugin::initNeutralStakeArm]" << std::endl; + + node_ptr_->declare_parameter("tank_robot_plugin.neutral_stake_arm_kp", 0.0); + node_ptr_->declare_parameter("tank_robot_plugin.neutral_stake_arm_gear_ratio", 0.0); + node_ptr_->declare_parameter("tank_robot_plugin.neutral_stake_arm_rest_pos_deg", 0.0); + node_ptr_->declare_parameter("tank_robot_plugin.neutral_stake_arm_loading_pos_deg", 0.0); + node_ptr_->declare_parameter("tank_robot_plugin.neutral_stake_arm_loaded_pos_deg", 0.0); + node_ptr_->declare_parameter("tank_robot_plugin.neutral_stake_arm_score_neutral_pos_deg", 0.0); + node_ptr_->declare_parameter("tank_robot_plugin.neutral_stake_arm_score_alliance_pos_deg", 0.0); + node_ptr_->declare_parameter("tank_robot_plugin.neutral_stake_arm_down_pos_deg", 0.0); + + m_neutral_stake_arm_kp = node_ptr_->get_parameter("tank_robot_plugin.neutral_stake_arm_kp").as_double(); + m_neutral_stake_arm_gear_ratio = node_ptr_->get_parameter("tank_robot_plugin.neutral_stake_arm_gear_ratio").as_double(); + m_neutral_stake_arm_rest_pos_deg = node_ptr_->get_parameter("tank_robot_plugin.neutral_stake_arm_rest_pos_deg").as_double(); + m_neutral_stake_arm_loading_pos_deg = node_ptr_->get_parameter("tank_robot_plugin.neutral_stake_arm_loading_pos_deg").as_double(); + m_neutral_stake_arm_loaded_pos_deg = node_ptr_->get_parameter("tank_robot_plugin.neutral_stake_arm_loaded_pos_deg").as_double(); + m_neutral_stake_arm_score_neutral_pos_deg = node_ptr_->get_parameter("tank_robot_plugin.neutral_stake_arm_score_neutral_pos_deg").as_double(); + m_neutral_stake_arm_score_alliance_pos_deg = node_ptr_->get_parameter("tank_robot_plugin.neutral_stake_arm_score_alliance_pos_deg").as_double(); + m_neutral_stake_arm_down_pos_deg = node_ptr_->get_parameter("tank_robot_plugin.neutral_stake_arm_down_pos_deg").as_double(); + + m_neutral_stake_arm_des_pos = m_neutral_stake_arm_rest_pos_deg; +} + + void TankRobotPlugin::initTankModel() { std::cout << "[TankRobotPlugin::initTankModel]" << std::endl; @@ -261,7 +285,7 @@ void TankRobotPlugin::initTankModel() double wheel_base_inches = node_ptr_->get_parameter("tank_robot_plugin.wheel_base_inches").as_double(); TankConfig tank_model_config; - tank_model_config.motor_list = m_all_motor_names; + tank_model_config.motor_list = m_all_drive_motor_names; tank_model_config.wheel_radius = wheel_rad_in; //in tank_model_config.wheel_gear_ratio = 1.0 / drive_gear_ratio; tank_model_config.wheel_dist = wheel_base_inches / 2.0; //in @@ -323,6 +347,9 @@ void TankRobotPlugin::onNewSensorData() first_loop = false; } + // Clear current limits at start of loop + m_loop_current_limits.clear(); + updateConveyorPositionSensing(); publishIMUData(); updateAndPublishOdometry(); @@ -421,11 +448,11 @@ void TankRobotPlugin::teleop(double current_time) } toggleBagRecorder(joy_data); + updateNeutralStakeArm(joy_data); updateIntake(joy_data->btn_r2, joy_data->btn_r1, joy_data->btn_l1, joy_data->btn_r, current_time); updateBite(joy_data); updateClamp(joy_data); updateGoalRush(joy_data); - updateNeutralStake(joy_data); updateDrivetrain(joy_data); } @@ -467,24 +494,74 @@ void TankRobotPlugin::toggleBagRecorder(std::shared_ptr joy_ } } -void TankRobotPlugin::updateNeutralStake(std::shared_ptr joy_data) +void TankRobotPlugin::updateNeutralStakeArm(std::shared_ptr joy_data) { + std::vector arm_mode_position_map{ + m_neutral_stake_arm_rest_pos_deg, + m_neutral_stake_arm_loading_pos_deg, + m_neutral_stake_arm_loaded_pos_deg, + m_neutral_stake_arm_score_neutral_pos_deg, + m_neutral_stake_arm_score_alliance_pos_deg, + m_neutral_stake_arm_down_pos_deg + }; + + double curr_pos = rhi_ptr_->getMotorPosition("neutral_stake_l") / m_neutral_stake_arm_gear_ratio; double power = 0.0; - double current = 0.0; - if (joy_data->btn_u) { - power = 1.0; - current = 2500; - } else if (joy_data->btn_d) { - power = -1.0; - current = 2500; - } else { + + static bool btn_l_pressed = false; + static bool btn_d_pressed = false; + + if (joy_data->btn_l && m_arm_mode != 5 && !btn_l_pressed) { + m_arm_mode++; + btn_l_pressed = true; + } else if (!joy_data->btn_l) { + btn_l_pressed = false; + } + + if (joy_data->btn_d && m_arm_mode != 0 && !btn_d_pressed) { + m_arm_mode--; + btn_d_pressed = true; + } else if (!joy_data->btn_d) { + btn_d_pressed = false; + } + + m_neutral_stake_arm_des_pos = arm_mode_position_map[m_arm_mode]; + + int32_t current_ma; + double position_error = (m_neutral_stake_arm_des_pos - curr_pos); + if (m_arm_mode == 0 && std::fabs(position_error) < 1) { power = 0.0; - current = 0; + current_ma = 0; + } else { + current_ma = 2500; + power = m_neutral_stake_arm_kp * (m_neutral_stake_arm_des_pos - curr_pos); + } + + // Don't exert positive power at upper limit + if (curr_pos > m_neutral_stake_arm_down_pos_deg) { + power = ghost_util::clamp(power, -1.0, 0.0); + } + + // Don't exert negative power at lower limit + if (curr_pos < m_neutral_stake_arm_rest_pos_deg) { + power = ghost_util::clamp(power, 0.0, 1.0); + } + + if (m_arm_mode == 4) { + power = ghost_util::clamp(power, -0.2, 0.2); } - rhi_ptr_->setMotorCurrentLimitMilliAmps("neutral_stake_motor_l", current); - rhi_ptr_->setMotorCurrentLimitMilliAmps("neutral_stake_motor_r", current); - rhi_ptr_->setMotorVoltageCommandPercent("neutral_stake_motor_l", power); - rhi_ptr_->setMotorVoltageCommandPercent("neutral_stake_motor_r", power); + + if (m_arm_mode == 5) { + power = ghost_util::clamp(power, -0.4, 0.4); + } + + rhi_ptr_->setMotorCurrentLimitMilliAmps("neutral_stake_l", current_ma); + rhi_ptr_->setMotorCurrentLimitMilliAmps("neutral_stake_r", current_ma); + m_loop_current_limits.push_back(current_ma); + m_loop_current_limits.push_back(current_ma); + + rhi_ptr_->setMotorVoltageCommandPercent("neutral_stake_l", power); + rhi_ptr_->setMotorVoltageCommandPercent("neutral_stake_r", power); } void TankRobotPlugin::updateIntake(bool R2, bool R1, bool L1, bool R, double current_time) @@ -574,6 +651,9 @@ void TankRobotPlugin::updateIntake(bool R2, bool R1, bool L1, bool R, double cur rhi_ptr_->setMotorVoltageCommandPercent("conveyor_motor", conveyor_power); rhi_ptr_->setMotorCurrentLimitMilliAmps("conveyor_motor", conveyor_current); + + m_loop_current_limits.push_back(ground_pickup_current); + m_loop_current_limits.push_back(conveyor_current); } void TankRobotPlugin::updateBite(std::shared_ptr joy_data) @@ -616,6 +696,14 @@ void TankRobotPlugin::updateDrivetrain(std::shared_ptr joy_d { m_tank_model_ptr->driveCommandJoystick(joy_data->left_y, -joy_data->right_x, 0.05); + int32_t drive_curr_lim = static_cast(ghost_control::v5_current_limiting::getRemainingCurrentDistributed(m_loop_current_limits, m_num_motors)); + if (std::fabs(joy_data->left_y / 127.0) < 0.05 && std::fabs(-joy_data->right_x / 127.0) < 0.05) { + drive_curr_lim = 0; + } + + for (const auto & name : m_all_drive_motor_names) { + rhi_ptr_->setMotorCurrentLimitMilliAmps(name, drive_curr_lim); + } } void TankRobotPlugin::worldOdometryUpdateCallback(const nav_msgs::msg::Odometry::SharedPtr msg)