· 6 years ago · Feb 06, 2020, 04:20 PM
1/* Copyright (c) 2019 FIRST. All rights reserved.
2 *
3 * Redistribution and use in source and binary forms, with or without modification,
4 * are permitted (subject to the limitations in the disclaimer below) provided that
5 * the following conditions are met:
6 *
7 * Redistributions of source code must retain the above copyright notice, this list
8 * of conditions and the following disclaimer.
9 *
10 * Redistributions in binary form must reproduce the above copyright notice, this
11 * list of conditions and the following disclaimer in the documentation and/or
12 * other materials provided with the distribution.
13 *
14 * Neither the name of FIRST nor the names of its contributors may be used to endorse or
15 * promote products derived from this software without specific prior written permission.
16 *
17 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
18 * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
20 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 */
29
30package org.firstinspires.ftc.teamcode;
31
32import com.qualcomm.robotcore.eventloop.opmode.Disabled;
33import org.firstinspires.ftc.robotcore.external.Telemetry;
34import com.qualcomm.robotcore.hardware.DcMotor;
35import com.qualcomm.robotcore.hardware.Servo;
36import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
37import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
38import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
39import java.util.List;
40import org.firstinspires.ftc.robotcore.external.ClassFactory;
41import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
42import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
43import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
44import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
45
46/**
47 * This 2019-2020 OpMode illustrates the basics of using the TensorFlow Object Detection API to
48 * determine the position of the Skystone game elements.
49 *
50 * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
51 * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
52 *
53 * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
54 * is explained below.
55 */
56@Autonomous(name = "Concept: TensorFlow Object Detection", group = "Concept")
57
58public class TensorFlow extends LinearOpMode {
59 private static final String TFOD_MODEL_ASSET = "Skystone.tflite";
60 private static final String LABEL_FIRST_ELEMENT = "Stone";
61 private static final String LABEL_SECOND_ELEMENT = "Skystone";
62 private DcMotor right_drive;
63 private DcMotor left_drive;
64 private DcMotor left_front;
65 private DcMotor right_front;
66 private DcMotor arm_motor;
67 private DcMotor h_drive;
68 private Servo servo_motor;
69 private Servo servo_motor2;
70 private Servo servo_motor4;
71 int i = 1;
72 int Size = 4;
73 /*
74 * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
75 * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
76 * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
77 * web site at https://developer.vuforia.com/license-manager.
78 *
79 * Vuforia license keys are always 380 characters long, and look as if they contain mostly
80 * random data. As an example, here is a example of a fragment of a valid key:
81 * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
82 * Once you've obtained a license key, copy the string from the Vuforia web site
83 * and paste it in to your code on the next line, between the double quotes.
84 */
85 private static final String VUFORIA_KEY =
86 "Afdiq+j/////AAABmaTxPeCAukvgtegJjSCZmWky13NOi0sIIuLe5bcxwvDCt+eeILCnAoBdhAkcJkOg92E/c27XfWR2cLrr0JUbxIQ+9Dn9Z/7ca/aMqFE4wTWxe9gMESBHiVIeWzaYj/U1jEP6omOMcELmxLog4lGi4cuPwZmYwtlQWeN7+He01nbwHpFIScJQhvoZBPj12ZuBu9N1dge3qFsv4BLvlUn0GDqL69BN+x5UG4qtAoQvqNiKa1V/w4HTJLRk/Xm18ttCRM9OWOLWydoBORpHIvTrLKcrDUinZ/gPYvru+QS7T5kGCu0f19C7AFA1m1lKTf8ZBOolmlb4og/5gkkPvDBruoUdo7oWhC+BJ1UvsTJxrcqa";
87
88 /**
89 * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
90 * localization engine.
91 */
92 private VuforiaLocalizer vuforia;
93
94 /**
95 * {@link #tfod} is the variable we will use to store our instance of the TensorFlow Object
96 * Detection engine.
97 */
98 private TFObjectDetector tfod;
99
100 @Override
101 public void runOpMode() {
102 // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
103 // first.
104 right_drive = hardwareMap.dcMotor.get("right_drive");
105 left_drive = hardwareMap.dcMotor.get("left_drive");
106 left_front = hardwareMap.dcMotor.get("left_front");
107 right_front = hardwareMap.dcMotor.get("right_front");
108 h_drive = hardwareMap.dcMotor.get("h_drive");
109 arm_motor = hardwareMap.dcMotor.get("arm_motor");
110 servo_motor = hardwareMap.servo.get("servo_motor");
111 servo_motor2 = hardwareMap.servo.get("servo_motor2");
112 servo_motor4 = hardwareMap.servo.get("servo_motor4");
113 right_drive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
114 left_drive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
115 left_front.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
116 right_front.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
117 initVuforia();
118
119 if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
120 initTfod();
121 } else {
122 telemetry.addData("Sorry!", "This device is not compatible with TFOD");
123 }
124
125 /**
126 * Activate TensorFlow Object Detection before we wait for the start command.
127 * Do it here so that the Camera Stream window will have the TensorFlow annotations visible.
128 **/
129 if (tfod != null) {
130 tfod.activate();
131 }
132
133 /** Wait for the game to begin */
134 telemetry.addData(">", "Press Play to start op mode");
135 telemetry.update();
136
137
138
139 waitForStart();
140 telemetry.addData(">", "Running");
141 telemetry.update();
142 if (opModeIsActive()) {
143
144 List<Recognition> listOfThings = tfod.getUpdatedRecognitions();
145 // telemetry.addData("List:", listOfThings);
146 right_front.setPower(-1);
147 left_front.setPower(1);
148 right_drive.setPower(1);
149 left_drive.setPower(-1);
150 sleep(1000);
151 right_front.setPower(0);
152 left_front.setPower(0);
153 right_drive.setPower(0);
154 left_drive.setPower(0);
155 sleep(500);
156 sleep(1000);
157
158 // List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
159
160 { while(!listOfThings.isEmpty()){ telemetry.addLine("running");
161 if(tfod.getUpdatedRecognitions() != null){
162 listOfThings = tfod.getUpdatedRecognitions();
163 }
164 telemetry.addData("List", listOfThings);
165 telemetry.update();
166 h_drive.setPower(1);
167 sleep(100);
168 if(listOfThings.indexOf("Skystone") != -1){
169 servo_motor4.setPosition(1);
170 sleep(1000);
171
172 }
173
174}
175}
176
177 { while(listOfThings.equals("NullPointerException"));
178 while(!listOfThings.equals("Skystone")){
179 if(tfod.getUpdatedRecognitions()!=null){
180 // if(tfod.getUpdatedRecognitions.size)
181 // listOfThings = tfod.getUpdatedRecognitions();
182 }
183
184 h_drive.setPower(0.25);
185 }
186 servo_motor4.setPosition(1);
187
188
189
190
191
192
193
194
195
196 }
197
198
199
200
201
202 //telemetry.update();
203
204 }
205
206 }
207
208
209
210 //-*
211
212
213
214
215 //after you see skystone
216
217
218
219
220
221
222
223
224
225
226
227
228 // while (opModeIsActive()) {
229 // if (tfod != null) {
230 // // getUpdatedRecognitions() will return null if no new information is available since
231 // // the last time that call was made.
232 // List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
233 // if (updatedRecognitions != null) {
234 // telemetry.addData("# Object Detected", updatedRecognitions.size());
235 // telemetry.addData("UpdateRecongnition",updatedRecognitions);
236 // // step through the list of recognitions and display boundary info.
237 // int i = 0;
238 // for (Recognition recognition : updatedRecognitions) {
239 // telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
240 // telemetry.addData(String.format(" left,top (%d)", i), "%.03f , %.03f",
241 // recognition.getLeft(), recognition.getTop());
242 // telemetry.addData(String.format(" right,bottom (%d)", i), "%.03f , %.03f",
243 // recognition.getRight(), recognition.getBottom());
244 // }
245 // telemetry.update();
246 // }
247 // }
248 // }
249 // }
250
251 // if (tfod != null) {
252 // tfod.shutdown();
253 // }
254
255
256
257
258 /**
259 * Initialize the Vuforia localization engine.
260 */
261 private void initVuforia() {
262 /*
263 * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
264 */
265 VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
266
267 parameters.vuforiaLicenseKey = VUFORIA_KEY;
268 parameters.cameraDirection = CameraDirection.BACK;
269
270 // Instantiate the Vuforia engine
271 vuforia = ClassFactory.getInstance().createVuforia(parameters);
272
273 // Loading trackables is not necessary for the TensorFlow Object Detection engine.
274 }
275
276 /**
277 * Initialize the TensorFlow Object Detection engine.
278 */
279 private void initTfod() {
280 int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
281 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
282 TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
283 tfodParameters.minimumConfidence = 0.5;
284 tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
285 tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
286 }
287
288}