Feature (#6): Controllers can set motor voltage

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

include/okapi/api/device/motor/abstractMotor.hpp

@@ -65,6 +65,35 @@ class AbstractMotor : public ControllerOutput<double> {
     double ratio = 1;
   };
 
+  /**
+   * A wrapper that allows Controllers to set the voltage of a motor.
+   */
+  class VoltageControllerOutput : ControllerOutput<double> {
+    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<double> {
    * @return the encoder for this motor
    */
   virtual std::shared_ptr<ContinuousRotarySensor> getEncoder() = 0;
+
+  /**
+   * Returns the voltage ControllerOutput associated with this motor.
+   *
+   * @return the voltage ControllerOutput for this motor
+   */
+  std::shared_ptr<VoltageControllerOutput> getVoltageControllerOutput();
+
+  private:
+  std::shared_ptr<VoltageControllerOutput> ivoltageController;
 };
 
 AbstractMotor::GearsetRatioPair operator*(AbstractMotor::gearset gearset, double ratio);

src/api/device/motor/abstractMotor.cpp

@@ -13,11 +13,23 @@ 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();
 }
 
 double AbstractMotor::getVelocityError() {
   return getTargetVelocity() - getActualVelocity();
 }
+
+std::shared_ptr<AbstractMotor::VoltageControllerOutput> AbstractMotor::getVoltageControllerOutput() {
+  if (ivoltageController == nullptr) {
+    ivoltageController = std::make_shared<VoltageControllerOutput>(*this);
+  }
+
+  return ivoltageController;
+}
 } // namespace okapi

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 <gtest/gtest.h>
+#include <memory>
+
+using namespace okapi;
+
+
+class VoltageControllerOutputTest : public ::testing::Test {
+  protected:
+  void SetUp() override {
+    motor = std::make_shared<MockMotor>();
+  }
+
+  void TearDown() override {}
+
+  std::shared_ptr<MockMotor> 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);
+}