add some of the constants for AlignToScore
[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / commands / AlignToScore.java
CommitLineData
ff5c6dcc
ME
1package org.usfirst.frc.team3501.robot.commands;
2
1696ae28
ME
3import org.usfirst.frc.team3501.robot.Robot;
4
ff5c6dcc
ME
5import 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 *
9d87dd2d
ME
12 * dependency on sensors: lidars, encoders, gyro
13 *
14 * dependency on subsystems: drivetrain
15 *
16 * dependency on other commands: TurnForAngle(), DriveForDistance()
ff5c6dcc
ME
17 *
18 * pre-condition: robot is flush against a defense at the specified position in
19 * the opponent's courtyard
20 *
21 * post-condition: the robot is parallel to one of the three goals and the
22 * shooter is facing that goal
23 *
24 */
25public class AlignToScore extends CommandGroup {
ff9a504f
ME
26 private final static double CENTER_OF_MASS_TO_ROBOT_FRONT = 0;
27 private final static double DIST_CASTLE_WALL_TO_SIDE_GOAL = 0;
28 private final static double DIST_CASTLE_WALL_TO_FRONT_GOAL = 0;
1696ae28 29
5e1e9b1e 30 private final double DEFAULT_SPEED = 0.5;
5e1e9b1e 31
af279fe0
ME
32 // in inches
33 // assuming that positive angle means turning right
34 // and negative angle means turning left
35
1696ae28 36 // constants for position 1: low bar
af279fe0
ME
37 private final double POS1_DIST1 = 109;
38 private final double POS1_TURN1 = 60;
5e1e9b1e
ME
39 private final double POS1_DIST2 = 0;
40
41 // constants for position 2
af279fe0
ME
42 private final double POS2_DIST1 = 140;
43 private final double POS2_TURN1 = 60;
4347c80d
ME
44 private final double POS2_DIST2 = 0;
45
5e1e9b1e 46 // constants for position 3
a2b9c4a9 47 private final double POS3_DIST1 = 0;
af279fe0
ME
48 private final double POS3_TURN1 = 90;
49 private final double POS3_DIST2 = 35.5;
50 private final double POS3_TURN2 = -90;
a2b9c4a9 51 private final double POS3_DIST3 = 0;
5e1e9b1e
ME
52
53 // constants for position 4
1696ae28 54 private final double POS4_DIST1 = 0;
af279fe0
ME
55 private final double POS4_TURN1 = -90;
56 private final double POS4_DIST2 = 18.5;
57 private final double POS4_TURN2 = 90;
1696ae28 58 private final double POS4_DIST3 = 0;
5e1e9b1e
ME
59
60 // constants for position 5
1696ae28
ME
61 private final double POS5_DIST1 = 0;
62 private final double POS5_TURN1 = 0;
63 private final double POS5_DIST2 = 0;
ff5c6dcc 64
ff9a504f
ME
65 public double horizontalDistToGoal;
66
ff5c6dcc
ME
67 public AlignToScore(int position) {
68
0d4900b6 69 if (position == 1) {
5e1e9b1e 70
0d4900b6 71 // position 1 is always the low bar
ff5c6dcc 72
4347c80d
ME
73 addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED));
74 addSequential(new TurnForAngle(POS1_TURN1));
75 addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED));
ff9a504f 76 horizontalDistToGoal = 0;
0d4900b6 77 } else if (position == 2) {
a2b9c4a9
ME
78
79 addSequential(new DriveForDistance(POS2_DIST1, DEFAULT_SPEED));
80 addSequential(new TurnForAngle(POS2_TURN1));
81 addSequential(new DriveForDistance(POS2_DIST2, DEFAULT_SPEED));
ff9a504f 82 horizontalDistToGoal = 0;
a2b9c4a9 83
0d4900b6 84 } else if (position == 3) {
ff5c6dcc 85
a2b9c4a9
ME
86 addSequential(new DriveForDistance(POS3_DIST1, DEFAULT_SPEED));
87 addSequential(new TurnForAngle(POS3_TURN1));
88 addSequential(new DriveForDistance(POS3_DIST2, DEFAULT_SPEED));
89 addSequential(new TurnForAngle(POS3_TURN2));
90 addSequential(new DriveForDistance(POS3_DIST3, DEFAULT_SPEED));
ff9a504f 91 horizontalDistToGoal = 0;
ff5c6dcc 92
0d4900b6 93 } else if (position == 4) {
ff5c6dcc 94
1696ae28
ME
95 addSequential(new DriveForDistance(POS4_DIST1, DEFAULT_SPEED));
96 addSequential(new TurnForAngle(POS4_TURN1));
97 addSequential(new DriveForDistance(POS4_DIST2, DEFAULT_SPEED));
98 addSequential(new TurnForAngle(POS4_TURN2));
99 addSequential(new DriveForDistance(POS4_DIST3, DEFAULT_SPEED));
ff9a504f 100 horizontalDistToGoal = 0;
ff5c6dcc 101
0d4900b6 102 } else if (position == 5) {
ff5c6dcc 103
1696ae28
ME
104 addSequential(new DriveForDistance(POS5_DIST1, DEFAULT_SPEED));
105 addSequential(new TurnForAngle(POS5_TURN1));
106 addSequential(new DriveForDistance(POS5_DIST2, DEFAULT_SPEED));
ff9a504f 107 horizontalDistToGoal = 0;
ff5c6dcc
ME
108 }
109 }
1696ae28 110
1e9f4600 111 public static double lidarCalculateAngleToTurn(int position,
ff9a504f
ME
112 double horizontalDistToGoal) {
113 double leftDist = Robot.driveTrain.getLeftLidarDistance();
114 double rightDist = Robot.driveTrain.getRightLidarDistance();
115
116 double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2);
117 double distToTower;
118 // TODO: figure out if we do want to shoot into the side goal if we are
119 // in position 1 or 2, or if we want to change that
120 if (position == 1 || position == 2) {
121 distToTower = Math
122 .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
123 - DIST_CASTLE_WALL_TO_SIDE_GOAL;
124 }
125
126 // TODO: figure out if we do want to shoot into the font goal if we are
127 // in position 3, 4, 5, or if we want to change that
128 else {
129 distToTower = Math
130 .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
131 - DIST_CASTLE_WALL_TO_SIDE_GOAL;
132 }
133
134 double angleToTurn = Math.atan(distToTower / horizontalDistToGoal);
135
136 return angleToTurn;
1696ae28 137 }
ff5c6dcc 138}