Add SpeedControllerGroup (#362)

wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2017 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "SpeedControllerGroup.h"
+
+using namespace frc;
+
+void SpeedControllerGroup::Set(double speed) {
+  for (auto speedController : m_speedControllers) {
+    speedController.get().Set(m_isInverted ? -speed : speed);
+  }
+}
+
+double SpeedControllerGroup::Get() const {
+  if (!m_speedControllers.empty()) {
+    return m_speedControllers.front().get().Get();
+  }
+  return 0.0;
+}
+
+void SpeedControllerGroup::SetInverted(bool isInverted) {
+  m_isInverted = isInverted;
+}
+
+bool SpeedControllerGroup::GetInverted() const { return m_isInverted; }
+
+void SpeedControllerGroup::Disable() {
+  for (auto speedController : m_speedControllers) {
+    speedController.get().Disable();
+  }
+}
+
+void SpeedControllerGroup::StopMotor() {
+  for (auto speedController : m_speedControllers) {
+    speedController.get().StopMotor();
+  }
+}
+
+void SpeedControllerGroup::PIDWrite(double output) {
+  for (auto speedController : m_speedControllers) {
+    speedController.get().PIDWrite(output);
+  }
+}

wpilibc/src/main/native/include/SpeedControllerGroup.h

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2017 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include "SpeedController.h"
+
+namespace frc {
+
+class SpeedControllerGroup : public SpeedController {
+ public:
+  template <class... SpeedControllers>
+  explicit SpeedControllerGroup(SpeedController& speedController,
+                                SpeedControllers&... speedControllers);
+
+  void Set(double speed) override;
+  double Get() const override;
+  void SetInverted(bool isInverted) override;
+  bool GetInverted() const override;
+  void Disable() override;
+  void StopMotor() override;
+  void PIDWrite(double output) override;
+
+ private:
+  bool m_isInverted = false;
+  std::vector<std::reference_wrapper<SpeedController>> m_speedControllers;
+};
+
+}  // namespace frc
+
+#include "SpeedControllerGroup.inc"

wpilibc/src/main/native/include/SpeedControllerGroup.inc

@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2017 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc {
+
+template <class... SpeedControllers>
+SpeedControllerGroup::SpeedControllerGroup(
+    SpeedController& speedController, SpeedControllers&... speedControllers)
+    : m_speedControllers{speedController, speedControllers...} {}
+
+}  // namespace frc

wpilibc/src/main/native/include/WPILib.h

@@ -78,6 +78,7 @@
 #include "Solenoid.h"
 #include "Spark.h"
 #include "SpeedController.h"
+#include "SpeedControllerGroup.h"
 #include "Talon.h"
 #include "Threads.h"
 #include "TimedRobot.h"

wpilibcIntegrationTests/src/FRCUserProgram/cpp/MockSpeedController.cpp

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "MockSpeedController.h"
+
+using namespace frc;
+
+void MockSpeedController::Set(double speed) {
+  m_speed = m_isInverted ? -speed : speed;
+}
+
+double MockSpeedController::Get() const { return m_speed; }
+
+void MockSpeedController::SetInverted(bool isInverted) {
+  m_isInverted = isInverted;
+}
+
+bool MockSpeedController::GetInverted() const { return m_isInverted; }
+
+void MockSpeedController::Disable() { m_speed = 0; }
+
+void MockSpeedController::StopMotor() { Disable(); }
+
+void MockSpeedController::PIDWrite(double output) { Set(output); }

wpilibcIntegrationTests/src/FRCUserProgram/cpp/SpeedControllerGroupTest.cpp

@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "SpeedControllerGroup.h"  // NOLINT(build/include_order)
+
+#include <memory>
+#include <vector>
+
+#include "MockSpeedController.h"
+#include "TestBench.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+enum SpeedControllerGroupTestType { TEST_ONE, TEST_TWO, TEST_THREE };
+
+std::ostream& operator<<(std::ostream& os,
+                         const SpeedControllerGroupTestType& type) {
+  switch (type) {
+    case TEST_ONE:
+      os << "SpeedControllerGroup with one speed controller";
+      break;
+    case TEST_TWO:
+      os << "SpeedControllerGroup with two speed controllers";
+      break;
+    case TEST_THREE:
+      os << "SpeedControllerGroup with three speed controllers";
+      break;
+  }
+
+  return os;
+}
+
+/**
+ * A fixture used for SpeedControllerGroup testing.
+ */
+class SpeedControllerGroupTest
+    : public testing::TestWithParam<SpeedControllerGroupTestType> {
+ protected:
+  std::vector<MockSpeedController> m_speedControllers;
+  std::unique_ptr<SpeedControllerGroup> m_group;
+
+  void SetUp() override {
+    switch (GetParam()) {
+      case TEST_ONE: {
+        m_speedControllers.emplace_back();
+        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0]);
+        break;
+      }
+
+      case TEST_TWO: {
+        m_speedControllers.emplace_back();
+        m_speedControllers.emplace_back();
+        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0],
+                                                         m_speedControllers[1]);
+        break;
+      }
+
+      case TEST_THREE: {
+        m_speedControllers.emplace_back();
+        m_speedControllers.emplace_back();
+        m_speedControllers.emplace_back();
+        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0],
+                                                         m_speedControllers[1],
+                                                         m_speedControllers[2]);
+        break;
+      }
+    }
+  }
+};
+
+TEST_P(SpeedControllerGroupTest, Set) {
+  m_group->Set(1.0);
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), 1.0);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, GetInverted) {
+  m_group->SetInverted(true);
+
+  EXPECT_TRUE(m_group->GetInverted());
+}
+
+TEST_P(SpeedControllerGroupTest, SetInvertedDoesNotModifySpeedControllers) {
+  for (auto& speedController : m_speedControllers) {
+    speedController.SetInverted(false);
+  }
+  m_group->SetInverted(true);
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_EQ(speedController.GetInverted(), false);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, SetInvertedDoesInvert) {
+  m_group->SetInverted(true);
+  m_group->Set(1.0);
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), -1.0);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, Disable) {
+  m_group->Set(1.0);
+  m_group->Disable();
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), 0.0);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, StopMotor) {
+  m_group->Set(1.0);
+  m_group->StopMotor();
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), 0.0);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, PIDWrite) {
+  m_group->PIDWrite(1.0);
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), 1.0);
+  }
+}
+
+INSTANTIATE_TEST_CASE_P(Test, SpeedControllerGroupTest,
+                        testing::Values(TEST_ONE, TEST_TWO, TEST_THREE));

wpilibcIntegrationTests/src/FRCUserProgram/headers/MockSpeedController.h

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "SpeedController.h"
+
+namespace frc {
+
+class MockSpeedController : public SpeedController {
+ public:
+  void Set(double speed) override;
+  double Get() const override;
+  void SetInverted(bool isInverted) override;
+  bool GetInverted() const override;
+  void Disable() override;
+  void StopMotor() override;
+
+  void PIDWrite(double output) override;
+
+ private:
+  double m_speed = 0.0;
+  bool m_isInverted = false;
+};
+
+}  // namespace frc

wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java

@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2017 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Allows multiple {@link SpeedController} objects to be linked together.
+ */
+public class SpeedControllerGroup implements SpeedController {
+
+  private boolean m_isInverted = false;
+  private final SpeedController[] m_speedControllers;
+
+  /**
+   * Create a new SpeedControllerGroup with the provided SpeedControllers.
+   *
+   * @param speedControllers The SpeedControllers to add
+   */
+  public SpeedControllerGroup(SpeedController speedController,
+                              SpeedController... speedControllers) {
+    m_speedControllers = new SpeedController[speedControllers.length + 1];
+    m_speedControllers[0] = speedController;
+    for (int i = 0; i < speedControllers.length; i++) {
+      m_speedControllers[i + 1] = speedControllers[i];
+    }
+  }
+
+  @Override
+  public void set(double speed) {
+    for (SpeedController speedController : m_speedControllers) {
+      speedController.set(m_isInverted ? -speed : speed);
+    }
+  }
+
+  @Override
+  public double get() {
+    if (m_speedControllers.length > 0) {
+      return m_speedControllers[0].get();
+    }
+    return 0.0;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    for (SpeedController speedController : m_speedControllers) {
+      speedController.disable();
+    }
+  }
+
+  @Override
+  public void stopMotor() {
+    for (SpeedController speedController : m_speedControllers) {
+      speedController.stopMotor();
+    }
+  }
+
+  @Override
+  public void pidWrite(double output) {
+    for (SpeedController speedController : m_speedControllers) {
+      speedController.pidWrite(output);
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+public class MockSpeedController implements SpeedController {
+  private double m_speed = 0.0;
+  private boolean m_isInverted = false;
+
+  @Override
+  public void set(double speed) {
+    m_speed = m_isInverted ? -speed : speed;
+  }
+
+  @Override
+  public double get() {
+    return m_speed;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    m_speed = 0;
+  }
+
+  @Override
+  public void stopMotor() {
+    disable();
+  }
+
+  @Override
+  public void pidWrite(double output) {
+    set(output);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java

@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.Test;
+import org.junit.runner.RunWith;
+import org.junit.runners.Parameterized;
+
+import java.util.Arrays;
+import java.util.Collection;
+
+import static org.junit.Assert.assertArrayEquals;
+import static org.junit.Assert.assertTrue;
+
+@RunWith(Parameterized.class)
+public class SpeedControllerGroupTest {
+  private final SpeedController[] m_speedControllers;
+  private final SpeedControllerGroup m_group;
+
+  /**
+   * Returns a Collection of ArrayLists with various MockSpeedController configurations.
+   */
+  @Parameterized.Parameters
+  public static Collection<Object[][]> data() {
+    return Arrays.asList((Object[][][]) new SpeedController[][][] {
+        {{new MockSpeedController()}},
+        {{new MockSpeedController(),
+            new MockSpeedController()}},
+        {{new MockSpeedController(),
+            new MockSpeedController(),
+            new MockSpeedController()}}
+    });
+  }
+
+  /**
+   * Construct SpeedControllerGroupTest.
+   */
+  public SpeedControllerGroupTest(SpeedController[] speedControllers) {
+    m_group = new SpeedControllerGroup(speedControllers[0],
+        Arrays.copyOfRange(speedControllers, 1, speedControllers.length));
+    m_speedControllers = speedControllers;
+  }
+
+  @Test
+  public void testSet() {
+    m_group.set(1.0);
+
+    assertArrayEquals(Arrays.stream(m_speedControllers).mapToDouble(__ -> 1.0).toArray(),
+        Arrays.stream(m_speedControllers).mapToDouble(SpeedController::get).toArray(),
+        0.0);
+  }
+
+  @Test
+  public void testGetInverted() {
+    m_group.setInverted(true);
+
+    assertTrue(m_group.getInverted());
+  }
+
+  @Test
+  public void testSetInvertedDoesNotModifySpeedControllers() {
+    for (SpeedController speedController : m_speedControllers) {
+      speedController.setInverted(false);
+    }
+    m_group.setInverted(true);
+
+    assertArrayEquals(Arrays.stream(m_speedControllers).map(__ -> false).toArray(),
+        Arrays.stream(m_speedControllers).map(SpeedController::getInverted).toArray());
+  }
+
+  @Test
+  public void testSetInvertedDoesInvert() {
+    m_group.setInverted(true);
+    m_group.set(1.0);
+
+    assertArrayEquals(Arrays.stream(m_speedControllers).mapToDouble(__ -> -1.0).toArray(),
+        Arrays.stream(m_speedControllers).mapToDouble(SpeedController::get).toArray(),
+        0.0);
+  }
+
+  @Test
+  public void testDisable() {
+    m_group.set(1.0);
+    m_group.disable();
+
+    assertArrayEquals(Arrays.stream(m_speedControllers).mapToDouble(__ -> 0.0).toArray(),
+        Arrays.stream(m_speedControllers).mapToDouble(SpeedController::get).toArray(),
+        0.0);
+  }
+
+  @Test
+  public void testStopMotor() {
+    m_group.set(1.0);
+    m_group.stopMotor();
+
+    assertArrayEquals(Arrays.stream(m_speedControllers).mapToDouble(__ -> 0.0).toArray(),
+        Arrays.stream(m_speedControllers).mapToDouble(SpeedController::get).toArray(),
+        0.0);
+  }
+
+  @Test
+  public void testPidWrite() {
+    m_group.pidWrite(1.0);
+
+    assertArrayEquals(Arrays.stream(m_speedControllers).mapToDouble(__ -> 1.0).toArray(),
+        Arrays.stream(m_speedControllers).mapToDouble(SpeedController::get).toArray(),
+        0.0);
+  }
+}