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   private final double POS2_DIST1 = 0;
  27   private final double POS2_TURN1 = 0;
  28   private final double POS2_DIST2 = 0;
  29
  30   // constants for position 3
  31
  32   // constants for position 4
  33
  34   // constants for position 5
  35
  36   public AlignToScore(int position) {
  37
  38     switch (position) {
  39
  40     // position 1 is always the low bar
  41     case 1:
  42
  43       addSequential(new DriveForDistance(POS2_DIST1, DEFAULT_SPEED));
  44       addSequential(new TurnForAngle(POS2_TURN1));
  45       addSequential(new DriveForDistance(POS2_DIST2, DEFAULT_SPEED));
  46
  47     case 2:
  48
  49       addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED));
  50       addSequential(new TurnForAngle(POS1_TURN1));
  51       addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED));
  52
  53     case 3:
  54
  55       addSequential();
  56
  57     case 4:
  58
  59       addSequential();
  60
  61     case 5:
  62
  63       addSequential();
  64     }
  65   }
  66 }