forked from ftctechnh/ftc_app
-
Notifications
You must be signed in to change notification settings - Fork 0
/
GoldAlignExample.java
245 lines (201 loc) · 10.7 KB
/
GoldAlignExample.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
/* Copyright (c) 2017 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;
import com.disnodeteam.dogecv.CameraViewDisplay;
import com.disnodeteam.dogecv.DogeCV;
import com.disnodeteam.dogecv.detectors.roverrukus.GoldAlignDetector;
import com.disnodeteam.dogecv.Dogeforia;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
import java.util.ArrayList;
import java.util.List;
import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.FRONT;
@Autonomous(name="Gold Test", group="Aftershock")
public class GoldAlignExample extends OpMode {
//Gold align detector
private GoldAlignDetector detector;
//Robot hardware
private AftershockHardware2018 robot = new AftershockHardware2018();
//Parameters for the Dogeforia object
private VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
private VuforiaLocalizer.CameraDirection CAMERA_CHOICE = VuforiaLocalizer.CameraDirection.BACK;
//A variable for the Dogeforia object
private Dogeforia vuforia;
//Measurements for field
private static final float mmPerInch = 25.4f;
private static final float mmFTCFieldWidth = (12*6) * mmPerInch;
private static final float mmTargetHeight = (6) * mmPerInch;
private final int CAMERA_FORWARD_DISPLACEMENT = 110;
private final int CAMERA_VERTICAL_DISPLACEMENT = 200;
private final int CAMERA_LEFT_DISPLACEMENT = 0;
//Vuforia trackable objects
private List<VuforiaTrackable> allTrackables = new ArrayList<>();
@Override
public void init() {
//Init the hardware map
robot.init(hardwareMap);
//--------DogeCV--------
//Create a GoldAlignDetector that can also be used for Vuforia
detector = new GoldAlignDetector();
detector.init(hardwareMap.appContext, CameraViewDisplay.getInstance(), 0, true);
detector.useDefaults();
//Set some parameters for detector
detector.alignSize = 100;
detector.alignPosOffset = 0;
detector.downscale = 0.4;
//Set the detector to use the max area
detector.areaScoringMethod = DogeCV.AreaScoringMethod.MAX_AREA;
detector.maxAreaScorer.weight = 0.005;
//Ratio scoring settings
detector.ratioScorer.weight = 5;
detector.ratioScorer.perfectRatio = 1.0;
//--------Dogeforia--------
/*
//Vuforia VuMarks
VuforiaTrackables targetsRoverRuckus = this.vuforia.loadTrackablesFromAsset("RoverRuckus");
VuforiaTrackable blueRover = targetsRoverRuckus.get(0);
blueRover.setName("Blue_Rover");
VuforiaTrackable redFootprint = targetsRoverRuckus.get(1);
redFootprint.setName("Red_Footprint");
VuforiaTrackable frontCraters = targetsRoverRuckus.get(2);
frontCraters.setName("Front_Craters");
VuforiaTrackable backSpace = targetsRoverRuckus.get(3);
backSpace.setName("Back_Space");
//Trackable positions
allTrackables.addAll(targetsRoverRuckus);
OpenGLMatrix blueRoverLocationOnField = OpenGLMatrix
.translation(0, mmFTCFieldWidth, mmTargetHeight)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0));
blueRover.setLocation(blueRoverLocationOnField);
OpenGLMatrix redFootprintLocationOnField = OpenGLMatrix
.translation(0, -mmFTCFieldWidth, mmTargetHeight)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180));
redFootprint.setLocation(redFootprintLocationOnField);
OpenGLMatrix frontCratersLocationOnField = OpenGLMatrix
.translation(-mmFTCFieldWidth, 0, mmTargetHeight)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90));
frontCraters.setLocation(frontCratersLocationOnField);
OpenGLMatrix backSpaceLocationOnField = OpenGLMatrix
.translation(mmFTCFieldWidth, 0, mmTargetHeight)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90));
backSpace.setLocation(backSpaceLocationOnField);
OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix
.translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES,
CAMERA_CHOICE == FRONT ? 90 : -90, 0, 0));
for (VuforiaTrackable trackable : allTrackables)
{
((VuforiaTrackableDefaultListener)trackable.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection);
}
targetsRoverRuckus.activate();
//Vuforia licence key
parameters.vuforiaLicenseKey = "AeD6c6X/////AAABmZ3ExKW40UAfroKSDQ2JrLxH4Mlxt6oFF+x0qihPTikBy3IoT+Qj/R4aFwnmv5ERnhX5w58J8AWMCC1pceL0E5GTfwJP/8cjdY831tmQlwioOssywwG7E8MbZys8uzNdBkTOJvnG8vEenOM+8zKR5cqDhG4AOi3v6ydSz8OHGlJf9IcMwQrh9vKP/qAYAWQmZXMIN5KUhipe9VS1zmDEq3h/nqmR/fZcHDwesqJOA2vsfqro/QAfTQa1abnJoT5D1VnLxrmMVlmYP+0KNhxDfIIRGNQGPLTfjYGyFoQpxpNi5Li3XqKajdhruIkluDZ1PXfiRy6SODW2SsJk5aznfDvAqJnXkK2jT2miYSOhhsb1";
//Define a Dogeforia object
vuforia = new Dogeforia(parameters);
vuforia.enableConvertFrameToBitmap();
//Stup and start the Dogeforia object
vuforia.setDogeCVDetector(detector);
vuforia.enableDogeCV();
vuforia.showDebug();
vuforia.start();
*/
}
@Override
public void start() {
//Start rotating to find gold
robot.rightDrive.setPower(0.12);
robot.leftDrive.setPower(-0.39);
}
@Override
public void loop() {
//Some diagnostic data
telemetry.addData("IsAligned" , detector.getAligned()); // Is the bot aligned with the gold mineral
telemetry.addData("X Pos" , detector.getXPosition()); // Gold X pos.
if(!detector.getAligned()){
//Not aligned, keep going
telemetry.addLine("NOOOOO");
telemetry.update();
}else{
//Aligned!
telemetry.addLine("YASSSS");
telemetry.update();
//Drive forward
robot.rightDrive.setPower(0.3);
robot.leftDrive.setPower(0.3);
}
}
@Override
public void stop() {
//Disable and stop detectors
detector.disable();
//vuforia.stop();
}
public void getRobotPosition(){
boolean targetVisible = false;
OpenGLMatrix lastLocation = null;
for (VuforiaTrackable trackable : allTrackables) {
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
telemetry.addData("Visible Target", trackable.getName());
targetVisible = true;
// getUpdatedRobotLocation() will return null if no new information is available since
// the last time that call was made, or if the trackable is not currently visible.
OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
if (robotLocationTransform != null) {
lastLocation = robotLocationTransform;
}
break;
}
}
// Provide feedback as to where the robot is located (if we know).
if (targetVisible) {
// express position (translation) of robot in inches.
assert lastLocation != null : "Everything broke. PAAAANIC!";
VectorF translation = lastLocation.getTranslation();
telemetry.addData("Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f", translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);
// express the rotation of the robot in degrees.
Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES);
telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle);
}
else {
telemetry.addData("Visible Target", "none");
}
}
}