diff --git a/CMakeLists.txt b/CMakeLists.txt index 11beee5c6..9d757d529 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -189,6 +189,7 @@ add_executable(OkapiLibV5 test/implMocks.cpp test/twoEncoderOdometryTests.cpp test/utilTests.cpp + test/voltageControllerOutputTests.cpp test/unitTests.cpp test/loggerTests.cpp test/skidSteerModelTests.cpp diff --git a/include/okapi/api/device/motor/abstractMotor.hpp b/include/okapi/api/device/motor/abstractMotor.hpp index ff46fe078..e2c794c87 100644 --- a/include/okapi/api/device/motor/abstractMotor.hpp +++ b/include/okapi/api/device/motor/abstractMotor.hpp @@ -65,6 +65,35 @@ class AbstractMotor : public ControllerOutput { double ratio = 1; }; + /** + * A wrapper that allows Controllers to set the voltage of a motor. + */ + class VoltageControllerOutput : ControllerOutput { + public: + /** + * A wrapper class allows Controllers to set the voltage of a motor. + * + * This should only be instantiated by the motor itself + * + * @param iabstractMotor The motor to control. + */ + explicit VoltageControllerOutput(AbstractMotor &iabstractMotor) + : internalMotor(iabstractMotor){ + } + + /** + * Controller method: sets the voltage for the motor. + * + * The voltage is scaled to the range [-12000, 12000]. + * + * @param ivalue The new setpoint + */ + void controllerSet(double ivalue) override; + + private: + AbstractMotor &internalMotor; + }; + virtual ~AbstractMotor(); /******************************************************************************/ @@ -530,6 +559,16 @@ class AbstractMotor : public ControllerOutput { * @return the encoder for this motor */ virtual std::shared_ptr getEncoder() = 0; + + /** + * Returns the voltage ControllerOutput associated with this motor. + * + * @return the voltage ControllerOutput for this motor + */ + std::shared_ptr getVoltageControllerOutput(); + + private: + std::shared_ptr ivoltageController; }; AbstractMotor::GearsetRatioPair operator*(AbstractMotor::gearset gearset, double ratio); diff --git a/src/api/device/motor/abstractMotor.cpp b/src/api/device/motor/abstractMotor.cpp index d4f9a687f..6fca871b2 100644 --- a/src/api/device/motor/abstractMotor.cpp +++ b/src/api/device/motor/abstractMotor.cpp @@ -13,6 +13,10 @@ AbstractMotor::GearsetRatioPair operator*(const AbstractMotor::gearset gearset, return AbstractMotor::GearsetRatioPair(gearset, ratio); } +void AbstractMotor::VoltageControllerOutput::controllerSet(double ivalue) { + internalMotor.moveVoltage(ivalue * 12000); +} + double AbstractMotor::getPositionError() { return getTargetPosition() - getPosition(); } @@ -20,4 +24,12 @@ double AbstractMotor::getPositionError() { double AbstractMotor::getVelocityError() { return getTargetVelocity() - getActualVelocity(); } + +std::shared_ptr AbstractMotor::getVoltageControllerOutput() { + if (ivoltageController == nullptr) { + ivoltageController = std::make_shared(*this); + } + + return ivoltageController; +} } // namespace okapi diff --git a/test/voltageControllerOutputTests.cpp b/test/voltageControllerOutputTests.cpp new file mode 100644 index 000000000..5f3523d87 --- /dev/null +++ b/test/voltageControllerOutputTests.cpp @@ -0,0 +1,38 @@ +/* +* This Source Code Form is subject to the terms of the Mozilla Public +* License, v. 2.0. If a copy of the MPL was not distributed with this +* file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ +#include "test/tests/api/implMocks.hpp" +#include +#include + +using namespace okapi; + + +class VoltageControllerOutputTest : public ::testing::Test { + protected: + void SetUp() override { + motor = std::make_shared(); + } + + void TearDown() override {} + + std::shared_ptr motor; +}; + +TEST_F(VoltageControllerOutputTest, OneVoltageControllerOutputPerMotor) { + auto output1 = motor->getVoltageControllerOutput(); + EXPECT_NE(output1, nullptr); + + auto output2 = motor->getVoltageControllerOutput(); + EXPECT_EQ(output1, output2); +} + +TEST_F(VoltageControllerOutputTest, SetsMotorVoltage) { + auto output = motor->getVoltageControllerOutput(); + double setpoint = 0.5; + + output->controllerSet(setpoint); + EXPECT_EQ(motor->lastVoltage, 12000*setpoint); +}