challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java

   1
   2 package org.usfirst.frc.team3501.robot.commandgroups;
   3
   4 import org.usfirst.frc.team3501.robot.Constants.Direction;
   5 import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
   6 import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
   7
   8 import edu.wpi.first.wpilibj.command.CommandGroup;
   9
  10 /**
  11  *
  12  * Authors Ayush, Nadia, Aziza and Abhinav
  13  *
  14  * This comandGroup, PlaceGearOnMiddlePeg is expected to have the robot start
  15  * from the middle of the starting line, right in front of the airship. The
  16  * robot will drive forward and place the gear on the peg. Then, the robot will
  17  * drive backwards, turn left, drive forward, turn right, and drive forward
  18  * again to cross the baseline.
  19  */
  20 public class AutonMiddleGear extends CommandGroup {
  21   private static final double DISTANCE_TO_PEG = 91.3;
  22   private static final double DISTANCE_TO_BACK_OUT = 29.75;
  23   private static final double THIRD_DISTANCE_TO_TRAVEL = 70;
  24   private static final double DISTANCE_TO_BASELINE = 50.5;
  25
  26   private static final double ANGLE_TO_TURN = 90;
  27
  28   private static final double maxTimeOut = 7;
  29
  30   /***
  31    * This auton command group places the gear on the middle peg then crosses the
  32    * baseline
  33    *
  34    * @param direction
  35    *          direction to turn after placing gear on peg in order to cross the
  36    *          baseline. Only Direction.LEFT and Direction.RIGHT will be accepted
  37    */
  38   public AutonMiddleGear(Direction direction) {
  39     addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut));
  40     addSequential(new DriveDistance(DISTANCE_TO_BACK_OUT, maxTimeOut));
  41     addSequential(new TurnForAngle(ANGLE_TO_TURN, direction, maxTimeOut));
  42     addSequential(new DriveDistance(THIRD_DISTANCE_TO_TRAVEL, maxTimeOut));
  43     addSequential(
  44         new TurnForAngle(ANGLE_TO_TURN, oppositeOf(direction), maxTimeOut));
  45     addSequential(new DriveDistance(DISTANCE_TO_BASELINE, maxTimeOut));
  46   }
  47
  48   private Direction oppositeOf(Direction direction) {
  49     if (direction == Direction.LEFT)
  50       return Direction.RIGHT;
  51     return Direction.LEFT;
  52   }
  53 }