Skip to content
This repository was archived by the owner on Sep 14, 2019. It is now read-only.

Commit 5c9444d

Browse files
MysteryPigMysteryPig
authored
Merge pull request #1 from corvinb/corvin-refactor-for-AutoMoveTo-unit-tests
JUnit Test for AutoMoveTo - * Defined SmartDashboardInterface to be able to mock calls to the sma…
2 parents cc0d6d5 + f985006 commit 5c9444d

10 files changed

Lines changed: 197 additions & 30 deletions

File tree

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -52,9 +52,9 @@ public OI() {
5252
shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
5353
shiftDriveTypeButton.whenPressed(new ShiftDriveType());
5454
PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
55-
PIDMoveButton.whenPressed(new PIDMove(40, Robot.dt, RobotMap.distEncAvg));
55+
PIDMoveButton.whenPressed(new PIDMove(40, Robot.dt, Robot.sd, RobotMap.distEncAvg));
5656
PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
57-
PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro));
57+
PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro, Robot.sd));
5858

5959
rightJoy = new Joystick(1);
6060
updatePIDConstantsButton = new JoystickButton(rightJoy, getButton("Get PID Constants", 8));

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java‎

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -48,12 +48,18 @@ public class Robot extends TimedRobot {
4848
SendableChooser<Position> posChooser = new SendableChooser<Position>();
4949
Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
5050
String[] fmsPossibilities = {"LL", "LR", "RL", "RR"};
51+
52+
public static SmartDashboardInterface sd = new SmartDashboardInterface() {
53+
public double getConst(String key, double def) {
54+
if (!SmartDashboard.containsKey("Const/" + key)) {
55+
SmartDashboard.putNumber("Const/" + key, def);
56+
}
57+
return SmartDashboard.getNumber("Const/" + key, def);
58+
}
59+
};
5160

5261
public static double getConst(String key, double def) {
53-
if (!SmartDashboard.containsKey("Const/" + key)) {
54-
SmartDashboard.putNumber("Const/" + key, def);
55-
}
56-
return SmartDashboard.getNumber("Const/" + key, def);
62+
return sd.getConst(key, def);
5763
}
5864

5965
public static boolean getBool(String key, boolean def) {

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
package org.usfirst.frc.team199.Robot2018;
2+
3+
public interface SmartDashboardInterface {
4+
public double getConst(String key, double def);
5+
}

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java‎

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,11 @@
11
package org.usfirst.frc.team199.Robot2018.commands;
22

3-
import org.usfirst.frc.team199.Robot2018.Robot;
3+
import edu.wpi.first.wpilibj.PIDSource;
4+
5+
import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
46
import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
57
import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
8+
import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
69

710
//import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain;
811

@@ -13,24 +16,25 @@
1316
*/
1417
public class AutoMoveTo extends CommandGroup {
1518

16-
public AutoMoveTo(String[] args) {
19+
public AutoMoveTo(String[] args, DrivetrainInterface dt,
20+
SmartDashboardInterface sd, PIDSource pidMoveSrc) {
1721
//requires(Drivetrain);
1822
double rotation;
19-
double[] point = new double[2];
23+
double[] point = {0,0};
2024
String parentheseless;
2125
String[] pointparts;
2226
for (String arg : args) {
2327
if (AutoUtils.isDouble(arg)) {
2428
rotation = Double.valueOf(arg);
25-
addSequential(new PIDTurn(rotation - AutoUtils.getRot(), Robot.dt, Robot.dt.getGyro()));
29+
addSequential(new PIDTurn(rotation - AutoUtils.getRot(), dt, dt.getGyro(), sd));
2630
AutoUtils.setRot(rotation);
2731
} else if (AutoUtils.isPoint(arg)) {
2832
parentheseless = arg.substring(1, arg.length() - 1);
2933
pointparts = parentheseless.split(",");
3034
point[0] = Double.parseDouble(pointparts[0]);
3135
point[1] = Double.parseDouble(pointparts[1]);
32-
addSequential(new PIDTurn(Math.toDegrees(Math.atan((point[0] - AutoUtils.getX())/(point[1] - AutoUtils.getY())) - AutoUtils.getRot()), Robot.dt, Robot.dt.getGyro()));
33-
addSequential(new PIDMove(Math.sqrt(((point[0] - AutoUtils.getX()) * (point[0] - AutoUtils.getX()) + ((point[1] - AutoUtils.getY()) * (point[1] - AutoUtils.getY())))), Robot.dt, new PIDSourceAverage(null, null)));
36+
addSequential(new PIDTurn(Math.toDegrees(Math.atan((point[0] - AutoUtils.getX())/(point[1] - AutoUtils.getY())) - AutoUtils.getRot()), dt, dt.getGyro(), sd));
37+
addSequential(new PIDMove(Math.sqrt(((point[0] - AutoUtils.getX()) * (point[0] - AutoUtils.getX()) + ((point[1] - AutoUtils.getY()) * (point[1] - AutoUtils.getY())))), dt, sd, pidMoveSrc));
3438
AutoUtils.setX(point[0]);
3539
AutoUtils.setY(point[1]);
3640
AutoUtils.setRot(Math.toDegrees(Math.atan((point[0] - AutoUtils.getX())/(point[1] - AutoUtils.getY()))));

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java‎

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,12 @@
11
package org.usfirst.frc.team199.Robot2018.commands;
22

33
import org.usfirst.frc.team199.Robot2018.Robot;
4+
import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
45
import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
56
import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
67

78
import edu.wpi.first.wpilibj.PIDController;
9+
import edu.wpi.first.wpilibj.PIDSource;
810
import edu.wpi.first.wpilibj.PIDOutput;
911
import edu.wpi.first.wpilibj.command.Command;
1012

@@ -17,15 +19,16 @@ public class PIDMove extends Command implements PIDOutput {
1719
private DrivetrainInterface dt;
1820
private PIDController moveController;
1921

20-
public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
22+
public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
2123
// Use requires() here to declare subsystem dependencies
2224
// eg. requires(chassis);
2325
target = targ;
2426
this.dt = dt;
25-
requires(Robot.dt);
26-
double kf = 1 / (dt.getCurrentMaxSpeed() * Robot.getConst("Default PID Update Time", 0.05));
27-
moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0),
28-
Robot.getConst("MovekD", 0), kf, avg, this);
27+
if (Robot.dt != null) {
28+
requires(Robot.dt);
29+
}
30+
moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0),
31+
sd.getConst("MovekD", 0), avg, this);
2932
}
3033

3134
// Called just before this Command runs the first time

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java‎

Lines changed: 8 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,11 @@
11
package org.usfirst.frc.team199.Robot2018.commands;
22

33
import org.usfirst.frc.team199.Robot2018.Robot;
4+
import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
45
import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
56

6-
import com.kauailabs.navx.frc.AHRS;
7-
87
import edu.wpi.first.wpilibj.PIDController;
8+
import edu.wpi.first.wpilibj.PIDSource;
99
import edu.wpi.first.wpilibj.PIDOutput;
1010
import edu.wpi.first.wpilibj.command.Command;
1111

@@ -18,19 +18,16 @@ public class PIDTurn extends Command implements PIDOutput {
1818
DrivetrainInterface dt;
1919
private PIDController turnController;
2020

21-
public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) {
21+
public PIDTurn(double targ, DrivetrainInterface dt, PIDSource ahrs, SmartDashboardInterface sd) {
2222
// Use requires() here to declare subsystem dependencies
2323
// eg. requires(chassis);
2424
target = targ;
2525
this.dt = dt;
26-
requires(Robot.dt);
27-
// calculates the maximum turning speed in degrees/sec based on the max linear
28-
// speed in inches/s and the distance (inches) between sides of the DT
29-
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360
30-
/ (Math.PI * Robot.getConst("Distance Between Wheels", 26.25));
31-
double kf = 1 / (maxTurnSpeed * Robot.getConst("Default PID Update Time", 0.05));
32-
turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0),
33-
Robot.getConst("TurnkD", 0), kf, ahrs, this);
26+
if (Robot.dt != null) {
27+
requires(Robot.dt);
28+
}
29+
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0),
30+
sd.getConst("TurnkD", 0), ahrs, this);
3431
}
3532

3633
// Called just before this Command runs the first time

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,9 +36,15 @@ public RunScript(String scriptName) {
3636

3737
switch (cmdName) {
3838
case "moveto":
39-
addSequential(new AutoMoveTo(cmdArgs.split(" ")));
39+
addSequential(new AutoMoveTo(cmdArgs.split(" "), Robot.dt, Robot.sd, new PIDSourceAverage(null, null)));
4040
break;
4141
case "turn":
42+
/**<<<<<<< corvin-refactor-for-AutoMoveTo-unit-tests This is commented out so I can come back after testing.
43+
addSequential(new PIDTurn(Double.parseDouble(cmdArgs), Robot.dt, Robot.dt.getGyro(), Robot.sd));
44+
break;
45+
case "move":
46+
addSequential(new PIDMove(Double.parseDouble(cmdArgs), Robot.dt, Robot.sd, new PIDSourceAverage(null, null)));
47+
=======**/
4248
addSequential(new PIDTurn(Double.parseDouble(cmdArgs), Robot.dt, Robot.dt.getGyro()));
4349
AutoUtils.setRot(rotation);
4450
break;

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
import edu.wpi.first.wpilibj.DoubleSolenoid;
1919
import edu.wpi.first.wpilibj.Encoder;
2020
import edu.wpi.first.wpilibj.SpeedControllerGroup;
21+
import edu.wpi.first.wpilibj.PIDSource;
2122
import edu.wpi.first.wpilibj.command.Subsystem;
2223
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
2324

@@ -232,11 +233,12 @@ public void shiftGears(boolean highGear) {
232233
public void shiftGearSolenoidOff() {
233234
dtGear.set(DoubleSolenoid.Value.kOff);
234235
}
236+
235237
/**
236238
* Returns the gyroscope
237239
* @return the gyroscope
238240
*/
239-
public AHRS getGyro() {
241+
public PIDSource getGyro() {
240242
return fancyGyro;
241243
}
242244
/**

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package org.usfirst.frc.team199.Robot2018.subsystems;
22

3+
import edu.wpi.first.wpilibj.PIDSource;
4+
35
public interface DrivetrainInterface {
46

57
public void initDefaultCommand();
@@ -126,6 +128,11 @@ public interface DrivetrainInterface {
126128
public void shiftGearSolenoidOff();
127129

128130
/**
131+
* Returns the gyroscope
132+
* @return the gyroscope
133+
*/
134+
public PIDSource getGyro();
135+
/**
129136
* Reset the kf constants for both VelocityPIDControllers based on current DT
130137
* gearing (high or low gear).
131138
*

‎Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java‎

Lines changed: 137 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
package org.usfirst.frc.team199.Robot2018;
2+
3+
import static org.junit.jupiter.api.Assertions.*;
4+
5+
import org.junit.jupiter.api.BeforeEach;
6+
import org.junit.jupiter.api.Test;
7+
8+
import static org.mockito.Mockito.*;
9+
10+
import edu.wpi.first.wpilibj.command.Command;
11+
import edu.wpi.first.wpilibj.command.CommandGroup;
12+
import edu.wpi.first.wpilibj.command.Subsystem;
13+
import edu.wpi.first.wpilibj.internal.HardwareTimer;
14+
import edu.wpi.first.wpilibj.HLUsageReporting;
15+
import edu.wpi.first.wpilibj.PIDController;
16+
import edu.wpi.first.wpilibj.PIDSource;
17+
import edu.wpi.first.wpilibj.PIDSourceType;
18+
import edu.wpi.first.wpilibj.Timer;
19+
import edu.wpi.first.wpilibj.PIDOutput;
20+
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
21+
22+
import com.kauailabs.navx.frc.AHRS;
23+
24+
import org.usfirst.frc.team199.Robot2018.commands.Autonomous;
25+
import org.usfirst.frc.team199.Robot2018.commands.TeleopDrive;
26+
import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Position;
27+
import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Strategy;
28+
import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
29+
import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
30+
import org.usfirst.frc.team199.Robot2018.commands.AutoMoveTo;
31+
32+
import java.util.HashMap;
33+
import java.util.Map;
34+
35+
class AutoMoveToTest {
36+
37+
// May need a BeforeEach to reset AutoUtils.
38+
39+
@BeforeEach
40+
void setUp() {
41+
// Since VelocityPIDController extends PIDController and PIDController calls
42+
// static methods in wpilib that only work on robot,
43+
// we setup these mocks to allow the tests to run off robot.
44+
HardwareTimer tim = mock(HardwareTimer.class);
45+
Timer.Interface timerInstance = mock(Timer.Interface.class);
46+
when(tim.newTimer()).thenReturn(timerInstance);
47+
Timer.SetImplementation(tim);
48+
HLUsageReporting.Interface usageReporter = mock(HLUsageReporting.Interface.class);
49+
HLUsageReporting.SetImplementation(usageReporter);
50+
}
51+
52+
@Test
53+
void testWPICommand() {
54+
Command command = new Command() {
55+
protected boolean isFinished() { return false; }
56+
};
57+
assertNotNull(command);
58+
}
59+
60+
@Test
61+
void testWPICommandGroup() {
62+
CommandGroup group = new CommandGroup();
63+
assertNotNull(group);
64+
65+
Command command = new Command() {
66+
protected boolean isFinished() { return false; }
67+
};
68+
group.addSequential(command);
69+
}
70+
71+
@Test
72+
void testPIDController() {
73+
PIDSource source = mock(PIDSource.class);
74+
PIDOutput output = mock(PIDOutput.class);
75+
76+
PIDController ctrl = new PIDController(0, 0, 0, source, output);
77+
assertNotNull(ctrl);
78+
}
79+
80+
//@Test
81+
// Problem instantiating Subsystem because of SendableBase using network tables.
82+
void testWPISubsystem() {
83+
//LiveWindow.setEnabled(false);
84+
Subsystem subsystem = new Subsystem() {
85+
protected void initDefaultCommand() {
86+
setDefaultCommand(new Command() {
87+
protected boolean isFinished() { return false; }
88+
});
89+
}
90+
};
91+
assertNotNull(subsystem);
92+
}
93+
94+
@Test
95+
void testForwardAndRight() {
96+
String[] args = {"(0,12)","(12,12)"};
97+
98+
AutoUtils.setRot(0);
99+
AutoUtils.setX(0);
100+
AutoUtils.setY(0);
101+
102+
PIDSource pidGyroSrc = mock(PIDSource.class);
103+
when(pidGyroSrc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement);
104+
DrivetrainInterface dt = mock(DrivetrainInterface.class);
105+
when(dt.getGyro()).thenReturn(pidGyroSrc);
106+
SmartDashboardInterface sd = mock(SmartDashboardInterface.class);
107+
PIDSource pidMoveSrc = mock(PIDSource.class);
108+
109+
AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc);
110+
111+
assertEquals(90, AutoUtils.getRot());
112+
assertEquals(12, AutoUtils.getX());
113+
assertEquals(12, AutoUtils.getY());
114+
}
115+
116+
@Test
117+
void testForward() {
118+
String[] args = {"(0,12)"};
119+
120+
AutoUtils.setRot(0);
121+
AutoUtils.setX(0);
122+
AutoUtils.setY(0);
123+
124+
PIDSource pidGyroSrc = mock(PIDSource.class);
125+
when(pidGyroSrc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement);
126+
DrivetrainInterface dt = mock(DrivetrainInterface.class);
127+
when(dt.getGyro()).thenReturn(pidGyroSrc);
128+
SmartDashboardInterface sd = mock(SmartDashboardInterface.class);
129+
PIDSource pidMoveSrc = mock(PIDSource.class);
130+
131+
AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc);
132+
133+
assertEquals(0, AutoUtils.getRot());
134+
assertEquals(0, AutoUtils.getX());
135+
assertEquals(12, AutoUtils.getY());
136+
}
137+
}

0 commit comments

Comments
 (0)