2 package org
.usfirst
.frc
.team3501
.robot
.commandgroups
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.DriveDistance
;
6 import edu
.wpi
.first
.wpilibj
.command
.CommandGroup
;
10 * Authors Ayush, Nadia, Aziza and Abhinav
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.
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;
24 private static final double ANGLE_TO_TURN
= 90;
26 private static final double maxTimeOut
= 7;
29 * This auton command group places the gear on the middle peg then crosses the
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
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));
43 // new TurnForAngle(ANGLE_TO_TURN, oppositeOf(direction), maxTimeOut));
44 // addSequential(new DriveDistance(DISTANCE_TO_BASELINE, maxTimeOut));