velocityPIDController class(es) and attempted tests

Author: lhmcgann

Robot2018/.classpath

@@ -13,5 +13,7 @@
 	<classpathentry kind="lib" path="lib/mockito-core-2.13.3.jar"/>
 	<classpathentry kind="lib" path="lib/byte-buddy-1.7.9.jar"/>
 	<classpathentry kind="lib" path="lib/objenesis-2.6.jar"/>
+	<classpathentry kind="lib" path="lib/CTRE_Phoenix-sources.jar"/>
+	<classpathentry kind="lib" path="lib/CTRE_Phoenix.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

Robot2018/lib/CTRE_Phoenix-sources.jar

@@ -0,0 +1,57 @@
+package org.usfirst.frc.team199.Robot2018.subsystems;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.SpeedController;
+
+public class TalonVelocityController implements SpeedController {
+
+	private WPI_TalonSRX talon;
+
+	public TalonVelocityController(WPI_TalonSRX output) {
+		talon = output;
+	}
+
+	@Override
+	public void pidWrite(double output) {
+		// TODO Auto-generated method stub
+		set(output);
+	}
+
+	@Override
+	public void set(double speed) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public double get() {
+		// TODO Auto-generated method stub
+		return 0;
+	}
+
+	@Override
+	public void setInverted(boolean isInverted) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean getInverted() {
+		// TODO Auto-generated method stub
+		return false;
+	}
+
+	@Override
+	public void disable() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopMotor() {
+		// TODO Auto-generated method stub
+
+	}
+
+}

Robot2018/lib/CTRE_Phoenix.jar

@@ -0,0 +1,58 @@
+package org.usfirst.frc.team199.Robot2018.subsystems;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.SpeedController;
+
+public class VelocityPIDController extends PIDController implements SpeedController {
+
+	private SpeedController out;
+
+	/**
+	 * @param kp the proportional PID constant
+	 * @param ki the integral PID constant
+	 * @param kd the derivative PID constant
+	 * @param source
+	 *            the SpeedController you are reading from
+	 * @param output
+	 *            the SpeedController you are telling what to do
+	 */
+	public VelocityPIDController(double kp, double ki, double kd, PIDSource source, SpeedController output) {
+		super(kp, ki, kd, source, output);
+		out = output;
+	}
+
+	@Override
+	public void pidWrite(double output) {
+		setSetpoint(output);
+	}
+
+	@Override
+	public void set(double speed) {
+		// TODO Auto-generated method stub
+		setSetpoint(speed);
+	}
+
+	@Override
+	public double get() {
+		// TODO Auto-generated method stub
+		// should possibly be actual spdCtr value instead???
+		return out.get();
+	}
+
+	@Override
+	public void setInverted(boolean isInverted) {
+		out.setInverted(isInverted);
+	}
+
+	@Override
+	public boolean getInverted() {
+		return out.getInverted();
+	}
+
+	@Override
+	public void stopMotor() {
+		// TODO Auto-generated method stub
+		out.stopMotor();
+	}
+}

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/TalonVelocityController.java

@@ -0,0 +1,60 @@
+package org.usfirst.frc.team199.Robot2018;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import org.junit.jupiter.api.Test;
+import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.internal.HardwareTimer;
+
+class VelocityPIDControllerTest {
+
+	@Test
+	void test1() {
+		HardwareTimer tim = new HardwareTimer();
+		Timer.SetImplementation(tim);
+		PIDSource source = mock(PIDSource.class);
+		SpeedController out = mock(SpeedController.class);
+		double p = 1;
+		double i = 0.5;
+		double d = 0.0037;
+		VelocityPIDController vPID = new VelocityPIDController(p, i, d, source, out);
+
+		when(out.get()).thenReturn(6.5);
+		assertEquals(6.5, vPID.get());
+		verify(out).get();
+	}
+
+	@Test
+	void test2() {
+		PIDSource source = mock(PIDSource.class);
+		SpeedController out = mock(SpeedController.class);
+		double p = 1;
+		double i = 0.5;
+		double d = 0.0037;
+		VelocityPIDController vPID = new VelocityPIDController(p, i, d, source, out);
+
+		vPID.pidWrite(20);
+		assertEquals(vPID.getSetpoint(), 20);
+	}
+
+	@Test
+	void test3() {
+		PIDSource source = mock(PIDSource.class);
+		SpeedController out = mock(SpeedController.class);
+		double p = 1;
+		double i = 0.5;
+		double d = 0.0037;
+		VelocityPIDController vPID = new VelocityPIDController(p, i, d, source, out);
+
+		vPID.set(20);
+		assertEquals(vPID.getSetpoint(), 20);
+	}
+
+}