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

Commit 8e5407e

Browse files
MysteryPigMysteryPig
committed
Fix merging, reorganize PIDTurn's constructor to match PIDMove's,
pass Corvin's tests, etc.
1 parent 2fd8b6e commit 8e5407e

7 files changed

Lines changed: 144 additions & 118 deletions

File tree

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ public OI() {
5454
PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
5555
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, Robot.sd));
57+
PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, Robot.sd, RobotMap.fancyGyro));
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: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,15 @@ public class Robot extends TimedRobot {
5555
Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
5656
String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
5757

58+
public static SmartDashboardInterface sd = new SmartDashboardInterface() {
59+
public double getConst(String key, double def) {
60+
if (!SmartDashboard.containsKey("Const/" + key)) {
61+
SmartDashboard.putNumber("Const/" + key, def);
62+
}
63+
return SmartDashboard.getNumber("Const/" + key, def);
64+
}
65+
};
66+
5867
public static double getConst(String key, double def) {
5968
return sd.getConst(key, def);
6069
}

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

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

3-
import edu.wpi.first.wpilibj.PIDSource;
4-
53
import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
64
import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
7-
import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
85
import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
96

7+
import edu.wpi.first.wpilibj.PIDSource;
8+
109
//import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain;
1110

1211
import edu.wpi.first.wpilibj.command.CommandGroup;
1312

1413
/**
15-
* The AutoMoveTo command, which makes the robot go through a set of movements and rotations.
14+
* The AutoMoveTo command, which makes the robot go through a set of movements
15+
* and rotations.
1616
*/
1717
public class AutoMoveTo extends CommandGroup {
18-
19-
public AutoMoveTo(String[] args, DrivetrainInterface dt,
20-
SmartDashboardInterface sd, PIDSource pidMoveSrc) {
21-
//requires(Drivetrain);
22-
double rotation;
23-
double[] point = {0,0};
24-
String parentheseless;
25-
String[] pointparts;
26-
for (String arg : args) {
27-
if (AutoUtils.isDouble(arg)) {
28-
rotation = Double.valueOf(arg);
29-
addSequential(new PIDTurn(rotation - AutoUtils.getRot(), dt, dt.getGyro(), sd));
30-
AutoUtils.setRot(rotation);
31-
} else if (AutoUtils.isPoint(arg)) {
32-
parentheseless = arg.substring(1, arg.length() - 1);
33-
pointparts = parentheseless.split(",");
34-
point[0] = Double.parseDouble(pointparts[0]);
35-
point[1] = Double.parseDouble(pointparts[1]);
36-
addSequential(new PIDTurn(Math.toDegrees(Math.atan((point[0] - AutoUtils.getX())/(point[1] - AutoUtils.getY())) - AutoUtils.getRot()),
37-
Robot.dt, Robot.dt.getGyro()));
38-
addSequential(new PIDMove(Math.sqrt(((point[0] - AutoUtils.getX()) * (point[0] - AutoUtils.getX()) +
39-
((point[1] - AutoUtils.getY()) * (point[1] - AutoUtils.getY())))),
40-
Robot.dt, new PIDSourceAverage(null, null)));
41-
AutoUtils.setX(point[0]);
42-
AutoUtils.setY(point[1]);
43-
AutoUtils.setRot(Math.toDegrees(Math.atan((point[0] - AutoUtils.getX())/(point[1] - AutoUtils.getY()))));
44-
} else {
45-
throw new IllegalArgumentException();
46-
}
47-
}
48-
}
18+
19+
public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc) {
20+
// requires(Drivetrain);
21+
double rotation;
22+
double[] point = { 0, 0 };
23+
String parentheseless;
24+
String[] pointparts;
25+
for (String arg : args) {
26+
if (AutoUtils.isDouble(arg)) {
27+
rotation = Double.valueOf(arg);
28+
addSequential(new PIDTurn(rotation - AutoUtils.getRot(), dt, sd, pidMoveSrc));
29+
AutoUtils.setRot(rotation);
30+
} else if (AutoUtils.isPoint(arg)) {
31+
parentheseless = arg.substring(1, arg.length() - 1);
32+
pointparts = parentheseless.split(",");
33+
point[0] = Double.parseDouble(pointparts[0]);
34+
point[1] = Double.parseDouble(pointparts[1]);
35+
addSequential(new PIDTurn(Math.toDegrees(
36+
Math.atan((point[0] - AutoUtils.getX()) / (point[1] - AutoUtils.getY())) - AutoUtils.getRot()),
37+
dt, sd, pidMoveSrc));
38+
addSequential(new PIDMove(
39+
Math.sqrt(((point[0] - AutoUtils.getX()) * (point[0] - AutoUtils.getX())
40+
+ ((point[1] - AutoUtils.getY()) * (point[1] - AutoUtils.getY())))),
41+
dt, sd, pidMoveSrc));
42+
double x = AutoUtils.getX();
43+
double y = AutoUtils.getY();
44+
AutoUtils.setX(point[0]);
45+
AutoUtils.setY(point[1]);
46+
AutoUtils.setRot(Math.toDegrees(Math.atan((point[0] - x) / (point[1] - y))));
47+
} else {
48+
throw new IllegalArgumentException();
49+
}
50+
}
51+
}
4952
}

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

Lines changed: 11 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +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.autonomous.PIDSourceAverage;
4+
import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
55
import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
66

77
import edu.wpi.first.wpilibj.PIDController;
88
import edu.wpi.first.wpilibj.PIDOutput;
9+
import edu.wpi.first.wpilibj.PIDSource;
910
import edu.wpi.first.wpilibj.command.Command;
1011

1112
/**
@@ -32,14 +33,18 @@ public class PIDMove extends Command implements PIDOutput {
3233
* @param avg
3334
* the PIDSourceAverage of the DT's two Encoders
3435
*/
35-
public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
36+
public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
3637
// Use requires() here to declare subsystem dependencies
37-
requires(Robot.dt);
3838
target = targ;
3939
this.dt = dt;
40-
double kf = 1 / (dt.getCurrentMaxSpeed() * Robot.getConst("Default PID Update Time", 0.05));
41-
moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0),
42-
Robot.getConst("MovekD", 0), kf, avg, this);
40+
if (Robot.dt != null) {
41+
requires(Robot.dt);
42+
}
43+
moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
44+
avg, this);
45+
double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
46+
moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
47+
kf, avg, this);
4348
}
4449

4550
/**

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

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +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.subsystems.DrivetrainInterface;
56

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

1212
/**
@@ -33,19 +33,25 @@ public class PIDTurn extends Command implements PIDOutput {
3333
* testing)
3434
* @param ahrs
3535
* the AHRS (gyro)
36+
* @param sd
37+
* the Smart Dashboard reference, or a SmartDashboardInterface for
38+
* testing
3639
*/
37-
public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) {
40+
public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
3841
// Use requires() here to declare subsystem dependencies
39-
requires(Robot.dt);
4042
target = targ;
4143
this.dt = dt;
44+
if (Robot.dt != null) {
45+
requires(Robot.dt);
46+
}
47+
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
48+
ahrs, this);
4249
// calculates the maximum turning speed in degrees/sec based on the max linear
4350
// speed in inches/s and the distance (inches) between sides of the DT
44-
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360
45-
/ (Math.PI * Robot.getConst("Distance Between Wheels", 26.25));
46-
double kf = 1 / (maxTurnSpeed * Robot.getConst("Default PID Update Time", 0.05));
47-
turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0),
48-
Robot.getConst("TurnkD", 0), kf, ahrs, this);
51+
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25));
52+
double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
53+
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
54+
kf, ahrs, this);
4955
}
5056

5157
/**

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

Lines changed: 59 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -14,69 +14,63 @@
1414
*/
1515
public class RunScript extends CommandGroup {
1616

17-
public RunScript(String scriptName) {
18-
ArrayList<String[]> script = Robot.autoScripts.getOrDefault(scriptName, new ArrayList<String[]>());
19-
20-
outerloop:
21-
for(String[] cmd : script) {
22-
String cmdName = cmd[0];
23-
String cmdArgs = cmd[1];
24-
25-
double[] point = new double[2];
26-
double rotation = 0;
27-
String parentheseless;
28-
String[] pointparts;
29-
if (AutoUtils.isDouble(cmdArgs)) {
30-
rotation = Double.valueOf(cmdArgs);
31-
} else if (AutoUtils.isPoint(cmdArgs)) {
32-
parentheseless = cmdArgs.substring(1, cmdArgs.length() - 1);
33-
pointparts = parentheseless.split(",");
34-
point[0] = Double.parseDouble(pointparts[0]);
35-
}
36-
37-
switch (cmdName) {
38-
case "moveto":
39-
addSequential(new AutoMoveTo(cmdArgs.split(" "), Robot.dt, Robot.sd, new PIDSourceAverage(null, null)));
40-
break;
41-
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-
=======**/
48-
addSequential(new PIDTurn(Double.parseDouble(cmdArgs), Robot.dt, Robot.dt.getGyro()));
49-
AutoUtils.setRot(rotation);
50-
break;
51-
case "move":
52-
addSequential(new PIDMove(Double.parseDouble(cmdArgs), Robot.dt, new PIDSourceAverage(null, null)));
53-
AutoUtils.setX(point[0]);
54-
AutoUtils.setY(point[1]);
55-
break;
56-
case "switch":
57-
addSequential(new EjectToSwitch());
58-
break;
59-
case "scale":
60-
addSequential(new EjectToScale());
61-
break;
62-
case "exchange":
63-
addSequential(new EjectToExchange());
64-
break;
65-
case "wait":
66-
addSequential(new WaitCommand(Double.parseDouble(cmdArgs)));
67-
break;
68-
case "intake":
69-
addSequential(new IntakeCube());
70-
break;
71-
case "jump":
72-
addSequential(new RunScript(cmdArgs));
73-
break;
74-
case "end":
75-
break outerloop;
76-
default:
77-
// this should never happen since AutoUtils already validates the script.
78-
System.err.println("[ERROR] `" + cmdName + "` is not a valid command name.");
79-
}
80-
}
81-
}
17+
public RunScript(String scriptName) {
18+
ArrayList<String[]> script = Robot.autoScripts.getOrDefault(scriptName, new ArrayList<String[]>());
19+
20+
outerloop: for (String[] cmd : script) {
21+
String cmdName = cmd[0];
22+
String cmdArgs = cmd[1];
23+
24+
double[] point = new double[2];
25+
double rotation = 0;
26+
String parentheseless;
27+
String[] pointparts;
28+
if (AutoUtils.isDouble(cmdArgs)) {
29+
rotation = Double.valueOf(cmdArgs);
30+
} else if (AutoUtils.isPoint(cmdArgs)) {
31+
parentheseless = cmdArgs.substring(1, cmdArgs.length() - 1);
32+
pointparts = parentheseless.split(",");
33+
point[0] = Double.parseDouble(pointparts[0]);
34+
}
35+
36+
switch (cmdName) {
37+
case "moveto":
38+
addSequential(new AutoMoveTo(cmdArgs.split(" "), Robot.dt, Robot.sd, new PIDSourceAverage(null, null)));
39+
break;
40+
case "turn":
41+
addSequential(new PIDTurn(Double.parseDouble(cmdArgs), Robot.dt, Robot.sd, Robot.dt.getGyro()));
42+
AutoUtils.setRot(rotation);
43+
break;
44+
case "move":
45+
addSequential(
46+
new PIDMove(Double.parseDouble(cmdArgs), Robot.dt, Robot.sd, new PIDSourceAverage(null, null)));
47+
AutoUtils.setX(point[0]);
48+
AutoUtils.setY(point[1]);
49+
break;
50+
case "switch":
51+
addSequential(new EjectToSwitch());
52+
break;
53+
case "scale":
54+
addSequential(new EjectToScale());
55+
break;
56+
case "exchange":
57+
addSequential(new EjectToExchange());
58+
break;
59+
case "wait":
60+
addSequential(new WaitCommand(Double.parseDouble(cmdArgs)));
61+
break;
62+
case "intake":
63+
addSequential(new IntakeCube());
64+
break;
65+
case "jump":
66+
addSequential(new RunScript(cmdArgs));
67+
break;
68+
case "end":
69+
break outerloop;
70+
default:
71+
// this should never happen since AutoUtils already validates the script.
72+
System.err.println("[ERROR] `" + cmdName + "` is not a valid command name.");
73+
}
74+
}
75+
}
8276
}

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

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,8 @@
1717

1818
import edu.wpi.first.wpilibj.DoubleSolenoid;
1919
import edu.wpi.first.wpilibj.Encoder;
20-
import edu.wpi.first.wpilibj.SpeedControllerGroup;
2120
import edu.wpi.first.wpilibj.PIDSource;
21+
import edu.wpi.first.wpilibj.SpeedControllerGroup;
2222
import edu.wpi.first.wpilibj.command.Subsystem;
2323
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
2424

@@ -233,13 +233,14 @@ public void shiftGears(boolean highGear) {
233233
public void shiftGearSolenoidOff() {
234234
dtGear.set(DoubleSolenoid.Value.kOff);
235235
}
236-
236+
237237
/**
238238
* @return the gyroscope
239239
*/
240240
public PIDSource getGyro() {
241241
return fancyGyro;
242242
}
243+
243244
/**
244245
* Reset the kf constants for both VelocityPIDControllers based on current DT
245246
* gearing (high or low gear).
@@ -270,4 +271,12 @@ public double getCurrentMaxSpeed() {
270271
return Robot.getConst("Max Low Speed", 84);
271272
}
272273
}
274+
275+
/**
276+
* @return some average having to do with encoder distance (it's still
277+
* undocumented (ask laura))
278+
*/
279+
public PIDSourceAverage getDistEncAvg() {
280+
return distEncAvg;
281+
}
273282
}

0 commit comments

Comments
 (0)