11
11
12
12
// import org.mayheminc.robot2019.Robot;
13
13
14
- import edu .wpi .first .wpilibj .DriverStation ;
14
+ // import edu.wpi.first.wpilibj.DriverStation;
15
15
import edu .wpi .first .wpilibj .Timer ;
16
16
import edu .wpi .first .wpilibj2 .command .SubsystemBase ;
17
17
import edu .wpi .first .wpilibj .smartdashboard .SmartDashboard ;
@@ -25,51 +25,12 @@ public class Targeting extends SubsystemBase {
25
25
// Put methods for controlling this subsystem
26
26
// here. Call these from Commands.
27
27
28
- // COMPETITION ROBOT NEEDS THE ONE BELOW!
29
- // private double Y_WHEN_TARGET_AT_WALL = 0.65; // Worked fine and 0.70
30
-
31
- // PRACTICE ROBOT NEEDS THE ONE BELOW!
32
- // private double m_YWhenTargetAtWall= 0.75; // Worked fine and 0.70
33
-
34
- // Y when hatch panel is at wall when the arm is low
35
- private static final double Y_AT_WALL_SAFETY_MARGIN = 0.05 ;
36
-
37
- private static final double Y_WHEN_HATCH_LOW_AT_WALL = 0.75 + Y_AT_WALL_SAFETY_MARGIN ;
38
- // Y when hatch panel is at wall when the arm is high
39
- private static final double Y_WHEN_HATCH_MID_AT_WALL = 0.50 + Y_AT_WALL_SAFETY_MARGIN ;
40
- // Y when hatch panel is at wall when the arm is high
41
- private static final double Y_WHEN_HATCH_HIGH_AT_WALL = 0.60 + Y_AT_WALL_SAFETY_MARGIN ;
42
-
43
28
private static final double SPEED_EQ_M = -4.115 ;
44
29
private static final double SPEED_EQ_B = 2.244 ;
45
30
46
- // Below values are for centered arm
47
- // private static final double CENTER_EQ_M = -0.1925;
48
- // private static final double CENTER_EQ_B = 0.5719;
49
-
50
- // Below values are for arm off 2 inches to left on the practice robot
51
- // private static final double CENTER_EQ_M_HP = -0.2964;
52
- // private static final double CENTER_EQ_B_HP = 0.5871;
53
- // private static final double CENTER_EQ_M_CARGO = -0.6102;
54
- // private static final double CENTER_EQ_B_CARGO = 0.6334;
55
-
56
- // Below values are for competition robot based upon PineTree data
57
- private static final double CENTER_EQ_M_HP = -0.2762 ;
58
- private static final double CENTER_EQ_B_HP = 0.5563 ;
59
-
60
- // below values are guesses for competition robot; same as on practice robot
61
- private static final double CENTER_EQ_M_CARGO = -0.6102 ;
62
- private static final double CENTER_EQ_B_CARGO = 0.6334 ;
63
-
64
- // After computing a desired heading, add a "fudge" offset to correct
65
- // empirically measured error. Found to be approx -1 degree (to shift aim 1" to
66
- // the left) during NECMP Thursday AM practice field session, for competition
67
- // robot.
68
-
69
- // heading correction offset had been 0.0 for PineTree
70
- // changed to -1.0 for first 6 matches of NECMP
71
- // changed to 0.0 at lunch time on Friday
72
- private static final double HEADING_CORRECTION_OFFSET = 0.0 ; // was -2.0 at CMP
31
+ // After computing a desired azimuth, add a "fudge" offset to correct
32
+ // empirically measured error. Offset should be in azimuth "ticks."
33
+ private static final double AZIMUTH_CORRECTION_OFFSET = 0.0 ; // was -2.0 at CMP
73
34
74
35
private double m_desiredAzimuth ;
75
36
private double [] m_target_array ;
@@ -176,7 +137,7 @@ public void update() {
176
137
}
177
138
178
139
public double getDesiredAzimuth () {
179
- return m_desiredAzimuth + HEADING_CORRECTION_OFFSET ;
140
+ return m_desiredAzimuth + AZIMUTH_CORRECTION_OFFSET ;
180
141
}
181
142
182
143
public double getRecommendedSpeed () {
0 commit comments