1 package org
.usfirst
.frc
.team3501
.robot
.commands
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
5 import edu
.wpi
.first
.wpilibj
.command
.CommandGroup
;
8 * This command group will be used in autonomous. Based on what position the
9 * robot is in, the robot will align with the goal
11 * pre-condition: robot is flush against a defense at the specified position in
12 * the opponent's courtyard
14 * post-condition: the robot is parallel to one of the three goals and the
15 * shooter is facing that goal
18 public class AlignToScore
extends CommandGroup
{
19 private final double DIST_CENTER_OF_MASS_TO_FRONT_OF_ROBOT
= 0;
21 private final double DEFAULT_SPEED
= 0.5;
23 // constants for position 1: low bar
24 private final double POS1_DIST1
= 0;
25 private final double POS1_TURN1
= 0;
26 private final double POS1_DIST2
= 0;
28 // constants for position 2
29 private final double POS2_DIST1
= 0;
30 private final double POS2_TURN1
= 0;
31 private final double POS2_DIST2
= 0;
33 // constants for position 3
34 private final double POS3_DIST1
= 0;
35 private final double POS3_TURN1
= 0;
36 private final double POS3_DIST2
= 0;
37 private final double POS3_TURN2
= 0;
38 private final double POS3_DIST3
= 0;
40 // constants for position 4
41 private final double POS4_DIST1
= 0;
42 private final double POS4_TURN1
= 0;
43 private final double POS4_DIST2
= 0;
44 private final double POS4_TURN2
= 0;
45 private final double POS4_DIST3
= 0;
47 // constants for position 5
48 private final double POS5_DIST1
= 0;
49 private final double POS5_TURN1
= 0;
50 private final double POS5_DIST2
= 0;
52 public AlignToScore(int position
) {
56 // position 1 is always the low bar
59 addSequential(new DriveForDistance(POS1_DIST1
, DEFAULT_SPEED
));
60 addSequential(new TurnForAngle(POS1_TURN1
));
61 addSequential(new DriveForDistance(POS1_DIST2
, DEFAULT_SPEED
));
65 addSequential(new DriveForDistance(POS2_DIST1
, DEFAULT_SPEED
));
66 addSequential(new TurnForAngle(POS2_TURN1
));
67 addSequential(new DriveForDistance(POS2_DIST2
, DEFAULT_SPEED
));
71 addSequential(new DriveForDistance(POS3_DIST1
, DEFAULT_SPEED
));
72 addSequential(new TurnForAngle(POS3_TURN1
));
73 addSequential(new DriveForDistance(POS3_DIST2
, DEFAULT_SPEED
));
74 addSequential(new TurnForAngle(POS3_TURN2
));
75 addSequential(new DriveForDistance(POS3_DIST3
, DEFAULT_SPEED
));
79 addSequential(new DriveForDistance(POS4_DIST1
, DEFAULT_SPEED
));
80 addSequential(new TurnForAngle(POS4_TURN1
));
81 addSequential(new DriveForDistance(POS4_DIST2
, DEFAULT_SPEED
));
82 addSequential(new TurnForAngle(POS4_TURN2
));
83 addSequential(new DriveForDistance(POS4_DIST3
, DEFAULT_SPEED
));
87 addSequential(new DriveForDistance(POS5_DIST1
, DEFAULT_SPEED
));
88 addSequential(new TurnForAngle(POS5_TURN1
));
89 addSequential(new DriveForDistance(POS5_DIST2
, DEFAULT_SPEED
));
93 public static void calculatePath() {
94 double leftDistance
= Robot
.shooter
.getLeftDistanceToTower();
95 double rightDistance
= Robot
.shooter
.getRightDistanceToTower();