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