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

Commit 99d8b52

Browse files
kevinzwangkevinzwang
committed
remove redundant code in PID commands
1 parent 14ef3e5 commit 99d8b52

3 files changed

Lines changed: 102 additions & 101 deletions

File tree

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -109,21 +109,21 @@ public static boolean isValidCommand(String instruction, String args, int lineNu
109109
// moveto takes in a set of points, and the last arg can be a number
110110
if (instruction.equals("moveto")) {
111111
if (args == "") {
112-
logWarning(lineNumber, "The command `moveto` requires at least one argument.");
112+
logError(lineNumber, "The command `moveto` requires at least one argument.");
113113
return false;
114114
}
115115

116116
String[] splitArgs = args.split(" ");
117117
for (int i = 0; i < splitArgs.length - 1; i++) {
118118
if (!isPoint(splitArgs[i])) {
119-
logWarning(lineNumber,
119+
logError(lineNumber,
120120
"The arguments for command `moveto` should be points formatted like this: " + "`(x,y)`.");
121121
return false;
122122
}
123123
}
124124

125125
if (!isDouble(splitArgs[splitArgs.length - 1]) && !isPoint(splitArgs[splitArgs.length - 1])) {
126-
logWarning(lineNumber, "The last argument for command `moveto` should be a number, or a point "
126+
logError(lineNumber, "The last argument for command `moveto` should be a number, or a point "
127127
+ "formatted like this: `(x,y)`.");
128128
return false;
129129
}
@@ -132,12 +132,12 @@ public static boolean isValidCommand(String instruction, String args, int lineNu
132132
// turn can take a number or point
133133
else if (instruction.equals("turn")) {
134134
if (args.contains(" ")) {
135-
logWarning(lineNumber, "Command `turn` only accepts one argument.");
135+
logError(lineNumber, "Command `turn` only accepts one argument.");
136136
return false;
137137
}
138138

139139
if (!isDouble(args) && !isPoint(args)) {
140-
logWarning(lineNumber, "The argument for command `turn` should be a number or a point formatted like "
140+
logError(lineNumber, "The argument for command `turn` should be a number or a point formatted like "
141141
+ "this: `(x,y)`.");
142142
return false;
143143
}
@@ -146,12 +146,12 @@ else if (instruction.equals("turn")) {
146146
// move and wait can take only a number
147147
else if (instruction.equals("move") || instruction.equals("wait")) {
148148
if (args.contains(" ")) {
149-
logWarning(lineNumber, "Command `move` only accepts one argument.");
149+
logError(lineNumber, "Command `move` only accepts one argument.");
150150
return false;
151151
}
152152

153153
if (!isDouble(args)) {
154-
logWarning(lineNumber, "The argument for command `move` should be a number.");
154+
logError(lineNumber, "The argument for command `move` should be a number.");
155155
return false;
156156
}
157157
}
@@ -160,22 +160,22 @@ else if (instruction.equals("move") || instruction.equals("wait")) {
160160
else if (instruction.equals("switch") || instruction.equals("scale") || instruction.equals("exchange")
161161
|| instruction.equals("intake") || instruction.equals("end")) {
162162
if (!args.equals("")) {
163-
logWarning(lineNumber, "Command `" + instruction + "` does not accept any arguments.");
163+
logError(lineNumber, "Command `" + instruction + "` does not accept any arguments.");
164164
return false;
165165
}
166166
}
167167

168168
// Jump only takes one argument
169169
else if (instruction.equals("jump")) {
170170
if (args.contains(" ")) {
171-
logWarning(lineNumber, "Command `jump` only accepts one argument.");
171+
logError(lineNumber, "Command `jump` only accepts one argument.");
172172
return false;
173173
}
174174
}
175175

176176
// if it's not even a valid instruction
177177
else {
178-
logWarning(lineNumber, "`" + instruction + "` is not a valid command.");
178+
logError(lineNumber, "`" + instruction + "` is not a valid command.");
179179
return false;
180180
}
181181

@@ -193,7 +193,7 @@ else if (instruction.equals("jump")) {
193193
* the message to log
194194
*/
195195

196-
private static void logWarning(int lineNumber, String message) {
196+
private static void logError(int lineNumber, String message) {
197197
System.err.println("[ERROR] Line " + lineNumber + ": " + message);
198198
}
199199

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

Lines changed: 43 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -21,28 +21,37 @@ public class PIDMove extends Command implements PIDOutput {
2121
private PIDController moveController;
2222
private PIDSource avg;
2323
private SmartDashboardInterface sd;
24-
private double pointX;
25-
private double pointY;
24+
private double[] point;
25+
private boolean absolute;
2626

2727
/**
2828
* Constructs this command with a new PIDController. Sets all of the
2929
* controller's PID constants based on SD prefs. Sets the controller's PIDSource
3030
* to the encoder average object and sets its PIDOutput to this command which
31-
* implements PIDOutput's pidWrite() method.
31+
* implements PIDOutput's pidWrite() method. Uses either a relative target
32+
* specified with `target` or an absolute point with `point`.
3233
*
33-
* @param targ
34+
* @param target
3435
* the target distance (in inches) to move to
36+
* @param point
37+
* the target point in inches, absolute distance from the starting
38+
* point
3539
* @param dt
3640
* the Drivetrain (for actual code) or a DrivetrainInterface (for
3741
* testing)
3842
* @param sd
3943
* the SmartDashboard
4044
* @param avg
4145
* the PIDSourceAverage of the DT's two Encoders
46+
* @param absolute
47+
* whether to use the absolute variable `point` or the relative
48+
* variable `target`
4249
*/
43-
public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
44-
// Use requires() here to declare subsystem dependencies
45-
target = targ;
50+
public PIDMove(double target, double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg,
51+
boolean absolute) {
52+
this.point = point;
53+
this.target = target;
54+
this.absolute = absolute;
4655
this.dt = dt;
4756
this.avg = avg;
4857
this.sd = sd;
@@ -67,6 +76,26 @@ protected double calculateFeedForward() {
6776
sd.putData("Move PID", moveController);
6877
}
6978

79+
/**
80+
* Constructs this command with a new PIDController. Sets all of the
81+
* controller's PID constants based on SD prefs. Sets the controller's PIDSource
82+
* to the encoder average object and sets its PIDOutput to this command which
83+
* implements PIDOutput's pidWrite() method.
84+
*
85+
* @param target
86+
* the target distance (in inches) to move to
87+
* @param dt
88+
* the Drivetrain (for actual code) or a DrivetrainInterface (for
89+
* testing)
90+
* @param sd
91+
* the SmartDashboard
92+
* @param avg
93+
* the PIDSourceAverage of the DT's two Encoders
94+
*/
95+
public PIDMove(double target, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
96+
this(target, null, dt, sd, avg, false);
97+
}
98+
7099
/**
71100
* Constructs this command with a new PIDController. Sets all of the
72101
* controller's PID constants based on SD prefs. Sets the controller's PIDSource
@@ -85,34 +114,7 @@ protected double calculateFeedForward() {
85114
* the PIDSorceAverage of the DT's two Encoders
86115
*/
87116
public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
88-
pointX = point[0];
89-
pointY = point[1];
90-
91-
this.dt = dt;
92-
this.avg = avg;
93-
this.sd = sd;
94-
if (Robot.dt != null) {
95-
requires(Robot.dt);
96-
}
97-
double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
98-
moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
99-
kf, avg, this);
100-
sd.putData("Move PID", moveController);
101-
moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
102-
sd.getConst("MovekD", 0), kf, avg, this) {
103-
/**
104-
* Move Velocity: V = sqrt(8TGd) / (R*m) where T = max torque of wheels G = gear
105-
* ratio d = distance remaining R = radius of wheels m = mass
106-
*/
107-
@Override
108-
protected double calculateFeedForward() {
109-
double originalFF = super.calculateFeedForward();
110-
double feedForwardConst = dt.getPIDMoveConstant();
111-
double error = getError();
112-
return Math.signum(error) * feedForwardConst * Math.sqrt(Math.abs(error)) + originalFF;
113-
}
114-
};
115-
sd.putData("Move PID", moveController);
117+
this(0, point, dt, sd, avg, true);
116118
}
117119

118120
/**
@@ -121,11 +123,13 @@ protected double calculateFeedForward() {
121123
*/
122124
@Override
123125
public void initialize() {
124-
double dx = pointX - AutoUtils.state.getX();
125-
double dy = pointY - AutoUtils.state.getY();
126+
if (absolute) {
127+
double dx = point[0] - AutoUtils.state.getX();
128+
double dy = point[1] - AutoUtils.state.getY();
126129

127-
double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
128-
this.target = dist;
130+
double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
131+
this.target = dist;
132+
}
129133

130134
dt.resetDistEncs();
131135
moveController.disable();

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

Lines changed: 48 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -25,8 +25,7 @@ public class PIDTurn extends Command implements PIDOutput {
2525
private Timer tim;
2626
private double lastTime;
2727
private SmartDashboardInterface sd;
28-
private double pointX;
29-
private double pointY;
28+
private double[] point;
3029
private boolean turnToPoint;
3130
private boolean absoluteRotation;
3231

@@ -36,9 +35,12 @@ public class PIDTurn extends Command implements PIDOutput {
3635
* to the AHRS (gyro) object and sets its PIDOutput to this command which
3736
* implements PIDOutput's pidWrite() method.
3837
*
39-
* @param targ
38+
* @param target
4039
* the target bearing (in degrees) to turn to (so negative if turning
4140
* left, positive if turning right)
41+
* @param point
42+
* the target point (in inches) to turn to, relative to the starting
43+
* position
4244
* @param dt
4345
* the Drivetrain (for actual code) or a DrivetrainInterface (for
4446
* testing)
@@ -47,25 +49,28 @@ public class PIDTurn extends Command implements PIDOutput {
4749
* @param sd
4850
* the Smart Dashboard reference, or a SmartDashboardInterface for
4951
* testing
50-
* @param absolute
52+
* @param absoluteRotation
5153
* whether the target passed is absolute or relative
54+
* @param turnToPoint
55+
* whether to use the point or the target for turning
5256
*/
53-
public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs, boolean absolute) {
54-
// Use requires() here to declare subsystem dependencies
57+
public PIDTurn(double target, double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs,
58+
boolean absoluteRotation, boolean turnToPoint) {
59+
this.target = target;
60+
this.point = point;
5561
this.dt = dt;
5662
this.ahrs = ahrs;
5763
this.sd = sd;
58-
59-
turnToPoint = false;
60-
target = targ;
61-
absoluteRotation = absolute;
64+
this.turnToPoint = turnToPoint;
65+
this.target = target;
66+
this.absoluteRotation = absoluteRotation;
6267

6368
if (Robot.dt != null) {
6469
requires(Robot.dt);
6570
}
6671
// calculates the maximum turning speed in degrees/sec based on the max linear
6772
// speed in inches/s and the distance (inches) between sides of the DT
68-
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25));
73+
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * getDistanceBetweenWheels());
6974
double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
7075
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
7176
kf, ahrs, this) {
@@ -84,6 +89,7 @@ protected double calculateFeedForward() {
8489
}
8590
};
8691
// tim = new Timer();
92+
sd.putData("Turn PID", turnController);
8793
}
8894

8995
/**
@@ -92,7 +98,7 @@ protected double calculateFeedForward() {
9298
* to the AHRS (gyro) object and sets its PIDOutput to this command which
9399
* implements PIDOutput's pidWrite() method.
94100
*
95-
* @param targ
101+
* @param target
96102
* the target bearing (in degrees) to turn to (so negative if turning
97103
* left, positive if turning right)
98104
* @param dt
@@ -103,9 +109,34 @@ protected double calculateFeedForward() {
103109
* @param sd
104110
* the Smart Dashboard reference, or a SmartDashboardInterface for
105111
* testing
112+
* @param absoluteRotation
113+
* whether the target passed is absolute or relative
106114
*/
107-
public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
108-
this(targ, dt, sd, ahrs, false);
115+
public PIDTurn(double target, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs,
116+
boolean absoluteRotation) {
117+
this(target, null, dt, sd, ahrs, absoluteRotation, false);
118+
}
119+
120+
/**
121+
* Constructs this command with a new PIDController. Sets all of the
122+
* controller's PID constants based on SD prefs. Sets the controller's PIDSource
123+
* to the AHRS (gyro) object and sets its PIDOutput to this command which
124+
* implements PIDOutput's pidWrite() method.
125+
*
126+
* @param target
127+
* the target bearing (in degrees) to turn to (so negative if turning
128+
* left, positive if turning right)
129+
* @param dt
130+
* the Drivetrain (for actual code) or a DrivetrainInterface (for
131+
* testing)
132+
* @param ahrs
133+
* the AHRS (gyro)
134+
* @param sd
135+
* the Smart Dashboard reference, or a SmartDashboardInterface for
136+
* testing
137+
*/
138+
public PIDTurn(double target, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
139+
this(target, dt, sd, ahrs, false);
109140
}
110141

111142
/**
@@ -127,41 +158,7 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
127158
* testing
128159
*/
129160
public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
130-
this.dt = dt;
131-
this.sd = sd;
132-
this.ahrs = ahrs;
133-
this.sd = sd;
134-
135-
pointX = point[0];
136-
pointY = point[1];
137-
138-
turnToPoint = true;
139-
140-
if (Robot.dt != null) {
141-
requires(Robot.dt);
142-
}
143-
// calculates the maximum turning speed in degrees/sec based on the max linear
144-
// speed in inches/s and the distance (inches) between sides of the DT
145-
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * getDistanceBetweenWheels());
146-
double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
147-
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
148-
kf, ahrs, this) {
149-
/**
150-
* Turn Velocity: V = 4r sqrt((T*G*theta) / (R*m)) where r = half of distance
151-
* between wheels T = max torque of wheels G = gear ratio theta = rotational
152-
* distance to end of turn R = radius of wheels m = mass
153-
*/
154-
@Override
155-
protected double calculateFeedForward() {
156-
double originalFF = super.calculateFeedForward();
157-
double feedForwardConst = dt.getPIDTurnConstant();
158-
double error = getError();
159-
return feedForwardConst * (getDistanceBetweenWheels() / 2) * Math.signum(error)
160-
* Math.sqrt(Math.abs(error)) + originalFF;
161-
}
162-
};
163-
// tim = new Timer();
164-
sd.putData("Turn PID", turnController);
161+
this(0, point, dt, sd, ahrs, true, true);
165162
}
166163

167164
/**
@@ -176,8 +173,8 @@ protected void initialize() {
176173
target -= AutoUtils.state.getRot();
177174
}
178175
} else {
179-
double dx = pointX - AutoUtils.state.getX();
180-
double dy = pointY - AutoUtils.state.getY();
176+
double dx = point[0] - AutoUtils.state.getX();
177+
double dy = point[1] - AutoUtils.state.getY();
181178

182179
System.out.println("x = " + dx + ", y = " + dy);
183180

0 commit comments

Comments
 (0)