From efeec5d986f00d602ada8466d2c99ecb20e146d6 Mon Sep 17 00:00:00 2001 From: Jonathan Leitschuh Date: Wed, 28 May 2014 13:21:35 -0400 Subject: [PATCH] Adds/Extends several integration tests. Modifies Java Servo with correct values according to the datasheet assoiciated with the Hitec HS-322HD Changes the format of the MotorEncoderFixture usage. Also adds a motor type to easily determine what speed controller is being used Adds a TiltPanCameraTest to test the Gyroscope as well as the servo classes Updates the TestBench and the TestSuite to reflect the new tests and the changes to these Tests Begins integrating a test to check to see if counters and encoders are working correctly Modifies the ServoClass to have the correct data for PWM generation. Also modifies the PWM class to make the setBounds method to be protected Modifies the TiltPanTest to rotate slower and reduces the acceptable deviation Reduces the runtime of the MotorEncoderTest by lowering the runtime value Updates the TestBench to load the Gyroscope before allocating the servos in order to allow for setup time Also adds Documentation Reorders the Tests to run the TiltPanTest first Adds/Updates the copywright header to several files that were previously modified in this commit Change-Id: I093f998d9687c6b6c6b8d4f6949fbe56ba236212 --- .../main/java/edu/wpi/first/wpilibj/PWM.java | 4 +- .../java/edu/wpi/first/wpilibj/Servo.java | 23 ++- wpilibj/wpilibJavaIntegrationTests/README.md | 7 + .../wpi/first/wpilibj/MotorEncoderTest.java | 142 +++++++++++++---- .../edu/wpi/first/wpilibj/SampleTest.java | 2 +- .../wpi/first/wpilibj/TiltPanCameraTest.java | 69 ++++++++ .../java/edu/wpi/first/wpilibj/TimerTest.java | 6 + .../fixtures/DIOCrossConnectFixture.java | 29 ++++ .../first/wpilibj/fixtures/ITestFixture.java | 2 +- .../wpilibj/fixtures/MotorEncoderFixture.java | 139 +++++++++++++++++ .../first/wpilibj/fixtures/SampleFixture.java | 2 +- .../fixtures/TiltPanCameraFixture.java | 84 ++++++++++ .../first/wpilibj/groups/MotorEncoder.java | 48 ------ .../mockhardware/FakeCounterSource.java | 147 ++++++++++++++++++ .../first/wpilibj/test/AbstractComsSetup.java | 2 +- .../edu/wpi/first/wpilibj/test/TestBench.java | 128 ++++++++++----- .../edu/wpi/first/wpilibj/test/TestSuite.java | 10 +- 17 files changed, 715 insertions(+), 129 deletions(-) create mode 100644 wpilibj/wpilibJavaIntegrationTests/README.md create mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TiltPanCameraTest.java create mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java create mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java create mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java delete mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/groups/MotorEncoder.java create mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PWM.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PWM.java index 9cd76fff2b..85585fbe5c 100644 --- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PWM.java +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PWM.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) FIRST 2008-2014. 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. */ @@ -201,7 +201,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @param deadbandMin The low end of the deadband pulse width in ms * @param min The minimum pulse width in ms */ - void setBounds(double max, double deadbandMax, double center, double deadbandMin, double min) { + protected void setBounds(double max, double deadbandMax, double center, double deadbandMin, double min) { double loopTime = m_module.getLoopTiming()/(kSystemClockTicksPerMicrosecond*1e3); m_maxPwm = (int)((max-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1); diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/Servo.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/Servo.java index e4aa3bb78c..f3651b10d6 100644 --- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/Servo.java +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/Servo.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) FIRST 2008-2014. 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. */ @@ -22,17 +22,21 @@ import edu.wpi.first.wpilibj.tables.ITableListener; */ public class Servo extends PWM implements IDevice { - private static final double kMaxServoAngle = 170.0; + private static final double kMaxServoAngle = 180.0; private static final double kMinServoAngle = 0.0; + + private static final double kDefaultMaxServoPWM = 2.4; + private static final double kDefaultMinServoPWM = .6; /** * Common initialization code called by all constructors. * * InitServo() assigns defaults for the period multiplier for the servo PWM control signal, as * well as the minimum and maximum PWM values supported by the servo. + * */ private void initServo() { - setBounds(2.27, 0, 0, 0, .743); + setBounds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM); setPeriodMultiplier(PeriodMultiplier.k4X); LiveWindow.addActuator("Servo", getModuleNumber(), getChannel(), this); @@ -40,7 +44,10 @@ public class Servo extends PWM implements IDevice { } /** - * Constructor that assumes the default digital module. + * Constructor that assumes the default digital module.
+ * + * By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value
+ * By default {@value #kDefaultMinServoPWM} ms is used as the minPWM value
* * @param channel The PWM channel on the digital module to which the servo is attached. */ @@ -50,8 +57,11 @@ public class Servo extends PWM implements IDevice { } /** - * Constructor that specifies the digital module. + * Constructor that specifies the digital module.
* + * By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value
+ * By default {@value #kDefaultMinServoPWM} ms is used as the minPWM value
+ * * @param slot The slot in the chassis that the digital module is plugged into. * @param channel The PWM channel on the digital module to which the servo is attached. */ @@ -59,8 +69,9 @@ public class Servo extends PWM implements IDevice { super(slot, channel); initServo(); } + - /** + /** * Set the servo position. * * Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. diff --git a/wpilibj/wpilibJavaIntegrationTests/README.md b/wpilibj/wpilibJavaIntegrationTests/README.md new file mode 100644 index 0000000000..e4199f9eb6 --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/README.md @@ -0,0 +1,7 @@ +TO LOAD & RUN INTEGRATION TESTS + +1) Run 'mvn clean package' from this directory +2) Run 'scp target/wpilibJavaIntegrationTests-0.1.0-SNAPSHOT.jar admin@10.1.90.2:/home/admin' and enter the password on the RoboRio +3) ssh into the RoboRio using 'ssh admin@10.1.90.2' +4) Run the integration tests on the roborio using './runintegrationjavaprogram' +5) Enable the robot using a driver station (this will be automatic eventually) diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java index 66503a8177..33c479582c 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java @@ -1,91 +1,133 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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 static org.junit.Assert.assertFalse; import static org.junit.Assert.assertTrue; +import java.util.ArrayList; +import java.util.List; + import org.junit.AfterClass; import org.junit.Before; import org.junit.BeforeClass; import org.junit.Test; -import edu.wpi.first.wpilibj.groups.MotorEncoder; +import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture; import edu.wpi.first.wpilibj.test.AbstractComsSetup; import edu.wpi.first.wpilibj.test.TestBench; public class MotorEncoderTest extends AbstractComsSetup { - private static final double MOTOR_RUNTIME = 2.5; + private static final double MOTOR_RUNTIME = .5; - static MotorEncoder pairs [] = new MotorEncoder[3]; + private static final List pairs = new ArrayList(); @BeforeClass public static void classSetup() { // Set up the fixture before the test is created - pairs[0] = TestBench.getInstance().getTalonPair(); - pairs[1] = TestBench.getInstance().getVictorPair(); - pairs[2] = TestBench.getInstance().getJaguarPair(); - //pairs[3] = TestBench.getInstance().getCanJaguarPair(); + pairs.add(TestBench.getInstance().getTalonPair()); + pairs.add(TestBench.getInstance().getVictorPair()); + pairs.add(TestBench.getInstance().getJaguarPair()); + pairs.add(TestBench.getInstance().getCanJaguarPair()); - for(MotorEncoder me : pairs){ + for(MotorEncoderFixture me : pairs){ me.reset(); } } + + /** + * Checks to see if this MotorEncoderFixture is fully configured for encoder testing + * @param me The motor encoder under test + * @return true if this motor encoder has an encoder attached to it + */ + boolean shouldRunTest(MotorEncoderFixture me){ + return me.getType().equals(Victor.class.getSimpleName()) || me.getType().equals(Talon.class.getSimpleName()); + } @Before public void setUp() { - // Reset the fixture elements before every test + for(MotorEncoderFixture me : pairs){ + double initialSpeed = me.getMotor().get(); + assertTrue(me.getType() + " Did not start with an initial speeed of 0 instead got: " + initialSpeed, Math.abs(initialSpeed) < 0.001); + me.setup(); + } } @AfterClass public static void tearDown() { // Clean up the fixture after the test - for(MotorEncoder me : pairs){ - me.reset(); + for(MotorEncoderFixture me : pairs){ + me.teardown(); + } + pairs.clear(); + } + + /** + * Test to ensure that the isMotorWithinRange method is functioning properly. + * Really only needs to run on one MotorEncoderFixture to ensure that it is working correctly. + */ + @Test + public void testIsMotorWithinRange(){ + for(MotorEncoderFixture me: pairs){ + double range = 0.01; + assertTrue(me.getType() + " 1", me.isMotorSpeedWithinRange(0.0, range)); + assertTrue(me.getType() + " 2", me.isMotorSpeedWithinRange(0.0, -range)); + assertFalse(me.getType() + " 3", me.isMotorSpeedWithinRange(1.0, range)); + assertFalse(me.getType() + " 4", me.isMotorSpeedWithinRange(-1.0, range)); } } /** - * This test is designed to see if the values of different motors will increment + * This test is designed to see if the values of different motors will increment when spun forward */ @Test public void testIncrement() { - int i = 0; - for(MotorEncoder me: pairs){ + for(MotorEncoderFixture me: pairs){ int startValue = me.getEncoder().get(); me.getMotor().set(.75); Timer.delay(MOTOR_RUNTIME); int currentValue = me.getEncoder().get(); - if(i == 1){ //TODO REMOVE THIS WHEN ALL ENCODERS ARE PROPERLY ATTACHED - assertTrue("Encoder not incremented: start: " + startValue + "; current: " + currentValue, startValue < currentValue); + if(shouldRunTest(me)){ //TODO REMOVE THIS WHEN ALL ENCODERS ARE PROPERLY ATTACHED + assertTrue(me.getType() + " Encoder not incremented: start: " + startValue + "; current: " + currentValue, startValue < currentValue); } me.reset(); - i++; + encodersResetCheck(me); } } + /** + * This test is designed to see if the values of different motors will decrement when spun in reverse + */ @Test public void testDecrement(){ - int i = 0; - for(MotorEncoder me: pairs){ + for(MotorEncoderFixture me: pairs){ int startValue = me.getEncoder().get(); me.getMotor().set(-.75); Timer.delay(MOTOR_RUNTIME); int currentValue = me.getEncoder().get(); - if(i == 1){ //TODO REMOVE THIS WHEN ALL ENCODERS ARE PROPERLY ATTACHED - assertTrue("Encoder not decremented: start: " + startValue + "; current: " + currentValue, startValue > currentValue); + if(shouldRunTest(me)){ //TODO REMOVE THIS WHEN ALL ENCODERS ARE PROPERLY ATTACHED + assertTrue(me.getType() + " Encoder not decremented: start: " + startValue + "; current: " + currentValue, startValue > currentValue); } me.reset(); - i++; + encodersResetCheck(me); } } + /** + * This method test if the counters count when the motors rotate + */ @Test public void testCouter(){ - int i = 0; - for(MotorEncoder me: pairs){ + for(MotorEncoderFixture me: pairs){ int counter1Start = me.getCounters()[0].get(); int counter2Start = me.getCounters()[1].get(); @@ -93,13 +135,57 @@ public class MotorEncoderTest extends AbstractComsSetup { Timer.delay(MOTOR_RUNTIME); int counter1End = me.getCounters()[0].get(); int counter2End = me.getCounters()[1].get(); - if(i == 1){ //TODO REMOVE THIS WHEN ALL ENCODERS ARE PROPERLY ATTACHED - assertTrue("Counter not incremented: start: " + counter1Start + "; current: " + counter1End, counter1Start < counter1End); - assertTrue("Counter not incremented: start: " + counter1Start + "; current: " + counter2End, counter2Start < counter2End); + if(shouldRunTest(me)){ //TODO REMOVE THIS WHEN ALL ENCODERS ARE PROPERLY ATTACHED + assertTrue(me.getType() + " Counter not incremented: start: " + counter1Start + "; current: " + counter1End, counter1Start < counter1End); + assertTrue(me.getType() + " Counter not incremented: start: " + counter1Start + "; current: " + counter2End, counter2Start < counter2End); } me.reset(); - i++; + encodersResetCheck(me); } } + + /** + * Tests to see if you set the speed to something not <= 1.0 if the code appropriately throttles the value + */ + @Test + public void testSetHighForwardSpeed(){ + for(MotorEncoderFixture me:pairs){ + me.getMotor().set(15); + assertTrue(me.getType() + " Motor speed was not close to 1.0, was: " + me.getMotor().get(), me.isMotorSpeedWithinRange(1.0, 0.001)); + me.reset(); + } + } + + /** + * Tests to see if you set the speed to something not >= -1.0 if the code appropriately throttles the value + */ + @Test + public void testSetHighReverseSpeed(){ + for(MotorEncoderFixture me:pairs){ + me.getMotor().set(-15); + assertTrue(me.getType() + " Motor speed was not close to 1.0, was: " + me.getMotor().get(), me.isMotorSpeedWithinRange(-1.0, 0.001)); + me.reset(); + } + } + + + + + /** + * Checks to see if the encoders and counters are appropriately reset to zero when reset + * @param me The MotorEncoderFixture under test + */ + private void encodersResetCheck(MotorEncoderFixture me){ + int encoderVal = me.getEncoder().get(); + int counterVal[] = new int[2]; + for(int i = 0; i < 2; i++){ + counterVal[i] = me.getCounters()[i].get(); + } + double motorVal = me.getMotor().get(); + assertTrue(me.getType() + " Encoder value: " + encoderVal + " when it should be 0", encoderVal == 0); + assertTrue(me.getType() + " Motor value: " + motorVal + " when it should be 0", motorVal == 0); + assertTrue(me.getType() + " Counter value " + counterVal[0] + " when is should be 0", counterVal[0] == 0); + assertTrue(me.getType() + " Counter value " + counterVal[1] + " when is should be 0", counterVal[1] == 0); + } } diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java index 7850e1d449..a65e40d9a6 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) FIRST 2008-2014. 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. */ diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TiltPanCameraTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TiltPanCameraTest.java new file mode 100644 index 0000000000..6a85430a47 --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TiltPanCameraTest.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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 static org.junit.Assert.*; + +import org.junit.After; +import org.junit.AfterClass; +import org.junit.Before; +import org.junit.BeforeClass; +import org.junit.Test; + +import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture; +import edu.wpi.first.wpilibj.test.AbstractComsSetup; +import edu.wpi.first.wpilibj.test.TestBench; + +public class TiltPanCameraTest extends AbstractComsSetup { + + public static final double TEST_ANGLE = 180.0; + + private TiltPanCameraFixture tpcam; + + @BeforeClass + public static void setUpBeforeClass() throws Exception { + } + + @AfterClass + public static void tearDownAfterClass() throws Exception { + } + + @Before + public void setUp() throws Exception { + tpcam = TestBench.getInstance().getTiltPanCam(); + tpcam.reset(); + } + + @After + public void tearDown() throws Exception { + tpcam.teardown(); + } + + /** + * Test to see if the Servo and the gyroscope is turning 180 degrees + */ + @Test + public void testGyroAngle() { + for(double i = 0; i < 1.0; i+=.01){ + //System.out.println("i: " + i); + tpcam.getPan().set(i); + Timer.delay(.05); + } + Timer.delay(TiltPanCameraFixture.RESET_TIME); + double angle = tpcam.getGyro().getAngle(); + + double difference = TEST_ANGLE - angle; + + double diff = Math.abs(difference); + + + assertTrue("Gryo angle skewed " + difference + " deg away from target " + TEST_ANGLE, diff < 4.0); + + + } + +} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TimerTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TimerTest.java index 46a196a721..e5eee20099 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TimerTest.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TimerTest.java @@ -1,3 +1,9 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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 static org.junit.Assert.assertTrue; diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java new file mode 100644 index 0000000000..3690f8aa4e --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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.fixtures; + +public class DIOCrossConnectFixture implements ITestFixture { + + @Override + public boolean setup() { + // TODO Auto-generated method stub + return false; + } + + @Override + public boolean reset() { + // TODO Auto-generated method stub + return false; + } + + @Override + public boolean teardown() { + // TODO Auto-generated method stub + return false; + } + +} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java index 5a147dfc29..c4176f2e49 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) FIRST 2008-2014. 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. */ diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java new file mode 100644 index 0000000000..0923d5dd0f --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java @@ -0,0 +1,139 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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.fixtures; + +import edu.wpi.first.wpilibj.Counter; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.test.TestBench; + +/** + * Represents a physically connected Motor and Encoder to allow for unit tests on these difrent pairs
+ * Designed to allow the user to easily setup and tear down the fixture to allow for reuse. + * This class should be explicitly instantiated in the TestBed class to allow any test to access this fixture. + * This allows tests to be mailable so that you can easily reconfigure the physical testbed without breaking the tests. + * + * @author Jonathan Leitschuh + * + */ +public class MotorEncoderFixture implements ITestFixture { + + private final SpeedController motor; + private final Encoder encoder; + private final Counter counters[] = new Counter[2]; + private final DigitalInput aSource; //Stored so it can be freed at tear down + private final DigitalInput bSource; + + /** + * Default constructor for a MotorEncoderFixture + * @param motor The SpeedControler for this MotorEncoder pair + * @param aSource One of the inputs for the encoder + * @param bSource The other input for the encoder + */ + public MotorEncoderFixture(SpeedController motor, DigitalInput aSource, DigitalInput bSource){ + this.aSource = aSource; + this.bSource = bSource; + + this.motor = motor; + + this.encoder = new Encoder(aSource, bSource); + counters[0] = new Counter(aSource); + counters[1] = new Counter(bSource); + for(Counter c: counters){ + c.start(); + } + encoder.start(); + } + + @Override + public boolean setup() { + return reset(); + } + + /** + * Gets the motor for this Object + * @return the motor this object refers too + */ + public SpeedController getMotor(){ + return motor; + } + + /** + * Gets the encoder for this object + * @return the encoder that this object refers too + */ + public Encoder getEncoder(){ + return encoder; + } + + public Counter[] getCounters(){ + return counters; + } + + /** + * Retrieves the name of the motor that this object refers to + * @return The simple name of the motor {@link Class#getSimpleName()} + */ + public String getType(){ + return motor.getClass().getSimpleName(); + } + + /** + * Checks to see if the speed of the motor is within some range of a given value. + * This is used instead of equals() because doubles can have inaccuracies. + * @param value The value to compare against + * @param acuracy The accuracy range to check against to see if the + * @return true if the range of values between motors speed ± accuracy contains the 'value'
+ * {@code Math.abs((Math.abs(motor.get()) - Math.abs(value))) < Math.abs(accuracy)} + */ + public boolean isMotorSpeedWithinRange(double value, double accuracy){ + return Math.abs((Math.abs(motor.get()) - Math.abs(value))) < Math.abs(accuracy); + } + + @Override + public boolean reset(){ + boolean wasReset = true; + + motor.set(0); + Timer.delay(TestBench.MOTOR_STOP_TIME); //DEFINED IN THE TestBench + encoder.reset(); + for(Counter c : counters){ + c.reset(); + } + + wasReset &= motor.get() == 0; + wasReset &= encoder.get() == 0; + for(Counter c : counters){ + wasReset &= c.get() == 0; + } + + return wasReset; + } + + + + @Override + public boolean teardown() { + reset(); + if(motor instanceof PWM){ + ((PWM) motor).free(); + } + encoder.free(); + for(Counter c : counters){ + c.free(); + } + + aSource.free(); + bSource.free(); + + return true; + } + +} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java index 1f6c6cd90a..5ed31a5e3b 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) FIRST 2008-2014. 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. */ diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java new file mode 100644 index 0000000000..1d0ca196db --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java @@ -0,0 +1,84 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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.fixtures; + +import edu.wpi.first.wpilibj.Gyro; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.Timer; + +/** + * A class to represent the a physical Camera with two servos (tilt and pan) designed to test to see if the + * gyroscope is operating normally. + * + * @author Jonathan Leitschuh + * + */ +public class TiltPanCameraFixture implements ITestFixture { + + public static final double RESET_TIME = 3.0; + + private final Gyro gyro; + private final Servo tilt; + private final Servo pan; + + /** + * Constructs the TiltPanCamera + * @param tilt + * The servo to tilt the camera + * @param pan + * The servo to pan the camera + * @param gyro + */ + public TiltPanCameraFixture (Servo tilt, Servo pan, Gyro gyro){ + this.tilt = tilt; + this.pan = pan; + this.gyro = gyro; + + } + + @Override + public boolean setup() { + return reset(); + } + + @Override + public boolean reset(){ + boolean wasReset = true; + pan.setAngle(0); + tilt.set(0); + Timer.delay(RESET_TIME); + gyro.reset(); + + wasReset &= pan.get() == 0; + wasReset &= tilt.get() == 0; + wasReset &= gyro.getAngle() == 0; + return wasReset; + } + + public Servo getTilt() { + return tilt; + } + + public Servo getPan() { + return pan; + } + + public Gyro getGyro() { + return gyro; + } + + @Override + public boolean teardown() { + reset(); + + tilt.free(); + pan.free(); + gyro.free(); + return true; + } + +} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/groups/MotorEncoder.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/groups/MotorEncoder.java deleted file mode 100644 index 1a398a62fe..0000000000 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/groups/MotorEncoder.java +++ /dev/null @@ -1,48 +0,0 @@ -package edu.wpi.first.wpilibj.groups; - -import edu.wpi.first.wpilibj.Counter; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DigitalSource; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.SpeedController; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.test.TestBench; - -public class MotorEncoder { - - private final SpeedController motor; - private final Encoder encoder; - private final Counter counters[] = new Counter[2]; - - public MotorEncoder(SpeedController motor, DigitalInput aSource, DigitalInput bSource){ - this.motor = motor; - this.encoder = new Encoder(aSource, bSource); - counters[0] = new Counter(aSource); - counters[1] = new Counter(bSource); - for(Counter c: counters){ - c.start(); - } - } - - public SpeedController getMotor(){ - return motor; - } - - public Encoder getEncoder(){ - return encoder; - } - - public Counter[] getCounters(){ - return counters; - } - - public void reset(){ - motor.set(0); - Timer.delay(TestBench.MOTOR_STOP_TIME); - encoder.reset(); - for(Counter c : counters){ - c.reset(); - } - } - -} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java new file mode 100644 index 0000000000..d597e322c7 --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java @@ -0,0 +1,147 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2014. 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.mockhardware; + +import edu.wpi.first.wpilibj.DigitalOutput; +import edu.wpi.first.wpilibj.Timer; + +/** + * @file FakeCounterSource.java + * Simulates an encoder for testing purposes + * @author Ryan O'Meara + */ +public class FakeCounterSource +{ + + private Thread m_task; + private int m_count; + private int m_mSec; + private DigitalOutput m_output; + + /** + * Thread object that allows emulation of an encoder + */ + private class EncoderThread extends Thread + { + + FakeCounterSource m_encoder; + + EncoderThread(FakeCounterSource encode) + { + m_encoder = encode; + } + + public void run() + { + m_encoder.m_output.set(false); + try + { + for (int i = 0; i < m_encoder.m_count; i++) + { + Thread.sleep(m_encoder.m_mSec); + m_encoder.m_output.set(true); + Thread.sleep(m_encoder.m_mSec); + m_encoder.m_output.set(false); + } + } catch (InterruptedException e) + { + } + } + } + + /** + * Create a fake encoder on a given port + * @param port The port the encoder is supposed to be on + */ + public FakeCounterSource(int port) + { + m_output = new DigitalOutput(port); + initEncoder(); + } + + /** + * Create a new fake encoder on the indicated slot and port + * @param slot Slot to create on + * @param port THe port that the encoder is supposably on + */ + public FakeCounterSource(int slot, int port) + { + m_output = new DigitalOutput(slot, port); + initEncoder(); + } + + /** + * Destroy Object with minimum memory leak + */ + public void free() + { + m_task = null; + m_output.free(); + } + + /** + * Common initailization code + */ + private void initEncoder() + { + m_mSec = 1; + m_task = new EncoderThread(this); + m_output.set(false); + } + + /** + * Starts the thread execution task + */ + public void start() + { + m_task.start(); + } + + /** + * Waits for the thread to complete + */ + public void complete() + { + try + { + m_task.join(); + } catch (InterruptedException e) + { + } + m_task = new EncoderThread(this); + Timer.delay(.5); + } + + /** + * Starts and completes a task set - does not return until thred has finished + * its operations + */ + public void execute() + { + start(); + complete(); + } + + /** + * Sets the count to run encoder + * @param count The count to emulate to the controller + */ + public void setCount(int count) + { + m_count = count; + } + + /** + * Specify the rate to send pulses + * @param mSec The rate to send out pulses at + */ + public void setRate(int mSec) + { + m_mSec = mSec; + } +} + diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java index 9dedb79b3b..b0b5a63f81 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) FIRST 2008-2014. 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. */ diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java index 6cb79bf067..e7adaa7964 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) FIRST 2008-2014. 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. */ @@ -8,13 +8,14 @@ package edu.wpi.first.wpilibj.test; import edu.wpi.first.wpilibj.CANJaguar; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DigitalSource; -import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Gyro; import edu.wpi.first.wpilibj.Jaguar; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.can.CANTimeoutException; -import edu.wpi.first.wpilibj.groups.MotorEncoder; +import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture; +import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture; /** @@ -29,60 +30,109 @@ import edu.wpi.first.wpilibj.groups.MotorEncoder; public final class TestBench { - public static final double MOTOR_STOP_TIME = 0.10; + /** The time that it takes to have a motor go from rotating at full speed to completely stopped */ + public static final double MOTOR_STOP_TIME = 0.50; - public static TestBench instance; + /** The Singleton instance of the Test Bench */ + private static TestBench instance = null; + private CANJaguar canJag = null; //This is declared externally because it does not have a free method - public Talon talon = new Talon(1); - public DigitalInput encA1 = new DigitalInput(1); - public DigitalInput encB1 = new DigitalInput(2); + - public MotorEncoder talonPair = new MotorEncoder(talon, encA1, encB1); - - public Victor vic = new Victor(2); - public DigitalInput encA2 = new DigitalInput(3); - public DigitalInput encB2 = new DigitalInput(4); - public MotorEncoder vicPair = new MotorEncoder(vic, encA2, encB2); - - private Jaguar jag = new Jaguar(3); - public DigitalInput encA3 = new DigitalInput(5); - public DigitalInput encB3 = new DigitalInput(6); - private MotorEncoder jagPair = new MotorEncoder(jag, encA3, encB3); - - //private CANJaguar canJag = null; - //private Encoder enc4 = new Encoder(7, 8); - //private MotorEncoder canPair; - - public TestBench(){ -// try { -// canJag = new CANJaguar(1); -// } catch (CANTimeoutException e) { -// e.printStackTrace(); -// } - //canPair = new MotorEncoder(canJag, enc4); + /** + * The single constructor for the TestBench. + * This method is private in order to prevent multiple TestBench objects from being allocated + */ + private TestBench(){ } - public MotorEncoder getTalonPair(){ + /** + * Constructs a new set of objects representing a connected set of Talon controlled Motors and an encoder + * + * @return a freshly allocated Talon, Encoder pair + */ + public MotorEncoderFixture getTalonPair(){ + Talon talon = new Talon(1); + DigitalInput encA1 = new DigitalInput(1); + DigitalInput encB1 = new DigitalInput(2); + + MotorEncoderFixture talonPair = new MotorEncoderFixture(talon, encA1, encB1); return talonPair; } - public MotorEncoder getVictorPair(){ + /** + * Constructs a new set of objects representing a connected set of Victor controlled Motors and an encoder + * + * @return a freshly allocated Victor, Encoder pair + */ + public MotorEncoderFixture getVictorPair(){ + Victor vic = new Victor(2); + DigitalInput encA2 = new DigitalInput(3); + DigitalInput encB2 = new DigitalInput(4); + MotorEncoderFixture vicPair = new MotorEncoderFixture(vic, encA2, encB2); return vicPair; } - public MotorEncoder getJaguarPair(){ + /** + * Constructs a new set of objects representing a connected set of Jaguar controlled Motors and an encoder + * + * @return a freshly allocated Jaguar, Encoder pair + */ + public MotorEncoderFixture getJaguarPair(){ + Jaguar jag = new Jaguar(3); + DigitalInput encA3 = new DigitalInput(5); + DigitalInput encB3 = new DigitalInput(6); + MotorEncoderFixture jagPair = new MotorEncoderFixture(jag, encA3, encB3); return jagPair; } -// public MotorEncoder getCanJaguarPair(){ -// return canPair; -// } - + /** + * Constructs a new set of objects representing a connected set of CANJaguar controlled Motors and an encoder
+ * Note: The CANJaguar is not freshly allocated because the CANJaguar lacks a free() method + * @return an existing CANJaguar and a freshly allocated Encoder + */ + public MotorEncoderFixture getCanJaguarPair(){ + + DigitalInput encA4 = new DigitalInput(7); + DigitalInput encB4 = new DigitalInput(8); + MotorEncoderFixture canPair; + if(canJag == null){ //Again this is because the CanJaguar does not have a free method + try { + canJag = new CANJaguar(1); + } catch (CANTimeoutException e) { + e.printStackTrace(); + } + } + canPair = new MotorEncoderFixture(canJag, encA4, encB4); + assert canPair != null; + return canPair; + } + + /** + * Constructs a new set of two Servo's and a Gyroscope. + * @return a freshly allocated Servo's and a freshly allocated Gyroscope + */ + public TiltPanCameraFixture getTiltPanCam(){ + Gyro gyro = new Gyro(1); + gyro.setSensitivity(.007); //If a different gyroscope is used this value will be different + + Servo tilt = new Servo(10); + Servo pan = new Servo(9); + + TiltPanCameraFixture tpcam = new TiltPanCameraFixture(tilt, pan, gyro); + + return tpcam; + } + /** + * Gets the singleton of the TestBench. If the TestBench is not already allocated in constructs an new instance of it. + * Otherwise it returns the existing instance. + * @return The Singleton instance of the TestBench + */ public static TestBench getInstance() { if (instance == null) { instance = new TestBench(); diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java index bda0b01950..58faac8833 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java @@ -1,3 +1,9 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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.test; import org.junit.runner.JUnitCore; @@ -5,9 +11,9 @@ import org.junit.runner.RunWith; import org.junit.runners.Suite; import org.junit.runners.Suite.SuiteClasses; - import edu.wpi.first.wpilibj.MotorEncoderTest; import edu.wpi.first.wpilibj.SampleTest; +import edu.wpi.first.wpilibj.TiltPanCameraTest; import edu.wpi.first.wpilibj.TimerTest; /** @@ -17,7 +23,7 @@ import edu.wpi.first.wpilibj.TimerTest; * suite classes annotation. */ @RunWith(Suite.class) -@SuiteClasses({ SampleTest.class, MotorEncoderTest.class, TimerTest.class }) +@SuiteClasses({ SampleTest.class, TiltPanCameraTest.class, MotorEncoderTest.class, TimerTest.class }) public class TestSuite { public static void main(String[] args) {