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