Commit | Line | Data |
---|---|---|
ff5c6dcc ME |
1 | package org.usfirst.frc.team3501.robot.commands; |
2 | ||
1696ae28 ME |
3 | import org.usfirst.frc.team3501.robot.Robot; |
4 | ||
ff5c6dcc ME |
5 | import edu.wpi.first.wpilibj.command.CommandGroup; |
6 | ||
7 | /** | |
8 | * This command group will be used in autonomous. Based on what position the | |
10ca638d ME |
9 | * robot is in, the robot will align with the goal. In the Software 2015-2016 |
10 | * Google folder is a picture explaining each of the cases. | |
11 | * | |
12 | * dependency on sensors: lidars, encoders, gyro dependency on subsystems: | |
13 | * drivetrain dependency on other commands: TurnForAngle(), DriveForDistance() | |
ff5c6dcc ME |
14 | * |
15 | * pre-condition: robot is flush against a defense at the specified position in | |
16 | * the opponent's courtyard | |
17 | * | |
18 | * post-condition: the robot is parallel to one of the three goals and the | |
19 | * shooter is facing that goal | |
20 | * | |
21 | */ | |
22 | public class AlignToScore extends CommandGroup { | |
ff9a504f ME |
23 | private final static double CENTER_OF_MASS_TO_ROBOT_FRONT = 0; |
24 | private final static double DIST_CASTLE_WALL_TO_SIDE_GOAL = 0; | |
25 | private final static double DIST_CASTLE_WALL_TO_FRONT_GOAL = 0; | |
1696ae28 | 26 | |
5e1e9b1e | 27 | private final double DEFAULT_SPEED = 0.5; |
5e1e9b1e | 28 | |
1696ae28 | 29 | // constants for position 1: low bar |
5e1e9b1e ME |
30 | private final double POS1_DIST1 = 0; |
31 | private final double POS1_TURN1 = 0; | |
32 | private final double POS1_DIST2 = 0; | |
33 | ||
34 | // constants for position 2 | |
4347c80d ME |
35 | private final double POS2_DIST1 = 0; |
36 | private final double POS2_TURN1 = 0; | |
37 | private final double POS2_DIST2 = 0; | |
38 | ||
5e1e9b1e | 39 | // constants for position 3 |
a2b9c4a9 ME |
40 | private final double POS3_DIST1 = 0; |
41 | private final double POS3_TURN1 = 0; | |
42 | private final double POS3_DIST2 = 0; | |
43 | private final double POS3_TURN2 = 0; | |
44 | private final double POS3_DIST3 = 0; | |
5e1e9b1e ME |
45 | |
46 | // constants for position 4 | |
1696ae28 ME |
47 | private final double POS4_DIST1 = 0; |
48 | private final double POS4_TURN1 = 0; | |
49 | private final double POS4_DIST2 = 0; | |
50 | private final double POS4_TURN2 = 0; | |
51 | private final double POS4_DIST3 = 0; | |
5e1e9b1e ME |
52 | |
53 | // constants for position 5 | |
1696ae28 ME |
54 | private final double POS5_DIST1 = 0; |
55 | private final double POS5_TURN1 = 0; | |
56 | private final double POS5_DIST2 = 0; | |
ff5c6dcc | 57 | |
ff9a504f ME |
58 | public double horizontalDistToGoal; |
59 | ||
ff5c6dcc ME |
60 | public AlignToScore(int position) { |
61 | ||
62 | switch (position) { | |
5e1e9b1e ME |
63 | |
64 | // position 1 is always the low bar | |
ff5c6dcc ME |
65 | case 1: |
66 | ||
4347c80d ME |
67 | addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED)); |
68 | addSequential(new TurnForAngle(POS1_TURN1)); | |
69 | addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED)); | |
ff9a504f | 70 | horizontalDistToGoal = 0; |
ff5c6dcc | 71 | |
a2b9c4a9 ME |
72 | case 2: |
73 | ||
74 | addSequential(new DriveForDistance(POS2_DIST1, DEFAULT_SPEED)); | |
75 | addSequential(new TurnForAngle(POS2_TURN1)); | |
76 | addSequential(new DriveForDistance(POS2_DIST2, DEFAULT_SPEED)); | |
ff9a504f | 77 | horizontalDistToGoal = 0; |
a2b9c4a9 | 78 | |
ff5c6dcc ME |
79 | case 3: |
80 | ||
a2b9c4a9 ME |
81 | addSequential(new DriveForDistance(POS3_DIST1, DEFAULT_SPEED)); |
82 | addSequential(new TurnForAngle(POS3_TURN1)); | |
83 | addSequential(new DriveForDistance(POS3_DIST2, DEFAULT_SPEED)); | |
84 | addSequential(new TurnForAngle(POS3_TURN2)); | |
85 | addSequential(new DriveForDistance(POS3_DIST3, DEFAULT_SPEED)); | |
ff9a504f | 86 | horizontalDistToGoal = 0; |
ff5c6dcc ME |
87 | |
88 | case 4: | |
89 | ||
1696ae28 ME |
90 | addSequential(new DriveForDistance(POS4_DIST1, DEFAULT_SPEED)); |
91 | addSequential(new TurnForAngle(POS4_TURN1)); | |
92 | addSequential(new DriveForDistance(POS4_DIST2, DEFAULT_SPEED)); | |
93 | addSequential(new TurnForAngle(POS4_TURN2)); | |
94 | addSequential(new DriveForDistance(POS4_DIST3, DEFAULT_SPEED)); | |
ff9a504f | 95 | horizontalDistToGoal = 0; |
ff5c6dcc ME |
96 | |
97 | case 5: | |
98 | ||
1696ae28 ME |
99 | addSequential(new DriveForDistance(POS5_DIST1, DEFAULT_SPEED)); |
100 | addSequential(new TurnForAngle(POS5_TURN1)); | |
101 | addSequential(new DriveForDistance(POS5_DIST2, DEFAULT_SPEED)); | |
ff9a504f | 102 | horizontalDistToGoal = 0; |
ff5c6dcc ME |
103 | } |
104 | } | |
1696ae28 | 105 | |
10ca638d | 106 | public static double calculateAngleToTurn(int position, |
ff9a504f ME |
107 | double horizontalDistToGoal) { |
108 | double leftDist = Robot.driveTrain.getLeftLidarDistance(); | |
109 | double rightDist = Robot.driveTrain.getRightLidarDistance(); | |
110 | ||
111 | double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2); | |
112 | double distToTower; | |
113 | // TODO: figure out if we do want to shoot into the side goal if we are | |
114 | // in position 1 or 2, or if we want to change that | |
115 | if (position == 1 || position == 2) { | |
116 | distToTower = Math | |
117 | .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) | |
118 | - DIST_CASTLE_WALL_TO_SIDE_GOAL; | |
119 | } | |
120 | ||
121 | // TODO: figure out if we do want to shoot into the font goal if we are | |
122 | // in position 3, 4, 5, or if we want to change that | |
123 | else { | |
124 | distToTower = Math | |
125 | .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) | |
126 | - DIST_CASTLE_WALL_TO_SIDE_GOAL; | |
127 | } | |
128 | ||
129 | double angleToTurn = Math.atan(distToTower / horizontalDistToGoal); | |
130 | ||
131 | return angleToTurn; | |
1696ae28 | 132 | } |
ff5c6dcc | 133 | } |