2 package org
.usfirst
.frc
.team3501
.robot
.commandgroups
;
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
;
8 import edu
.wpi
.first
.wpilibj
.command
.CommandGroup
;
12 * Authors Ayush, Nadia, Aziza and Abhinav
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.
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;
26 private static final double ANGLE_TO_TURN
= 90;
28 private static final double maxTimeOut
= 7;
31 * This auton command group places the gear on the middle peg then crosses the
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
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
));
44 new TurnForAngle(ANGLE_TO_TURN
, oppositeOf(direction
), maxTimeOut
));
45 addSequential(new DriveDistance(DISTANCE_TO_BASELINE
, maxTimeOut
));
48 private Direction
oppositeOf(Direction direction
) {
49 if (direction
== Direction
.LEFT
)
50 return Direction
.RIGHT
;
51 return Direction
.LEFT
;