· 5 years ago · Dec 16, 2020, 12:00 AM
1package org.firstinspires.ftc.teamcode;
2import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
3import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
4import com.qualcomm.robotcore.util.ElapsedTime;
5import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
6import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
7import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
8import org.firstinspires.ftc.robotcore.external.ClassFactory;
9import com.qualcomm.robotcore.hardware.DcMotor;
10
11
12import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
13import com.qualcomm.robotcore.hardware.Servo;
14import com.qualcomm.robotcore.eventloop.opmode.Disabled;
15import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
16import com.qualcomm.robotcore.util.ElapsedTime;
17//import com.qualcomm.robotcore.hardware.ColorSensor;
18
19import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
20import java.lang.annotation.Target;
21import java.util.Timer;
22import com.qualcomm.robotcore.robot.Robot;
23import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
24import com.qualcomm.robotcore.util.ElapsedTime;
25import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
26import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
27//import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
28import com.qualcomm.robotcore.hardware.ColorSensor;
29import com.qualcomm.robotcore.hardware.DcMotor;
30import org.firstinspires.ftc.robotcore.external.ClassFactory;
31import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
32import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
33import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
34//import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
35import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
36import com.qualcomm.robotcore.eventloop.opmode.Disabled;
37import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
38import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
39import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
40import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
41import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
42import org.firstinspires.ftc.robotcore.external.navigation.VuMarkInstanceId;
43import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
44import java.util.List;
45
46
47
48import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
49import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
50import com.qualcomm.robotcore.eventloop.opmode.Disabled;
51import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
52
53import org.firstinspires.ftc.robotcore.external.ClassFactory;
54import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
55import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
56import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
57import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
58import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
59import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
60import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
61import org.firstinspires.ftc.robotcore.external.navigation.VuMarkInstanceId;
62import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
63import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
64import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
65import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
66
67/**
68 * This 2020-2021 OpMode illustrates the basics of using the TensorFlow Object Detection API to
69 * determine the position of the Ultimate Goal game elements.
70 *
71 * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
72 * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
73 *
74 * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
75 * is explained below.
76 */
77
78@Autonomous(name="MeccanumDriveAuto", group="2020/2021 auto programms")
79public class MeccanumDriveAuto extends LinearOpMode {
80 private static final String TFOD_MODEL_ASSET = "UltimateGoal.tflite";
81 private static final String LABEL_FIRST_ELEMENT = "Quad";
82 private static final String LABEL_SECOND_ELEMENT = "Single";
83 public int Timer = 10001;
84 public int Threshold = 10000;
85 public int didItDo = 0;
86 public String Ring = "blahblah";
87 public int Attempts = 0;
88 public String Labels = "";
89 public int colorAttempts = 0;
90 // color sensor stuff (declarations)
91 final double SCALE_FACTOR = 255;
92 float hsvValues[] = {0F, 0F, 0F};
93 final float values[] = hsvValues;
94 int isRing = 2; /* 1 = yes | 0 = no */
95 double ringColor;
96 double ringRGB;
97 double ringMinus2;
98 double ringPlus2;
99 public int button = 0;
100
101
102
103 UltimateGoalHardware robot = new UltimateGoalHardware();
104 private ElapsedTime runtime = new ElapsedTime();
105
106 static final double COUNTS_PER_MOTOR_REV = 1120 ;
107 static final double DRIVE_GEAR_REDUCTION = 2.0 ;
108 static final double WHEEL_DIAMETER_INCHES = 4.0 ;
109 static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
110 (WHEEL_DIAMETER_INCHES * 4);
111 static final double DRIVE_SPEED = 0.5;
112 static final double TURN_SPEED = 0.3;
113 static final double FORWARD_SPEED = 0.7;
114 static final double UNLATCH_SPEED = 0.8;
115 static final double LOWER_SPEED = 1.0;
116 static final double LEFT = 0.5;
117 static final double RIGHT = 0.5;
118 static final double LEFT2 = 0.5;
119 static final double RIGHT2 = 0.5;
120
121
122
123 /*
124 * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
125 * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
126 * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
127 * web site at https://developer.vuforia.com/license-manager.
128 *
129 * Vuforia license keys are always 380 characters long, and look as if they contain mostly
130 * random data. As an example, here is a example of a fragment of a valid key:
131 * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
132 * Once you've obtained a license key, copy the string from the Vuforia web site
133 * and paste it in to your code on the next line, between the double quotes.
134 */
135 private static final String VUFORIA_KEY =
136 "AWOq+Sb/////AAABmR8LgVv9q0czoyIWGYYU/k6GpmDKuquGhPsGgwsceOj2XRTcnDVVM3cBHsvhkm7ExnitUUTB38I28NbOVwL8XH1a8xeAHkDj6sq1P+tRE+b9+IyMt3AUGpuxI9HbBol7LoqjDD9gdTkMzX9UX4k4IYbg6I5a8v5M7VCSwxw4/LLXqx0wLgMLuElQlL/WZCndCc4oGZSNxkdC06PZiQiUUiaZBajRM6ExmrMcBjpzIXCYyKzcaeBeP29rG4TPLs3vdrXTvxuTw0Dm845DJTVvPlD6DmeW32njVdPk4KN3mBdnuYq33BeF5JVtbng1sY8R0IU3qDekxF65G1kg9JUyAR/tFbIwlglNJ0X6msOIN9H/";
137
138 /**
139 * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
140 * localization engine.
141 */
142 private VuforiaLocalizer vuforia;
143
144 /**
145 * {@link #tfod} is the variable we will use to store our instance of the TensorFlow Object
146 * Detection engine.
147 */
148 private TFObjectDetector tfod;
149
150 @Override
151 public void runOpMode() throws InterruptedException {
152 encoderDriveV2(0.5, 10, 10, 15);
153 sleep(1000);
154 mecanum_encoder_drive(0.5, 10, 10, 20);
155 }
156 /**
157 * Initialize the Vuforia localization engine.
158 */
159 private void initVuforia() {
160 /*
161 * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
162 */
163 VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
164
165 parameters.vuforiaLicenseKey = VUFORIA_KEY;
166 parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam1");
167
168 // Instantiate the Vuforia engine
169 vuforia = ClassFactory.getInstance().createVuforia(parameters);
170
171 // Loading trackables is not necessary for the TensorFlow Object Detection engine.
172 }
173
174 /**
175 * Initialize the TensorFlow Object Detection engine.
176 */
177 private void initTfod() {
178 int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
179 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
180 TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
181 tfodParameters.minResultConfidence = 0.8f;
182 tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
183 tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
184 }
185 //Wheel drive
186 public void encoderDriveV2(double speed,
187 double leftInches, double rightInches,
188 double timeoutS) {
189 robot.frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
190 robot.frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
191 robot.backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
192 robot.backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
193 int newLeftTarget;
194 int newRightTarget;
195 robot.frontLeft.setPower(0);
196 robot.frontRight.setPower(0);
197 robot.backLeft.setPower(0);
198 robot.backRight.setPower(0);
199 }
200 // Meccanum Drive
201 public void mecanum_encoder_drive(double speed,
202 double leftBackRightInches, double rightBackLeftInches,
203 double timeoutS) {
204 robot.frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
205 robot.frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
206 robot.backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
207 robot.backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
208 int newLeft_backRight_Target;
209 int newRight_backLeft_Target;
210
211 if (opModeIsActive()) {
212
213 newLeft_backRight_Target = robot.frontLeft.getCurrentPosition() + (int)(leftBackRightInches * COUNTS_PER_INCH);
214 newRight_backLeft_Target = robot.frontRight.getCurrentPosition() + (int)(rightBackLeftInches * COUNTS_PER_INCH);
215
216 robot.frontLeft.setTargetPosition(newLeft_backRight_Target);
217 robot.frontRight.setTargetPosition(newRight_backLeft_Target);
218 robot.backLeft.setTargetPosition(newRight_backLeft_Target);
219 robot.backRight.setTargetPosition(newLeft_backRight_Target);
220
221 robot.frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
222 robot.frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
223 robot.backLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
224 robot.backRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
225
226
227 runtime.reset();
228 robot.frontLeft.setPower(Math.abs(speed));
229 robot.frontRight.setPower(Math.abs(speed));
230 robot.backLeft.setPower(Math.abs(speed));
231 robot.backRight.setPower(Math.abs(speed));
232
233 while (opModeIsActive() &&
234 (runtime.seconds() < timeoutS) &&
235 (robot.frontLeft.isBusy() && robot.frontRight.isBusy())) {
236
237
238 telemetry.addData("Path1", "Running to %7d :%7d", newLeft_backRight_Target, newRight_backLeft_Target);
239 // telemetry.addData("Path", "Running at %7d :%7d",
240 robot.frontLeft.getCurrentPosition();
241 robot.frontRight.getCurrentPosition();
242 robot.backLeft.getCurrentPosition();
243 robot.backRight.getCurrentPosition();
244 telemetry.update();
245 }
246
247 // Stop all motion;
248 robot.frontLeft.setPower(0);
249 robot.frontRight.setPower(0);
250 robot.backLeft.setPower(0);
251 robot.backRight.setPower(0);
252
253 // Turn off RUN_TO_POSITION
254 robot.frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
255 robot.frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
256 robot.backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
257 robot.backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
258
259 sleep(500); // optional pause after each move
260 }
261 }
262
263
264
265
266}
267