// constants for position 5
private final double POS5_DIST1 = 0;
- private final double POS5_TURN1 = 0;
- private final double POS5_DIST2 = 0;
+ private final double POS5_TURN1 = -90;
+ private final double POS5_DIST2 = 72.5;
+ private final double POS5_TURN2 = 90;
+ private final double POS5_DIST3 = 0;
public double horizontalDistToGoal;
addSequential(new DriveForDistance(POS5_DIST1, DEFAULT_SPEED));
addSequential(new TurnForAngle(POS5_TURN1));
addSequential(new DriveForDistance(POS5_DIST2, DEFAULT_SPEED));
+ addSequential(new TurnForAngle(POS5_TURN2));
+ addSequential(new DriveForDistance(POS5_DIST3, DEFAULT_SPEED));
horizontalDistToGoal = 0;
+
}
}