|
11 | 11 |
|
12 | 12 | public class CenterOnFeeder extends Command {
|
13 | 13 |
|
14 |
| - public enum PossiblePoses { |
| 14 | + public enum PossiblePose { |
15 | 15 |
|
16 | 16 | FAR_LEFT(0, 0), MIDDLE_LEFT(0, 0), CLOSE_LEFT(0, 0),
|
17 | 17 | CENTER(0, 0), CLOSE_RIGHT(0, 0), MIDDLE_RIGHT(0, 0), FAR_RIGHT(0, 0);
|
18 | 18 |
|
19 | 19 | public final double xPose;
|
20 | 20 | public final double yPose;
|
21 | 21 |
|
22 |
| - PossiblePoses(double xPose, double yPose) { |
| 22 | + PossiblePose(double xPose, double yPose) { |
23 | 23 | this.xPose = xPose;
|
24 | 24 | this.yPose = yPose;
|
25 | 25 | }
|
@@ -59,12 +59,12 @@ public void execute() {
|
59 | 59 | yPIDController.setPID(pidSettings.getkP(), pidSettings.getkI(), pidSettings.getkD());
|
60 | 60 | yPIDController.setTolerance(pidSettings.getTolerance());
|
61 | 61 | yFeedForwardController.setGains(feedForwardSettings);
|
62 |
| - double shortestDistance = getDistanceFrom(PossiblePoses.FAR_LEFT.xPose, PossiblePoses.FAR_LEFT.yPose); |
63 |
| - PossiblePoses pose = PossiblePoses.FAR_LEFT; |
64 |
| - for (PossiblePoses possiblePoses : PossiblePoses.values()) { |
65 |
| - if (shortestDistance > getDistanceFrom(possiblePoses.xPose, possiblePoses.yPose)) { |
66 |
| - shortestDistance = getDistanceFrom(possiblePoses.xPose, possiblePoses.yPose); |
67 |
| - pose = possiblePoses; |
| 62 | + double shortestDistance = getDistanceFrom(PossiblePose.FAR_LEFT.xPose, PossiblePose.FAR_LEFT.yPose); |
| 63 | + PossiblePose pose = PossiblePose.FAR_LEFT; |
| 64 | + for (PossiblePose possiblePose : PossiblePose.values()) { |
| 65 | + if (shortestDistance > getDistanceFrom(possiblePose.xPose, possiblePose.yPose)) { |
| 66 | + shortestDistance = getDistanceFrom(possiblePose.xPose, possiblePose.yPose); |
| 67 | + pose = possiblePose; |
68 | 68 | }
|
69 | 69 | }
|
70 | 70 | drivetrain.drive(xPIDController.calculate(drivetrain.getPose2d().getX(), pose.xPose) +
|
|
0 commit comments