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