*
*/
public class AlignToScore extends CommandGroup {
+ private final double DEFAULT_SPEED = 0.5;
+ // constants for position 1: low bar
+
+ private final double POS1_DIST1 = 0;
+ private final double POS1_TURN1 = 0;
+ private final double POS1_DIST2 = 0;
+
+ // constants for position 2
+
+ // constants for position 3
+
+ // constants for position 4
+
+ // constants for position 5
public AlignToScore(int position) {
switch (position) {
+
+ // position 1 is always the low bar
case 1:
- addSequential();
+ addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED));
+ addSequential(new TurnForAngle(POS1_TURN1));
+ addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED));
case 2: