challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java

   1 package org.usfirst.frc.team3501.robot.commands;
   2
   3 import edu.wpi.first.wpilibj.command.CommandGroup;
   4
   5 /**
   6  * This command group will be used in autonomous. Based on what position the
   7  * robot is in, the robot will align with the goal
   8  *
   9  * pre-condition: robot is flush against a defense at the specified position in
  10  * the opponent's courtyard
  11  *
  12  * post-condition: the robot is parallel to one of the three goals and the
  13  * shooter is facing that goal
  14  *
  15  */
  16 public class AlignToScore extends CommandGroup {
  17   private final double DEFAULT_SPEED = 0.5;
  18   // constants for position 1: low bar
  19
  20   private final double POS1_DIST1 = 0;
  21   private final double POS1_TURN1 = 0;
  22   private final double POS1_DIST2 = 0;
  23
  24   // constants for position 2
  25
  26   // constants for position 3
  27
  28   // constants for position 4
  29
  30   // constants for position 5
  31
  32   public AlignToScore(int position) {
  33
  34     switch (position) {
  35
  36     // position 1 is always the low bar
  37     case 1:
  38
  39       addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED));
  40       addSequential(new TurnForAngle(POS1_TURN1));
  41       addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED));
  42
  43     case 2:
  44
  45       addSequential();
  46
  47     case 3:
  48
  49       addSequential();
  50
  51     case 4:
  52
  53       addSequential();
  54
  55     case 5:
  56
  57       addSequential();
  58     }
  59   }
  60 }