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

   1 package org.usfirst.frc.team3501.robot.commandgroups;
   2
   3 import org.usfirst.frc.team3501.robot.Constants.Direction;
   4 import org.usfirst.frc.team3501.robot.Robot;
   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  */
  13 public class AutonGearThenBaselinePegCloseToRetrievalZone extends CommandGroup {
  14
  15   private static final double DISTANCE_FROM_RETRIEVAL_ZONE_WALL = 111.5676;
  16   private static final int MOTOR_VALUE_TURN = 60;
  17   private static final int MOTOR_VALUE_FORWARD = 60;
  18
  19   public AutonGearThenBaselinePegCloseToRetrievalZone() {
  20
  21     double x = DISTANCE_FROM_RETRIEVAL_ZONE_WALL;
  22     requires(Robot.getDriveTrain());
  23     addSequential(new DriveDistance(
  24         DISTANCE_FROM_RETRIEVAL_ZONE_WALL / Math.sqrt(3) + 52.4614,
  25         MOTOR_VALUE_FORWARD));
  26     addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
  27     addSequential(new DriveDistance(
  28         143.3272 - 2 * DISTANCE_FROM_RETRIEVAL_ZONE_WALL / Math.sqrt(3) - 10.5,
  29         MOTOR_VALUE_FORWARD));
  30     // does not include drift distance
  31
  32   }
  33 }