-
- addSequential(new DriveDistance(POS1_DIST1, DEFAULT_SPEED));
- addSequential(new TurnForAngle(POS1_TURN1, maxTimeout));
- addSequential(new DriveDistance(POS1_DIST2, DEFAULT_SPEED));
- horizontalDistToGoal = 0;
- } else if (position == 2) {
-
- addSequential(new DriveDistance(POS2_DIST1, DEFAULT_SPEED));
- addSequential(new TurnForAngle(POS2_TURN1, maxTimeout));
- addSequential(new DriveDistance(POS2_DIST2, DEFAULT_SPEED));
- horizontalDistToGoal = 0;
-
- } else if (position == 3) {
-
- addSequential(new DriveDistance(POS3_DIST1, DEFAULT_SPEED));
- addSequential(new TurnForAngle(POS3_TURN1, maxTimeout));
- addSequential(new DriveDistance(POS3_DIST2, DEFAULT_SPEED));
- addSequential(new TurnForAngle(POS3_TURN2, maxTimeout));
- addSequential(new DriveDistance(POS3_DIST3, DEFAULT_SPEED));
- horizontalDistToGoal = 0;
-
- } else if (position == 4) {
-
- addSequential(new DriveDistance(POS4_DIST1, DEFAULT_SPEED));
- addSequential(new TurnForAngle(POS4_TURN1, maxTimeout));
- addSequential(new DriveDistance(POS4_DIST2, DEFAULT_SPEED));
- addSequential(new TurnForAngle(POS4_TURN2, maxTimeout));
- addSequential(new DriveDistance(POS4_DIST3, DEFAULT_SPEED));
- horizontalDistToGoal = 0;
-
- } else if (position == 5) {
-
- addSequential(new DriveDistance(POS5_DIST1, DEFAULT_SPEED));
- addSequential(new TurnForAngle(POS5_TURN1, maxTimeout));
- addSequential(new DriveDistance(POS5_DIST2, DEFAULT_SPEED));
- addSequential(new TurnForAngle(POS5_TURN2, maxTimeout));
- addSequential(new DriveDistance(POS5_DIST3, DEFAULT_SPEED));
- horizontalDistToGoal = 0;
-
+ if (position == 1) {
+ addSequential(new DriveDistance(Constants.Auton.POS1_DIST1,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+ addSequential(new TurnForAngle(Constants.Auton.POS1_TURN1,
+ Constants.Auton.TURN_MAX_TIMEOUT));
+ addSequential(new DriveDistance(Constants.Auton.POS1_DIST2,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+ } else if (position == 2) {
+
+ addSequential(new DriveDistance(Constants.Auton.POS2_DIST1,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+ addSequential(new TurnForAngle(Constants.Auton.POS2_TURN1,
+ Constants.Auton.TURN_MAX_TIMEOUT));
+ addSequential(new DriveDistance(Constants.Auton.POS2_DIST2,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+
+ } else if (position == 3) {
+
+ addSequential(new DriveDistance(Constants.Auton.POS3_DIST1,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+ addSequential(new TurnForAngle(Constants.Auton.POS3_TURN1, maxTimeout));
+ addSequential(new DriveDistance(Constants.Auton.POS3_DIST2,
+ Constants.Auton.TURN_MAX_TIMEOUT));
+ addSequential(new TurnForAngle(Constants.Auton.POS3_TURN2, maxTimeout));
+ addSequential(new DriveDistance(Constants.Auton.POS3_DIST3,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+
+ } else if (position == 4) {
+
+ addSequential(new DriveDistance(Constants.Auton.POS4_DIST1,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+ addSequential(new TurnForAngle(Constants.Auton.POS4_TURN1, maxTimeout));
+ addSequential(new DriveDistance(Constants.Auton.POS4_DIST2,
+ Constants.Auton.TURN_MAX_TIMEOUT));
+ addSequential(new TurnForAngle(Constants.Auton.POS4_TURN2, maxTimeout));
+ addSequential(new DriveDistance(Constants.Auton.POS4_DIST3,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+
+ } else if (position == 5) {
+
+ addSequential(new DriveDistance(Constants.Auton.POS5_DIST1,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+ addSequential(new TurnForAngle(Constants.Auton.POS5_TURN1, maxTimeout));
+ addSequential(new DriveDistance(Constants.Auton.POS5_DIST2,
+ Constants.Auton.TURN_MAX_TIMEOUT));
+ addSequential(new TurnForAngle(Constants.Auton.POS5_TURN2, maxTimeout));
+ addSequential(new DriveDistance(Constants.Auton.POS5_DIST3,
+ Constants.Auton.DRIVE_MAX_TIMEOUT));
+ }