1
1
package org .firstinspires .ftc .teamcode .opmode ;
2
2
3
+ import android .util .Log ;
4
+
3
5
import com .acmerobotics .dashboard .config .Config ;
4
6
import com .acmerobotics .roadrunner .geometry .Pose2d ;
5
7
import com .acmerobotics .roadrunner .geometry .Vector2d ;
@@ -47,7 +49,7 @@ public void runOpMode() throws InterruptedException {
47
49
48
50
49
51
TensorFlowDetection tensor = new TensorFlowDetection (cameras [1 ].webcamName );
50
- android . util . Log .i ("PROGRESS" , "I did this!" );
52
+ Log .i ("PROGRESS" , "I did this!" );
51
53
// while (!isStopRequested()) {
52
54
// tensor.getPropPosition();
53
55
// android.util.Log.w("LOCATION", "tensor result " + tensor.getPropPosition());
@@ -57,23 +59,23 @@ public void runOpMode() throws InterruptedException {
57
59
TensorFlowDetection .PropPosition tensorPos = tensor .getPropPosition ();
58
60
59
61
// TensorFlowDetection.PropPosition tensorPos = TensorFlowDetection.PropPosition.LEFT;
60
- android . util . Log .i ("PROGRESS" , "Made it here" );
61
- android . util . Log .i ("PROGRESS" , tensorPos + "" );
62
- android . util . Log .i ("PROGRESS" , tensor .getPropPosition () + "" );
62
+ Log .i ("PROGRESS" , "Made it here" );
63
+ Log .i ("PROGRESS" , tensorPos + "" );
64
+ Log .i ("PROGRESS" , tensor .getPropPosition () + "" );
63
65
64
66
65
67
AprilTagLocalizer aprilTag = new AprilTagLocalizer (cameras );
66
68
Pose2d startPose = null ;
67
- android . util . Log .i ("PROGRESS" , "here?" );
69
+ Log .i ("PROGRESS" , "here?" );
68
70
while (opModeInInit () && !isStopRequested () && startPose == null ) {
69
- android . util . Log .i ("PROGRESS" , "before" );
71
+ Log .i ("PROGRESS" , "before" );
70
72
startPose = estimateWithAllCameras (cameras , aprilTag );
71
- android . util . Log .i ("PROGRESS" , "after" );
73
+ Log .i ("PROGRESS" , "after" );
72
74
}
73
- android . util . Log .i ("PROGRESS" , "Maybe?" );
75
+ Log .i ("PROGRESS" , "Maybe?" );
74
76
if (startPose == null ) {
75
77
// Check one more time
76
- android . util . Log .w ("APRILTAG" , "Did not find AprilTag in init, trying again" );
78
+ Log .w ("APRILTAG" , "Did not find AprilTag in init, trying again" );
77
79
startPose = estimateWithAllCameras (cameras , aprilTag );
78
80
}
79
81
@@ -82,11 +84,11 @@ public void runOpMode() throws InterruptedException {
82
84
//startPose = new Pose2d(0, 0, 0);
83
85
throw new RuntimeException ("Could not find AprilTag to determine start pose" );
84
86
}
85
- android . util . Log .i ("PROGRESS" , "may as well check" );
87
+ Log .i ("PROGRESS" , "may as well check" );
86
88
TrajectorySequence traj = null ;
87
89
88
90
drive .setPoseEstimate (startPose );
89
- android . util . Log .i ("PROGRESS" , "Im here too!" );
91
+ Log .i ("PROGRESS" , "Im here too!" );
90
92
91
93
92
94
switch (AprilTagLocalizer .whichQuadrant (startPose )) {
@@ -118,7 +120,7 @@ public void runOpMode() throws InterruptedException {
118
120
.turn (Math .toRadians (-90 ))
119
121
.addTemporalMarker (() ->{
120
122
//ARM DROPS PIXEL
121
- android . util . Log .i ("DROP" , "drop" );
123
+ Log .i ("DROP" , "drop" );
122
124
})
123
125
.lineTo (new Vector2d (startPose .getX (), startPose .getY ()))
124
126
.splineToSplineHeading (new Pose2d (3 * 12 + 5 , -3 * 12 , 0 ), 0 )
@@ -130,7 +132,7 @@ public void runOpMode() throws InterruptedException {
130
132
.lineTo (new Vector2d (16 , -36 ))
131
133
.addTemporalMarker (() -> {
132
134
//ARM DROPS PIXEL
133
- android . util . Log .i ("DROP" , "drop" );
135
+ Log .i ("DROP" , "drop" );
134
136
135
137
})
136
138
.turn (Math .toRadians (90 ))
@@ -150,7 +152,7 @@ public void runOpMode() throws InterruptedException {
150
152
.addTemporalMarker (() -> {
151
153
//PIXEL DROP
152
154
153
- android . util . Log .i ("DROP" , "drop" );
155
+ Log .i ("DROP" , "drop" );
154
156
155
157
})
156
158
.waitSeconds (1 )
@@ -168,7 +170,7 @@ public void runOpMode() throws InterruptedException {
168
170
.turn (Math .toRadians (-90 ))
169
171
.addDisplacementMarker (() -> {
170
172
//PIXEL DROP
171
- android . util . Log .i ("DROP" , "drop" );
173
+ Log .i ("DROP" , "drop" );
172
174
})
173
175
.lineTo (new Vector2d (startPose .getX (), startPose .getY ()))
174
176
//.splineToSplineHeading(new Pose2d(3*12 + 5, -3*12, 0), 0)
@@ -181,7 +183,7 @@ public void runOpMode() throws InterruptedException {
181
183
.lineTo (new Vector2d (startPose .getX (), -3 * 12 + 2 )) // drive forward to prevent the spline from cutting through the poles
182
184
.addTemporalMarker (() -> {
183
185
//PIXEL DROP
184
- android . util . Log .i ("DROP" , "drop" );
186
+ Log .i ("DROP" , "drop" );
185
187
})
186
188
.turn (Math .toRadians (90 ))
187
189
//.splineToSplineHeading(new Pose2d(3*12 + 5, -3*12, 0), 0)
@@ -197,9 +199,9 @@ public void runOpMode() throws InterruptedException {
197
199
traj = drive .trajectorySequenceBuilder (startPose )
198
200
.lineTo (new Vector2d (startPose .getX (), 3 *12 -2 ))
199
201
.turn (Math .toRadians (90 ))
200
- .addDisplacementMarker (()->{
202
+ .addTemporalMarker (()->{
201
203
//PIXEL DROP
202
- android . util . Log .i ("ARM" , "drop" );
204
+ Log .i ("ARM" , "drop" );
203
205
})
204
206
.lineToSplineHeading (new Pose2d (2 * 12 , 3 * 12 + 2 , Math .toRadians (90 )))
205
207
.lineToSplineHeading (new Pose2d (3 * 12 + 5 , 3 * 12 + 2 , 0 ))
@@ -211,10 +213,11 @@ public void runOpMode() throws InterruptedException {
211
213
traj = drive .trajectorySequenceBuilder (startPose )
212
214
.lineTo (new Vector2d (startPose .getX (), 3 *12 -2 ))
213
215
.turn (Math .toRadians (-90 ))
214
- .addDisplacementMarker (()->{
216
+ .addTemporalMarker (()->{
215
217
//PIXEL DROP
216
- android . util . Log .i ("DROP" , "drop" );
218
+ Log .i ("DROP" , "drop" );
217
219
})
220
+ .lineTo (new Vector2d (startPose .getX (),startPose .getY ()))
218
221
.turn (Math .toRadians (180 ))
219
222
.lineToSplineHeading (new Pose2d (2 * 12 , 3 * 12 + 2 , Math .toRadians (90 )))
220
223
.lineToSplineHeading (new Pose2d (3 * 12 + 5 , 3 * 12 + 2 , 0 ))
@@ -223,10 +226,10 @@ public void runOpMode() throws InterruptedException {
223
226
case CENTER :
224
227
traj = drive .trajectorySequenceBuilder (startPose )
225
228
.lineTo (new Vector2d (startPose .getX (), 3 *12 -2 ))
226
- .addDisplacementMarker (()->{
229
+ .addTemporalMarker (()->{
227
230
//PIXEL DROP
228
231
//wait(200);
229
- android . util . Log .i ("DROP" , "drop" );
232
+ Log .i ("DROP" , "drop" );
230
233
})
231
234
.turn (Math .toRadians (90 ))
232
235
.lineToSplineHeading (new Pose2d (2 * 12 , 3 * 12 + 2 , Math .toRadians (90 )))
@@ -245,9 +248,9 @@ public void runOpMode() throws InterruptedException {
245
248
traj = drive .trajectorySequenceBuilder (startPose )
246
249
.lineTo (new Vector2d (startPose .getX (), 36 ))
247
250
.turn (Math .toRadians (90 ))
248
- .addDisplacementMarker (()->{
251
+ .addTemporalMarker (()->{
249
252
//PIXEL DROP
250
- android . util . Log .i ("DROP" , "drop" );
253
+ Log .i ("DROP" , "drop" );
251
254
})
252
255
253
256
.splineToSplineHeading (new Pose2d (3 * 12 + 5 , 3 * 12 , 0 ), 0 )
@@ -257,10 +260,11 @@ public void runOpMode() throws InterruptedException {
257
260
traj = drive .trajectorySequenceBuilder (startPose )
258
261
.lineTo (new Vector2d (startPose .getX (), 36 ))
259
262
.turn (Math .toRadians (-90 ))
260
- .addDisplacementMarker (()->{
263
+ .addTemporalMarker (()->{
261
264
//PIXEL DROP
262
- android . util . Log .i ("DROP" , "drop" );
265
+ Log .i ("DROP" , "drop" );
263
266
})
267
+ .lineTo (new Vector2d (startPose .getX (),startPose .getY ()))
264
268
.turn (Math .toRadians (180 ))
265
269
.splineToSplineHeading (new Pose2d (3 * 12 + 5 , 3 * 12 , 0 ), 0 )
266
270
.build ();
@@ -269,9 +273,9 @@ public void runOpMode() throws InterruptedException {
269
273
traj = drive .trajectorySequenceBuilder (startPose )
270
274
.lineTo (new Vector2d (startPose .getX (), 36 ))
271
275
272
- .addDisplacementMarker (()->{
276
+ .addTemporalMarker (()->{
273
277
//PIXEL DROP
274
- android . util . Log .i ("DROP" , "drop" );
278
+ Log .i ("DROP" , "drop" );
275
279
})
276
280
.turn (Math .toRadians (90 ))
277
281
.splineToSplineHeading (new Pose2d (3 * 12 + 5 , 3 * 12 , 0 ), 0 )
@@ -280,15 +284,15 @@ public void runOpMode() throws InterruptedException {
280
284
}
281
285
break ;
282
286
}
283
- android . util . Log .i ("PROGRESS" , "I got here???" );
287
+ Log .i ("PROGRESS" , "I got here???" );
284
288
waitForStart ();
285
289
286
290
if (!isStopRequested ())
287
291
drive .followTrajectorySequence (traj );
288
292
289
293
//aprilTag.centerOnTag(cameras[1], drive, 5);
290
294
291
- android . util . Log .i ("PROGRESS" , "whatttttt" );
295
+ Log .i ("PROGRESS" , "whatttttt" );
292
296
}
293
297
}
294
298
0 commit comments