- } else if (distanceFromBoilerCorner > SECOND_LIMIT_LENGTH
- && distanceFromBoilerCorner <= DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE) // mirror
- // of
- // right
- // peg
- // path
- {
- this.distanceFromCorner = DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE
- - distanceFromBoilerCorner;
- // Turning DISTANCE_FROM_BOILER_WALL into starting point from left side by
- // reversing it
- addSequential(
- new DriveDistance(
- ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
- + DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE
- - (ROBOT_LENGTH / 2.0),
- MOTOR_VALUE_FORWARD));
- addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
- addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE
- - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2)
- - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0),
- MOTOR_VALUE_FORWARD));
+ } else if (side.equals("RETRIEVAL")) {
+ addSequential(new DriveDistance(
+ 131.6 - (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+ - ROBOT_LENGTH / 2,
+ 5));
+ addSequential(new TurnForAngle(60, Direction.LEFT, 5));
+ addSequential(new DriveDistance(
+ 2 * (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+ - ROBOT_LENGTH / 2,
+ 5));
+ // addSequential(new DriveDistance(
+ // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
+ // + DISTANCE_TO_RETRIEVAL_PEG - (ROBOT_LENGTH / 2.0),
+ // MOTOR_VALUE_FORWARD));
+ // addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
+ // addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE
+ // - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2)
+ // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD));