uploading to add to 2020_template

src/main/java/com/robototes/motors/PIDCanSparkMax.java

@@ -62,7 +62,7 @@ public void setSpeed(double speed) {
 
 	@Override
 	public double usePIDOutput() {
-		return usePIDOutput(new Time(5, TimeUnits.MILLISECOND)); // loop time of robot
+		return usePIDOutput(new Time(50, TimeUnits.MILLISECOND)); // loop time of robot
 	}
 
 	@Override

src/main/java/com/robototes/utils/DebugPrint.java

@@ -0,0 +1,21 @@
+package com.robototes.utils;
+
+public class DebugPrint {
+	public static boolean DEBUG_MODE = false;
+
+	public static void setDebugMode(boolean newMode) {
+		DEBUG_MODE = newMode;
+	}
+
+	public static <T> void debugPrint(T print) {
+		if (DEBUG_MODE) {
+			System.out.println(print);
+		}
+	}
+
+	public static void debugPrintf(String print, Object... objects) {
+		if (DEBUG_MODE) {
+			System.out.printf(print, objects);
+		}
+	}
+}

src/test/java/com/robototes/helpers/MockButton.java

@@ -0,0 +1,22 @@
+package com.robototes.helpers;
+
+import edu.wpi.first.wpilibj.buttons.Button;
+
+public class MockButton extends Button {
+
+	private boolean pushed = false;
+
+	@Override
+	public boolean get() {
+		// TODO Auto-generated method stub
+		return pushed;
+	}
+
+	public void push() {
+		pushed = true;
+	}
+
+	public void release() {
+		pushed = false;
+	}
+}

src/test/java/com/robototes/helpers/MockHardwareExtension.java

@@ -0,0 +1,30 @@
+package com.robototes.helpers;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * JUnit 5 testing extension which ensures all WPILib foundational bits are
+ * initialized to be able to run the scheduler.
+ */
+public final class MockHardwareExtension {
+
+	public static void beforeAll() {
+		initializeHardware();
+	}
+
+	public static void afterAll() {
+		DriverStation.getInstance().release();
+		HAL.releaseDSMutex();
+	}
+
+	private static void initializeHardware() {
+		HAL.initialize(500, 0);
+		DriverStationSim dsSim = new DriverStationSim();
+		dsSim.setDsAttached(true);
+		dsSim.setAutonomous(false);
+		dsSim.setEnabled(true);
+		dsSim.setTest(true);
+	}
+}

src/test/java/com/robototes/helpers/SchedulerPumpHelper.java

@@ -0,0 +1,65 @@
+package com.robototes.helpers;
+
+import java.util.concurrent.TimeUnit;
+
+import edu.wpi.first.wpilibj.command.Scheduler;
+
+/**
+ * Integration test helper class that will run the command scheduler. Use of
+ * this class requires that your test class be extended from
+ * {@link TestWithScheduler}.
+ */
+public final class SchedulerPumpHelper {
+	private static int defaultHeartbeatInMs = 20;
+
+	/**
+	 * Static class. Do not initialize.
+	 */
+	private SchedulerPumpHelper() {
+	}
+
+	/**
+	 * Change the default heartbeat for the scheduler pump.
+	 * 
+	 * @param defaultHeartbeatInMs Heartbeat in milliseconds
+	 */
+	public static void setDefaultHeartbeat(int defaultHeartbeatInMs) {
+		SchedulerPumpHelper.defaultHeartbeatInMs = defaultHeartbeatInMs;
+	}
+
+	/**
+	 * Helper to figure out what heartbeat to use.
+	 * 
+	 * @param optionalHeartbeatInMs Optional heartbeat in array form to simulate
+	 *                              optional parameters
+	 * @return The heartbeat to use
+	 */
+	private static int getHeartbeatToUse(int[] optionalHeartbeatInMs) {
+		if (optionalHeartbeatInMs.length > 1) {
+			throw new IllegalArgumentException("There can be only one optional heartbeat parameter.");
+		}
+		return optionalHeartbeatInMs.length > 0 ? optionalHeartbeatInMs[0] : defaultHeartbeatInMs;
+	}
+
+	/**
+	 * Run the command scheduler every heartbeatInMs for a durationInMs amount of
+	 * time. Calls will be serialized as the Scheduler is not threadsafe, so beware
+	 * of deadlocks. As of this writing, parallel testing is NOT the default mode
+	 * for JUnit. So if you have not decorated your tests to run in parallel, you
+	 * are fine.
+	 * 
+	 * @param durationInMs          Duration to run in milliseconds
+	 * @param optionalHeartbeatInMs Optional pump time in milliseconds. If omitted,
+	 *                              20ms default unless changed.
+	 * @throws InterruptedException Thrown if sleeping interrupted
+	 */
+	public static synchronized void runForDuration(long durationInMs, int... optionalHeartbeatInMs)
+			throws InterruptedException {
+		int heartbeatToUseInMs = getHeartbeatToUse(optionalHeartbeatInMs);
+		long start = System.nanoTime();
+		while (System.nanoTime() < (start + TimeUnit.MILLISECONDS.toNanos(durationInMs))) {
+			Scheduler.getInstance().run();
+			Thread.sleep(heartbeatToUseInMs);
+		}
+	}
+}

src/test/java/com/robototes/helpers/TestWithScheduler.java

@@ -0,0 +1,26 @@
+package com.robototes.helpers;
+
+import edu.wpi.first.wpilibj.command.Scheduler;
+
+/**
+ * Extend this class when your test requires commands or command groups to be
+ * exercised with the full WPI scheduler. Use
+ * {@link SchedulerPumpHelper#runForDuration(int, int...)} to pump the
+ * scheduler.
+ */
+public class TestWithScheduler {
+
+	public static void schedulerStart() {
+		Scheduler.getInstance().removeAll();
+		Scheduler.getInstance().enable();
+	}
+
+	public static void schedulerClear() {
+		Scheduler.getInstance().removeAll();
+	}
+
+	public static void schedulerDestroy() {
+		Scheduler.getInstance().disable();
+		Scheduler.getInstance().close();
+	}
+}

src/test/java/com/robototes/motors/PIDControlTest.java

@@ -0,0 +1,60 @@
+package com.robototes.motors;
+
+import static org.junit.Assert.*;
+
+import static org.mockito.Mockito.*;
+
+import org.junit.Before;
+import org.junit.Test;
+import com.robototes.helpers.MockHardwareExtension;
+
+import com.robototes.PIDControls.PIDConstants;
+import com.robototes.control.DistanceSubsystem;
+import com.robototes.helpers.TestWithScheduler;
+import com.robototes.math.MathUtils;
+import com.robototes.motors.MotorRotations.SparkMaxRotations;
+import com.robototes.units.Distance;
+import com.robototes.units.InterUnitRatio;
+import com.robototes.units.UnitTypes.DistanceUnits;
+import com.robototes.units.UnitTypes.RotationUnits;
+
+@SuppressWarnings("unused")
+public class PIDControlTest extends TestWithScheduler {
+
+	static {
+		System.loadLibrary("ntcorejni");
+	}
+
+	@Before
+	public void setupTest() {
+		MockHardwareExtension.beforeAll();
+		schedulerStart();
+	}
+
+	@Test
+	public void testPIDMotor() {
+		PIDConstants constants = new PIDConstants(1);
+		PIDMotor<SparkMaxRotations> motor = new PIDMotor<SparkMaxRotations>(new SparkMaxRotations(0), constants);
+
+		double testMotorSetSpeed = 1;
+		motor.setSpeed(testMotorSetSpeed);
+		assertEquals("Motor has correct speed", testMotorSetSpeed, motor.getSpeed(), MathUtils.EPSILON);
+	}
+
+	@Test
+	public void testPIDDistanceSubsystem() {
+
+		InterUnitRatio<RotationUnits, DistanceUnits> distanceSubsystemRatio = new InterUnitRatio<RotationUnits, DistanceUnits>(
+				RotationUnits.ROTATION, 1, DistanceUnits.METER);
+
+		PIDConstants constants = new PIDConstants(1);
+
+		PIDMotor<?>[] motors = new PIDMotor[] { new PIDMotor<SparkMaxRotations>(new SparkMaxRotations(0), constants) };
+
+		DistanceSubsystem distanceSubsystem = new DistanceSubsystem(motors, distanceSubsystemRatio);
+
+//		distanceSubsystem.setReference(new Distance(1)); //Currently broken, skipping for now
+
+	}
+
+}

src/test/java/com/robototes/motors/PIDMotor.java

@@ -39,7 +39,7 @@ public void setSpeed(double speed) {
 
 	@Override
 	public double usePIDOutput() {
-		return usePIDOutput(new Time(20, TimeUnits.MILLISECOND));
+		return usePIDOutput(new Time(50, TimeUnits.MILLISECOND));
 	}
 
 	@Override