+ // position 1 is always the low bar
+ 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));
+ }