Skip to content

Commit

Permalink
uhhh, i think i did something
Browse files Browse the repository at this point in the history 
added camera into FieldCentricBlue
  • Loading branch information
@abirarun
abirarun committed Sep 23, 2024
1 parent a302e5d commit 60e7bcb
Showing 1 changed file with 74 additions and 6 deletions.
80 changes: 74 additions & 6 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FieldCentricBlue.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,22 +18,40 @@
import com.qualcomm.robotcore.hardware.Servo;
//servo impost did not come with the class. so just incase you make a new one please make sure to put import servo. thank you
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;

import java.util.List;

@TeleOp
public class FieldCentricBlue
extends LinearOpMode {
private static final boolean USE_WEBCAM = true;
private Position cameraPosition = new Position(DistanceUnit.INCH,
5, 7, 0, 0);
private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES,
0, -90, 0, 0);
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
IntegratingGyroscope gyro;
NavxMicroNavigationSensor navxMicro;
ElapsedTime timer = new ElapsedTime();

@Override
public void runOpMode() throws InterruptedException {
initAprilTag();
StraferHardware hardware = new StraferHardware(hardwareMap);
navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "gyro");
gyro = (IntegratingGyroscope) navxMicro;
Expand Down Expand Up @@ -66,6 +84,8 @@ public void runOpMode() throws InterruptedException {
double distance = 100;
boolean haveTurned = false;
while (opModeIsActive()) {
telemetryAprilTag();
telemetry.update();

// this is the gamepad controls for the driving. good luck
double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
Expand Down Expand Up @@ -99,13 +119,13 @@ public void runOpMode() throws InterruptedException {

// double color= ribbit.argb();
distance = sensor.getDistance(INCH);
telemetry.addData("distance", distance);
// telemetry.addData("distance", distance);
// telemetry.addData("color",color);

AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES);
// Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);

telemetry.addLine()
/* telemetry.addLine()
.addData("dx", formatRate(rates.xRotationRate))
.addData("dy", formatRate(rates.yRotationRate))
.addData("dz", "%s deg/s", formatRate(rates.zRotationRate));
Expand All @@ -115,7 +135,7 @@ public void runOpMode() throws InterruptedException {
.addData("roll", formatAngle(angles.angleUnit, angles.secondAngle))
.addData("pitch", "%s deg", formatAngle(angles.angleUnit, angles.thirdAngle));
telemetry.update();

*/
handservo(1.0);

if (gamepad1.dpad_up) {
Expand All @@ -129,12 +149,60 @@ public void runOpMode() throws InterruptedException {
if (gamepad1.a) {
turn2(-90);
}
Rumble();
//Rumble();

idle();

}

}
private void initAprilTag() {

// Create the AprilTag processor.
aprilTag = new AprilTagProcessor.Builder()
.setCameraPose(cameraPosition, cameraOrientation)
.build();

VisionPortal.Builder builder = new VisionPortal.Builder();

// Set the camera (webcam vs. built-in RC phone camera).
if (USE_WEBCAM) {
builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"));
} else {
builder.setCamera(BuiltinCameraDirection.BACK);
}

builder.addProcessor(aprilTag);
visionPortal = builder.build();

}
private void telemetryAprilTag() {

List<AprilTagDetection> currentDetections = aprilTag.getDetections();
telemetry.addData("# AprilTags Detected", currentDetections.size());

// Step through the list of detections and display info for each one.
for (AprilTagDetection detection : currentDetections) {
if (detection.metadata != null) {
telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)",
detection.robotPose.getPosition().x,
detection.robotPose.getPosition().y,
detection.robotPose.getPosition().z));
telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)",
detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES),
detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES),
detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES)));
} else {
telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y));
}
} // end for() loop

// Add "key" information to telemetry
telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist.");
telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");

}

String formatRate(float rate) {
Expand Down Expand Up @@ -311,7 +379,7 @@ public void straightline(double power, double distance, double heading) {

}

public void Rumble() {
/* public void Rumble() {
StraferHardware hardware = new StraferHardware(hardwareMap);
ColorSensor ribbit = hardwareMap.get(ColorSensor.class, "colorSensor");
double red = ribbit.red();
Expand All @@ -328,7 +396,7 @@ public void Rumble() {
}

*/
}


Expand Down

0 comments on commit 60e7bcb

Please sign in to comment.