|
|
|
|
@ -48,9 +48,8 @@ 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.AxesOrder.XZY;
|
|
|
|
|
import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
|
|
|
|
|
import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.BACK;
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* This 2020-2021 OpMode illustrates the basics of using the Vuforia localizer to determine
|
|
|
|
|
@ -89,10 +88,6 @@ import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocaliz
|
|
|
|
|
@Disabled
|
|
|
|
|
public class ConceptVuforiaUltimateGoalNavigationWebcam extends LinearOpMode {
|
|
|
|
|
|
|
|
|
|
// IMPORTANT: If you are using a USB WebCam, you must select CAMERA_CHOICE = BACK; and PHONE_IS_PORTRAIT = false;
|
|
|
|
|
private static final VuforiaLocalizer.CameraDirection CAMERA_CHOICE = BACK;
|
|
|
|
|
private static final boolean PHONE_IS_PORTRAIT = false ;
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
|
|
|
|
|
* 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
|
|
|
|
|
@ -140,8 +135,9 @@ public class ConceptVuforiaUltimateGoalNavigationWebcam extends LinearOpMode {
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
|
|
|
|
|
* We can pass Vuforia the handle to a camera preview resource (on the RC phone);
|
|
|
|
|
* We can pass Vuforia the handle to a camera preview resource (on the RC screen);
|
|
|
|
|
* If no camera monitor is desired, use the parameter-less constructor instead (commented out below).
|
|
|
|
|
* Note: A preview window is required if you want to view the camera stream on the Driver Station Phone.
|
|
|
|
|
*/
|
|
|
|
|
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
|
|
|
|
|
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
|
|
|
|
|
@ -207,12 +203,12 @@ public class ConceptVuforiaUltimateGoalNavigationWebcam extends LinearOpMode {
|
|
|
|
|
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0)));
|
|
|
|
|
frontWallTarget.setLocation(OpenGLMatrix
|
|
|
|
|
.translation(-halfField, 0, mmTargetHeight)
|
|
|
|
|
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90)));
|
|
|
|
|
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 90)));
|
|
|
|
|
|
|
|
|
|
// The tower goal targets are located a quarter field length from the ends of the back perimeter wall.
|
|
|
|
|
blueTowerGoalTarget.setLocation(OpenGLMatrix
|
|
|
|
|
.translation(halfField, quadField, mmTargetHeight)
|
|
|
|
|
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , -90)));
|
|
|
|
|
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90)));
|
|
|
|
|
redTowerGoalTarget.setLocation(OpenGLMatrix
|
|
|
|
|
.translation(halfField, -quadField, mmTargetHeight)
|
|
|
|
|
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90)));
|
|
|
|
|
@ -220,42 +216,33 @@ public class ConceptVuforiaUltimateGoalNavigationWebcam extends LinearOpMode {
|
|
|
|
|
//
|
|
|
|
|
// Create a transformation matrix describing where the phone is on the robot.
|
|
|
|
|
//
|
|
|
|
|
// NOTE !!!! It's very important that you turn OFF your phone's Auto-Screen-Rotation option.
|
|
|
|
|
// Lock it into Portrait for these numbers to work.
|
|
|
|
|
//
|
|
|
|
|
// Info: The coordinate frame for the robot looks the same as the field.
|
|
|
|
|
// The robot's "forward" direction is facing out along X axis, with the LEFT side facing out along the Y axis.
|
|
|
|
|
// Z is UP on the robot. This equates to a bearing angle of Zero degrees.
|
|
|
|
|
//
|
|
|
|
|
// The phone starts out lying flat, with the screen facing Up and with the physical top of the phone
|
|
|
|
|
// pointing to the LEFT side of the Robot.
|
|
|
|
|
// The two examples below assume that the camera is facing forward out the front of the robot.
|
|
|
|
|
// For a WebCam, the default starting orientation of the camera is looking UP (pointing in the Z direction),
|
|
|
|
|
// with the wide (horizontal) axis of the camera aligned with the X axis, and
|
|
|
|
|
// the Narrow (vertical) axis of the camera aligned with the Y axis
|
|
|
|
|
//
|
|
|
|
|
// But, this example assumes that the camera is actually facing forward out the front of the robot.
|
|
|
|
|
// So, the "default" camera position requires two rotations to get it oriented correctly.
|
|
|
|
|
// 1) First it must be rotated +90 degrees around the X axis to get it horizontal (it's now facing out the right side of the robot)
|
|
|
|
|
// 2) Next it must be be rotated +90 degrees (counter-clockwise) around the Z axis to face forward.
|
|
|
|
|
//
|
|
|
|
|
// Finally the camera can be translated to its actual mounting position on the robot.
|
|
|
|
|
// In this example, it is centered (left to right), but 4" forward of the middle of the robot, and 8" above ground level.
|
|
|
|
|
|
|
|
|
|
// We need to rotate the camera around it's long axis to bring the correct camera forward.
|
|
|
|
|
if (CAMERA_CHOICE == BACK) {
|
|
|
|
|
phoneYRotate = -90;
|
|
|
|
|
} else {
|
|
|
|
|
phoneYRotate = 90;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Rotate the phone vertical about the X axis if it's in portrait mode
|
|
|
|
|
if (PHONE_IS_PORTRAIT) {
|
|
|
|
|
phoneXRotate = 90 ;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Next, translate the camera lens to where it is on the robot.
|
|
|
|
|
// In this example, it is centered (left to right), but forward of the middle of the robot, and above ground level.
|
|
|
|
|
final float CAMERA_FORWARD_DISPLACEMENT = 4.0f * mmPerInch; // eg: Camera is 4 Inches in front of robot-center
|
|
|
|
|
final float CAMERA_VERTICAL_DISPLACEMENT = 8.0f * mmPerInch; // eg: Camera is 8 Inches above ground
|
|
|
|
|
final float CAMERA_LEFT_DISPLACEMENT = 0; // eg: Camera is ON the robot's center line
|
|
|
|
|
|
|
|
|
|
OpenGLMatrix robotFromCamera = OpenGLMatrix
|
|
|
|
|
OpenGLMatrix cameraLocationOnRobot = OpenGLMatrix
|
|
|
|
|
.translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT)
|
|
|
|
|
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, phoneYRotate, phoneZRotate, phoneXRotate));
|
|
|
|
|
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XZY, DEGREES, 90, 90, 0));
|
|
|
|
|
|
|
|
|
|
/** Let all the trackable listeners know where the phone is. */
|
|
|
|
|
for (VuforiaTrackable trackable : allTrackables) {
|
|
|
|
|
((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(robotFromCamera, parameters.cameraDirection);
|
|
|
|
|
((VuforiaTrackableDefaultListener) trackable.getListener()).setCameraLocationOnRobot(parameters.cameraName, cameraLocationOnRobot);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// WARNING:
|
|
|
|
|
|
Cal Kestis