From 877a9eae1fcc61d61333eae3378f87b5e7544670 Mon Sep 17 00:00:00 2001 From: Austin Shalit Date: Mon, 16 Oct 2017 22:54:36 -0400 Subject: [PATCH] Add SpeedControllerGroup (#362) --- .../main/native/cpp/SpeedControllerGroup.cpp | 47 ++++++ .../native/include/SpeedControllerGroup.h | 38 +++++ .../native/include/SpeedControllerGroup.inc | 17 +++ wpilibc/src/main/native/include/WPILib.h | 1 + .../cpp/MockSpeedController.cpp | 28 ++++ .../cpp/SpeedControllerGroupTest.cpp | 137 ++++++++++++++++++ .../headers/MockSpeedController.h | 30 ++++ .../first/wpilibj/SpeedControllerGroup.java | 77 ++++++++++ .../first/wpilibj/MockSpeedController.java | 48 ++++++ .../wpilibj/SpeedControllerGroupTest.java | 114 +++++++++++++++ 10 files changed, 537 insertions(+) create mode 100644 wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp create mode 100644 wpilibc/src/main/native/include/SpeedControllerGroup.h create mode 100644 wpilibc/src/main/native/include/SpeedControllerGroup.inc create mode 100644 wpilibcIntegrationTests/src/FRCUserProgram/cpp/MockSpeedController.cpp create mode 100644 wpilibcIntegrationTests/src/FRCUserProgram/cpp/SpeedControllerGroupTest.cpp create mode 100644 wpilibcIntegrationTests/src/FRCUserProgram/headers/MockSpeedController.h create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java diff --git a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp new file mode 100644 index 0000000000..b77f9552ab --- /dev/null +++ b/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); + } +} diff --git a/wpilibc/src/main/native/include/SpeedControllerGroup.h b/wpilibc/src/main/native/include/SpeedControllerGroup.h new file mode 100644 index 0000000000..ca862d6da4 --- /dev/null +++ b/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 +#include + +#include "SpeedController.h" + +namespace frc { + +class SpeedControllerGroup : public SpeedController { + public: + template + 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> m_speedControllers; +}; + +} // namespace frc + +#include "SpeedControllerGroup.inc" diff --git a/wpilibc/src/main/native/include/SpeedControllerGroup.inc b/wpilibc/src/main/native/include/SpeedControllerGroup.inc new file mode 100644 index 0000000000..44f06c2813 --- /dev/null +++ b/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 +SpeedControllerGroup::SpeedControllerGroup( + SpeedController& speedController, SpeedControllers&... speedControllers) + : m_speedControllers{speedController, speedControllers...} {} + +} // namespace frc diff --git a/wpilibc/src/main/native/include/WPILib.h b/wpilibc/src/main/native/include/WPILib.h index b4b988d2aa..189a6b5d58 100644 --- a/wpilibc/src/main/native/include/WPILib.h +++ b/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" diff --git a/wpilibcIntegrationTests/src/FRCUserProgram/cpp/MockSpeedController.cpp b/wpilibcIntegrationTests/src/FRCUserProgram/cpp/MockSpeedController.cpp new file mode 100644 index 0000000000..8162fb0680 --- /dev/null +++ b/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); } diff --git a/wpilibcIntegrationTests/src/FRCUserProgram/cpp/SpeedControllerGroupTest.cpp b/wpilibcIntegrationTests/src/FRCUserProgram/cpp/SpeedControllerGroupTest.cpp new file mode 100644 index 0000000000..692859790f --- /dev/null +++ b/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 +#include + +#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 { + protected: + std::vector m_speedControllers; + std::unique_ptr m_group; + + void SetUp() override { + switch (GetParam()) { + case TEST_ONE: { + m_speedControllers.emplace_back(); + m_group = std::make_unique(m_speedControllers[0]); + break; + } + + case TEST_TWO: { + m_speedControllers.emplace_back(); + m_speedControllers.emplace_back(); + m_group = std::make_unique(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(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)); diff --git a/wpilibcIntegrationTests/src/FRCUserProgram/headers/MockSpeedController.h b/wpilibcIntegrationTests/src/FRCUserProgram/headers/MockSpeedController.h new file mode 100644 index 0000000000..4569c4dba4 --- /dev/null +++ b/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 diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java new file mode 100644 index 0000000000..559022e0cb --- /dev/null +++ b/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); + } + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java new file mode 100644 index 0000000000..9f53ac5447 --- /dev/null +++ b/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); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java new file mode 100644 index 0000000000..a88a71226e --- /dev/null +++ b/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 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); + } +}