TimeCrafters/UltimateGoal
1
0
Fork 0
mirror of https://github.com/TimeCrafters/UltimateGoal.git synced 2025-12-16 06:02:33 +00:00
Code Issues Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/FIRST-Tech-Challenge/FtcRobotController

Browse Source
This commit is contained in:
cyberarm
2021-07-24 15:10:46 +00:00
parent 59d79b117e cd037e0e74
commit 93fcfef1c6
16 changed files with 169 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
TeamCode/src/main/java/org/samples/ConceptVuforiaUltimateGoalNavigationWebcam.java
View File

@@ -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
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: