13
13
import org .firstinspires .ftc .teamcode .trajectorysequence .TrajectorySequence ;
14
14
import org .firstinspires .ftc .teamcode .trajectorysequence .sequencesegment .WaitSegment ;
15
15
import org .firstinspires .ftc .teamcode .util .GlobalOpMode ;
16
+ import org .firstinspires .ftc .teamcode .util .Timeout ;
16
17
import org .firstinspires .ftc .teamcode .vision .AprilTagCamera ;
17
18
import org .firstinspires .ftc .teamcode .vision .AprilTagLocalizer ;
18
19
import org .firstinspires .ftc .teamcode .vision .PropellerDetection1 ;
19
20
import org .firstinspires .ftc .teamcode .vision .TensorFlowDetection ;
21
+ import org .tensorflow .lite .Tensor ;
20
22
21
23
@ Autonomous
22
24
@ Config
@@ -37,41 +39,50 @@ private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalize
37
39
return pose ;
38
40
}
39
41
42
+ public static double LEFTFORTYFIVE = -45 ;
43
+ public static double LEFTSEVENTY = 70 ;
44
+ public static double RIGHTFORTYFIVE = 45 ;
45
+ public static double RIGHTSEVENTY = -70 ;
46
+
40
47
@ Override
41
48
public void runOpMode () throws InterruptedException {
42
49
GlobalOpMode .opMode = this ;
43
50
TestBotDrive drive = new TestBotDrive (hardwareMap );
44
51
45
52
AprilTagCamera [] cameras = new AprilTagCamera [3 ];
46
- cameras [0 ] = new AprilTagCamera (hardwareMap .get (WebcamName .class , "Left" ), 8 , Math .toRadians (70 ), Math .toRadians (- 45 ));
53
+ cameras [0 ] = new AprilTagCamera (hardwareMap .get (WebcamName .class , "Left" ), 8 , Math .toRadians (LEFTSEVENTY ), Math .toRadians (LEFTFORTYFIVE ));
47
54
cameras [1 ] = new AprilTagCamera (hardwareMap .get (WebcamName .class , "Center" ), 7 , Math .toRadians (90 ), Math .toRadians (0 ));
48
- cameras [2 ] = new AprilTagCamera (hardwareMap .get (WebcamName .class , "Right" ), 8 , Math .toRadians (- 70 ), Math .toRadians (45 ));
55
+ cameras [2 ] = new AprilTagCamera (hardwareMap .get (WebcamName .class , "Right" ), 8 , Math .toRadians (RIGHTSEVENTY ), Math .toRadians (RIGHTFORTYFIVE ));
49
56
50
57
51
- TensorFlowDetection tensor = new TensorFlowDetection (cameras [1 ].webcamName );
58
+ //TensorFlowDetection tensor = new TensorFlowDetection(cameras[1].webcamName);
59
+ TensorFlowDetection tensor = null ;
52
60
Log .i ("PROGRESS" , "I did this!" );
53
61
// while (!isStopRequested()) {
54
62
// tensor.getPropPosition();
55
63
// android.util.Log.w("LOCATION", "tensor result " + tensor.getPropPosition());
56
64
// }
57
65
58
66
59
- TensorFlowDetection . PropPosition tensorPos = tensor . getPropPosition ();
67
+
60
68
61
69
// TensorFlowDetection.PropPosition tensorPos = TensorFlowDetection.PropPosition.LEFT;
62
- Log .i ("PROGRESS" , "Made it here" );
63
- Log .i ("PROGRESS" , tensorPos + "" );
64
- Log .i ("PROGRESS" , tensor .getPropPosition () + "" );
70
+
65
71
66
72
67
73
AprilTagLocalizer aprilTag = new AprilTagLocalizer (cameras );
68
74
Pose2d startPose = null ;
69
75
Log .i ("PROGRESS" , "here?" );
70
- while (opModeInInit () && !isStopRequested () && startPose == null ) {
76
+ while (opModeInInit () && !isStopRequested () ) {
71
77
Log .i ("PROGRESS" , "before" );
72
78
startPose = estimateWithAllCameras (cameras , aprilTag );
73
79
Log .i ("PROGRESS" , "after" );
80
+ if (startPose != null ) {
81
+ drive .setPoseEstimate (startPose );
82
+ drive .update ();
83
+ }
74
84
}
85
+ waitForStart ();
75
86
Log .i ("PROGRESS" , "Maybe?" );
76
87
if (startPose == null ) {
77
88
// Check one more time
@@ -90,6 +101,11 @@ public void runOpMode() throws InterruptedException {
90
101
drive .setPoseEstimate (startPose );
91
102
Log .i ("PROGRESS" , "Im here too!" );
92
103
104
+ TensorFlowDetection .PropPosition tensorPos = tensor .getPropPosition ();
105
+ if (tensorPos == null ) {
106
+ tensorPos = TensorFlowDetection .PropPosition .CENTER ;
107
+ }
108
+
93
109
94
110
switch (AprilTagLocalizer .whichQuadrant (startPose )) {
95
111
case RED_BOARD :
@@ -285,7 +301,6 @@ public void runOpMode() throws InterruptedException {
285
301
break ;
286
302
}
287
303
Log .i ("PROGRESS" , "I got here???" );
288
- waitForStart ();
289
304
290
305
if (!isStopRequested ())
291
306
drive .followTrajectorySequence (traj );
0 commit comments