From 546aa7d31c412a5ca671ffadb1eb74e1bacb369f Mon Sep 17 00:00:00 2001 From: CrisianFG <147004159+CrisianFG@users.noreply.github.com> Date: Fri, 12 Jan 2024 16:32:42 -0600 Subject: [PATCH 01/11] camera subsystem test --- .../firstinspires/ftc/teamcode/Chassis.java | 4 +- .../ftc/teamcode/autonomous.java | 7 +- .../subsystems/AprilTagSubsystem.java | 168 +++++++++++++++++ .../teamcode/subsystems/CameraSubsystem.java | 96 ---------- .../teamcode/subsystems/ChassisSubsystem.java | 12 +- .../teamcode/subsystems/OpenCVSubsystem.java | 171 ++++++++++++++++++ 6 files changed, 350 insertions(+), 108 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java index e5afb58..c45d73c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java @@ -27,7 +27,7 @@ public class Chassis extends LinearOpMode { private Blinker control_Hub; - private double speed=1; + private double speed = 1; private IMU imu; private double baseSpeed=0.5; private Orientation orientation; @@ -139,7 +139,7 @@ public void runOpMode() { //this.telemetry = telemetry; control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); //imu = hardwareMap.get(IMU.class, "imu"); - if(Parameters.robot=="marvin"){ + if (Parameters.robot == "marvin") { intake = hardwareMap.get(DcMotor.class, "IntakeMotor"); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java index b59f19a..9180d81 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java @@ -2,16 +2,15 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.subsystems.CameraSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.OpenCVSubsystem; @Autonomous public class autonomous extends LinearOpMode { - private CameraSubsystem cameraSubsystem; + private OpenCVSubsystem openCVSubsystem; @Override public void runOpMode() { - cameraSubsystem = CameraSubsystem.getInstance(hardwareMap); + openCVSubsystem = OpenCVSubsystem.getInstance(hardwareMap); waitForStart(); while (opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java new file mode 100644 index 0000000..bf43c11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java @@ -0,0 +1,168 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +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.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +public class AprilTagSubsystem { + private static AprilTagSubsystem instance; + private static HardwareMap hardwareMap; + public AprilTagSubsystem(HardwareMap hardwareMap) { + OpenCVSubsystem.hardwareMap = hardwareMap; + } + + public static AprilTagSubsystem getInstance(HardwareMap hardwareMap){ + if (instance == null) { + instance = new AprilTagSubsystem(hardwareMap); + } + return instance; + } + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + waitForStart(); + + + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + + + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + +} // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + .setDrawAxes(false) + .setDrawCubeProjection(false) + .setDrawTagOutline(true) + .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + .setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + .setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + 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); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + builder.setCameraResolution(new Size(1080, 1920)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + builder.setAutoStopLiveView(true); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List 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.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } 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)); + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java deleted file mode 100644 index d825e2c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java +++ /dev/null @@ -1,96 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import android.graphics.Bitmap; -import android.graphics.BitmapFactory; -import android.util.Size; - -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; - -public class CameraSubsystem { - private static HardwareMap hardwareMap; - private static CameraSubsystem instance; - - private static VisionPortal visionPortal; - - private TfodProcessor tfod; - private CameraSubsystem(HardwareMap hardwareMap){ - this.hardwareMap = hardwareMap; - initVisionPortal(); - - } - public static CameraSubsystem getInstance(HardwareMap hardwareMap){ - if (instance == null) { - instance = new CameraSubsystem(hardwareMap); - } - return instance; - } - private void initVisionPortal(){ - VisionPortal.Builder builder = new VisionPortal.Builder(); - builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); - builder.setCameraResolution(new Size(1280, 720)); - builder.enableLiveView(true); - - tfod = TfodProcessor.easyCreateWithDefaults(); - - builder.addProcessor(tfod); - - visionPortal = builder.build(); - - getFrame(); - averageColor("blue", 10); - - } - private void getFrame() { - visionPortal.saveNextFrameRaw("frame.png"); // Assuming this returns the file path - - } - - private void averageColor(String color, int threshold) { - String filePath = "frame.png"; - Bitmap bitmap = BitmapFactory.decodeFile(filePath); - - if (bitmap != null) { - long totalColorValue = 0; - int count = 0; - int width = bitmap.getWidth(); - int height = bitmap.getHeight(); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - int pixel = bitmap.getPixel(x, y); - int value = 0; - - switch (color.toLowerCase()) { - case "red": - value = (pixel >> 16) & 0xff; - break; - case "green": - value = (pixel >> 8) & 0xff; - break; - case "blue": - value = pixel & 0xff; - break; - default: - throw new IllegalArgumentException("Invalid color: " + color); - } - - if (value >= threshold) { - totalColorValue += value; - count++; - } - } - } - - if (count > 0) { - float averageColorValue = totalColorValue / (float) count; - System.out.println("Average " + color + " Value: " + averageColorValue); - } else { - System.out.println("No pixels met the threshold for " + color); - } - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java index de21a5c..a65f6d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java @@ -128,13 +128,13 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre telemetry.addData("pi", Math.PI); //x = Math.cos(0.7) * x - Math.sin(0.7) * y; // cos(°) sin(°) //y = Math.cos(0.7) * x + Math.sin(0.7) * y; - frontRightPower=0; - backLeftPower=0; - frontLeftPower=0; - backRightPower=0; - if(Parameters.robot=="marvin"){ + frontRightPower = 0; + backLeftPower = 0; + frontLeftPower = 0; + backRightPower = 0; + if (Parameters.robot == "marvin") { frontLeftPower=-speed*(y + x - r); - }else{ + } else { } // = /*-*/speed*(y + x - r); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java new file mode 100644 index 0000000..492f98f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -0,0 +1,171 @@ +package org.firstinspires.ftc.teamcode.subsystems; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; +import org.opencv.imgproc.Moments; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; + +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.Odometry; + +import java.util.ArrayList; +import java.util.List; + +@TeleOp(name = "OpenCV Testing") + +public class OpenCVSubsystem extends LinearOpMode { + public static HardwareMap hardwareMap; + + public OpenCVSubsystem(HardwareMap hardwareMap) { + OpenCVSubsystem.hardwareMap = hardwareMap; + } + + public static OpenCVSubsystem getInstance(HardwareMap hardwareMap){ + OpenCVSubsystem instance = new OpenCVSubsystem(hardwareMap); + if (instance == null) { + } + return instance; + } + + double cX = 0; + double cY = 0; + double width = 0; + + private OpenCvCamera controlHubCam; // Use OpenCvCamera class from FTC SDK + private static final int CAMERA_WIDTH = 640; // width of wanted camera resolution + private static final int CAMERA_HEIGHT = 360; // height of wanted camera resolution + + // Calculate the distance using the formula + public static final double objectWidthInRealWorldUnits = 3.75; // Replace with the actual width of the object in real-world units + public static final double focalLength = 728; // Replace with the focal length of the camera in pixels + + + @Override + public void runOpMode() { + + initOpenCV(); + waitForStart(); + + while (opModeIsActive()) { + telemetry.addData("Coordinate", "(" + (int) cX + ", " + (int) cY + ")"); + telemetry.addData("Distance in Inch", (getDistance(width))); + telemetry.update(); + + // The OpenCV pipeline automatically processes frames and handles detection + } + + // Release resources + controlHubCam.stopStreaming(); + } + + private void initOpenCV() { + + // Create an instance of the camera + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + + // Use OpenCvCameraFactory class from FTC SDK to create camera instance + controlHubCam = OpenCvCameraFactory.getInstance().createWebcam( + hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + + controlHubCam.setPipeline(new YellowBlobDetectionPipeline()); + + controlHubCam.openCameraDevice(); + controlHubCam.startStreaming(CAMERA_WIDTH, CAMERA_HEIGHT, OpenCvCameraRotation.UPRIGHT); + } + class YellowBlobDetectionPipeline extends OpenCvPipeline { + @Override + public Mat processFrame(Mat input) { + // Preprocess the frame to detect yellow regions + Mat yellowMask = preprocessFrame(input); + + // Find contours of the detected yellow regions + List contours = new ArrayList<>(); + Mat hierarchy = new Mat(); + Imgproc.findContours(yellowMask, contours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); + + // Find the largest yellow contour (blob) + MatOfPoint largestContour = findLargestContour(contours); + + if (largestContour != null) { + // Draw a red outline around the largest detected object + Imgproc.drawContours(input, contours, contours.indexOf(largestContour), new Scalar(255, 0, 0), 2); + // Calculate the width of the bounding box + width = calculateWidth(largestContour); + + // Display the width next to the label + String widthLabel = "Width: " + (int) width + " pixels"; + Imgproc.putText(input, widthLabel, new Point(cX + 10, cY + 20), Imgproc.FONT_HERSHEY_SIMPLEX, 0.5, new Scalar(0, 255, 0), 2); + //Display the Distance + String distanceLabel = "Distance: " + String.format("%.2f", getDistance(width)) + " inches"; + Imgproc.putText(input, distanceLabel, new Point(cX + 10, cY + 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.5, new Scalar(0, 255, 0), 2); + // Calculate the centroid of the largest contour + Moments moments = Imgproc.moments(largestContour); + cX = moments.get_m10() / moments.get_m00(); + cY = moments.get_m01() / moments.get_m00(); + + // Draw a dot at the centroid + String label = "(" + (int) cX + ", " + (int) cY + ")"; + Imgproc.putText(input, label, new Point(cX + 10, cY), Imgproc.FONT_HERSHEY_COMPLEX, 0.5, new Scalar(0, 255, 0), 2); + Imgproc.circle(input, new Point(cX, cY), 5, new Scalar(0, 255, 0), -1); + + } + + return input; + } + + private Mat preprocessFrame(Mat frame) { + Mat hsvFrame = new Mat(); + Imgproc.cvtColor(frame, hsvFrame, Imgproc.COLOR_BGR2HSV); + + Scalar lowerYellow = new Scalar(5,50,50); + Scalar upperYellow = new Scalar(15,255,255); + + + Mat yellowMask = new Mat(); + Core.inRange(hsvFrame, lowerYellow, upperYellow, yellowMask); + + Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); + Imgproc.morphologyEx(yellowMask, yellowMask, Imgproc.MORPH_OPEN, kernel); + Imgproc.morphologyEx(yellowMask, yellowMask, Imgproc.MORPH_CLOSE, kernel); + + return yellowMask; + } + + private MatOfPoint findLargestContour(List contours) { + double maxArea = 0; + MatOfPoint largestContour = null; + + for (MatOfPoint contour : contours) { + double area = Imgproc.contourArea(contour); + if (area > maxArea) { + maxArea = area; + largestContour = contour; + } + } + + return largestContour; + } + private double calculateWidth(MatOfPoint contour) { + Rect boundingRect = Imgproc.boundingRect(contour); + return boundingRect.width; + } + + } + private static double getDistance(double width){ + double distance = (objectWidthInRealWorldUnits * focalLength) / width; + return distance; + } + + +} \ No newline at end of file From 8d9da1226dd760298482c6caefbaaeb089e00480 Mon Sep 17 00:00:00 2001 From: CrisianFG <147004159+CrisianFG@users.noreply.github.com> Date: Sat, 13 Jan 2024 12:32:08 -0600 Subject: [PATCH 02/11] camera subsystems implementation (bugged) --- .../firstinspires/ftc/teamcode/Chassis.java | 3 - .../ftc/teamcode/autonomous.java | 37 +++++++++++ .../subsystems/AprilTagSubsystem.java | 63 +++++-------------- .../{Odometry.java => OdometrySubsystem.java} | 10 +-- .../teamcode/subsystems/OpenCVSubsystem.java | 43 +++---------- .../ftc/teamcode/tests/EncoderTest.java.d | 2 +- 6 files changed, 70 insertions(+), 88 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/{Odometry.java => OdometrySubsystem.java} (86%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java index c45d73c..a63b3bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java @@ -3,7 +3,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; -import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -12,11 +11,9 @@ import com.qualcomm.robotcore.hardware.Blinker; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.Odometry; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.teamcode.subsystems.Parameters; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java index 9180d81..9180055 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java @@ -2,19 +2,56 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Blinker; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.teamcode.subsystems.AprilTagSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; import org.firstinspires.ftc.teamcode.subsystems.OpenCVSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.Parameters; @Autonomous public class autonomous extends LinearOpMode { private OpenCVSubsystem openCVSubsystem; + private AprilTagSubsystem aprilTagSubsystem; + private ChassisSubsystem chassisSubsystem; + private GyroscopeSubsystem gyroscopeSubsystem; + //private ElevatorSubsystem elevatorSubsystem; + //private OdometrySubsystem odometrySubsystem; + private Blinker control_Hub; + private double speed = 1; + private IMU imu; + private double baseSpeed = 0.5; + private Orientation orientation; + private DcMotor intake; + boolean hasTarget = false; + public double gyr; @Override public void runOpMode() { + //this.telemetry = telemetry; + control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); + //imu = hardwareMap.get(IMU.class, "imu"); + if (Parameters.robot == "marvin") { + intake = hardwareMap.get(DcMotor.class, "IntakeMotor"); + } + openCVSubsystem = OpenCVSubsystem.getInstance(hardwareMap); + aprilTagSubsystem = AprilTagSubsystem.getInstance(hardwareMap,telemetry); + chassisSubsystem = ChassisSubsystem.getInstance(hardwareMap,telemetry); + gyroscopeSubsystem = GyroscopeSubsystem.getInstance(hardwareMap); + //elevatorSubsystem = ElevatorSubsystem.getInstance(hardwareMap); + //odometrySubsystem = OdometrySubsystem.getInstance(hardwareMap, chassisSubsystem); waitForStart(); while (opModeIsActive()) { + telemetry.addData("Status", "Running"); + //telemetry.addData("Width: ", openCVSubsystem.width); telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java index bf43c11..e6bda75 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java @@ -19,14 +19,17 @@ public class AprilTagSubsystem { private static AprilTagSubsystem instance; - private static HardwareMap hardwareMap; - public AprilTagSubsystem(HardwareMap hardwareMap) { - OpenCVSubsystem.hardwareMap = hardwareMap; + private HardwareMap hardwareMap; + private Telemetry telemetry; + public AprilTagSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.hardwareMap = hardwareMap; + this.telemetry = telemetry; + initAprilTag(); } - public static AprilTagSubsystem getInstance(HardwareMap hardwareMap){ + public static AprilTagSubsystem getInstance(HardwareMap hardwareMap, Telemetry telemetry) { if (instance == null) { - instance = new AprilTagSubsystem(hardwareMap); + instance = new AprilTagSubsystem(hardwareMap, telemetry); } return instance; } @@ -43,41 +46,7 @@ public static AprilTagSubsystem getInstance(HardwareMap hardwareMap){ */ private VisionPortal visionPortal; - initAprilTag(); - // Wait for the DS start button to be touched. - telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); - telemetry.addData(">", "Touch Play to start OpMode"); - telemetry.update(); - waitForStart(); - - - - telemetryAprilTag(); - - // Push telemetry to the Driver Station. - telemetry.update(); - - // Save CPU resources; can resume streaming when needed. - if (gamepad1.dpad_down) { - visionPortal.stopStreaming(); - } else if (gamepad1.dpad_up) { - visionPortal.resumeStreaming(); - } - - // Share the CPU. - sleep(20); - - - - // Save more CPU resources when camera is no longer needed. - visionPortal.close(); - -} // end method runOpMode() - - /** - * Initialize the AprilTag processor. - */ private void initAprilTag() { // Create the AprilTag processor. @@ -119,7 +88,7 @@ private void initAprilTag() { } // Choose a camera resolution. Not all cameras support all resolutions. - builder.setCameraResolution(new Size(1080, 1920)); + builder.setCameraResolution(new Size(640, 360)); // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. builder.enableLiveView(true); @@ -143,10 +112,6 @@ private void initAprilTag() { } // end method initAprilTag() - - /** - * Add telemetry about AprilTag detections. - */ private void telemetryAprilTag() { List currentDetections = aprilTag.getDetections(); @@ -163,6 +128,12 @@ private void telemetryAprilTag() { 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)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odometry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java similarity index 86% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odometry.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index 965535d..a96ecd2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odometry.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -4,7 +4,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; -public class Odometry { +public class OdometrySubsystem { public int fieldX; public int fieldY; @@ -18,16 +18,16 @@ public class Odometry { private ChassisSubsystem chassis; - private static Odometry instance; - public Odometry(HardwareMap hardwareMap, ChassisSubsystem chassis){ + private static OdometrySubsystem instance; + public OdometrySubsystem(HardwareMap hardwareMap, ChassisSubsystem chassis){ this.hardwareMap=hardwareMap; this.xEncoder = hardwareMap.get(DcMotor.class, "xEncoder"); this.yEncoder = hardwareMap.get(DcMotor.class, "yEncoder"); this.chassis = chassis; } - public static Odometry getInstance(HardwareMap hardwareMap, ChassisSubsystem chassis){ + public static OdometrySubsystem getInstance(HardwareMap hardwareMap, ChassisSubsystem chassis){ if (instance == null) { - instance = new Odometry(hardwareMap, chassis); + instance = new OdometrySubsystem(hardwareMap, chassis); } return instance; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index 492f98f..2a4b9de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -1,8 +1,8 @@ package org.firstinspires.ftc.teamcode.subsystems; +import static java.lang.Thread.sleep; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; - import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; @@ -12,22 +12,15 @@ import org.openftc.easyopencv.OpenCvCameraRotation; import org.openftc.easyopencv.OpenCvPipeline; -import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.Odometry; - import java.util.ArrayList; import java.util.List; -@TeleOp(name = "OpenCV Testing") - -public class OpenCVSubsystem extends LinearOpMode { +public class OpenCVSubsystem { public static HardwareMap hardwareMap; public OpenCVSubsystem(HardwareMap hardwareMap) { OpenCVSubsystem.hardwareMap = hardwareMap; + initOpenCV(); } public static OpenCVSubsystem getInstance(HardwareMap hardwareMap){ @@ -39,7 +32,7 @@ public static OpenCVSubsystem getInstance(HardwareMap hardwareMap){ double cX = 0; double cY = 0; - double width = 0; + public double width = 0; private OpenCvCamera controlHubCam; // Use OpenCvCamera class from FTC SDK private static final int CAMERA_WIDTH = 640; // width of wanted camera resolution @@ -49,25 +42,6 @@ public static OpenCVSubsystem getInstance(HardwareMap hardwareMap){ public static final double objectWidthInRealWorldUnits = 3.75; // Replace with the actual width of the object in real-world units public static final double focalLength = 728; // Replace with the focal length of the camera in pixels - - @Override - public void runOpMode() { - - initOpenCV(); - waitForStart(); - - while (opModeIsActive()) { - telemetry.addData("Coordinate", "(" + (int) cX + ", " + (int) cY + ")"); - telemetry.addData("Distance in Inch", (getDistance(width))); - telemetry.update(); - - // The OpenCV pipeline automatically processes frames and handles detection - } - - // Release resources - controlHubCam.stopStreaming(); - } - private void initOpenCV() { // Create an instance of the camera @@ -83,8 +57,9 @@ private void initOpenCV() { controlHubCam.openCameraDevice(); controlHubCam.startStreaming(CAMERA_WIDTH, CAMERA_HEIGHT, OpenCvCameraRotation.UPRIGHT); } - class YellowBlobDetectionPipeline extends OpenCvPipeline { - @Override + public MatOfPoint largestContour; + public class YellowBlobDetectionPipeline extends OpenCvPipeline { + public Mat processFrame(Mat input) { // Preprocess the frame to detect yellow regions Mat yellowMask = preprocessFrame(input); @@ -95,7 +70,7 @@ public Mat processFrame(Mat input) { Imgproc.findContours(yellowMask, contours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); // Find the largest yellow contour (blob) - MatOfPoint largestContour = findLargestContour(contours); + largestContour = findLargestContour(contours); if (largestContour != null) { // Draw a red outline around the largest detected object @@ -121,6 +96,8 @@ public Mat processFrame(Mat input) { } + + return input; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/EncoderTest.java.d b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/EncoderTest.java.d index eac79e1..b8b0a47 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/EncoderTest.java.d +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/EncoderTest.java.d @@ -10,7 +10,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Orientation; import com.qualcomm.robotcore.hardware.Blinker; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.Odometry; +import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.teamcode.subsystems.Parameters; From 59f1b24c43003c155e11633a02041d7bf96f1c00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ccasta42=E2=80=9D?= Date: Mon, 22 Jan 2024 15:59:11 -0600 Subject: [PATCH 03/11] Blue Color Filter implementation, in autonomous.java file all subsystem were commented --- TeamCode/build.gradle | 4 +- .../ftc/teamcode/autonomous.java | 15 +- .../teamcode/subsystems/OpenCVSubsystem.java | 150 ++++++------------ 3 files changed, 58 insertions(+), 111 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 959ceaf..b912144 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -14,13 +14,13 @@ // Include common definitions from above. apply from: '../build.common.gradle' apply from: '../build.dependencies.gradle' - +/* repositories { flatDir { dirs 'libs', 'C:\\Users\\rfgar\\navx-micro\\android\\libs' } } - +*/ android { namespace = 'org.firstinspires.ftc.teamcode' diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java index 9180055..e412fdd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java @@ -33,7 +33,7 @@ public class autonomous extends LinearOpMode { public double gyr; @Override public void runOpMode() { - //this.telemetry = telemetry; + control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); //imu = hardwareMap.get(IMU.class, "imu"); if (Parameters.robot == "marvin") { @@ -41,18 +41,13 @@ public void runOpMode() { } openCVSubsystem = OpenCVSubsystem.getInstance(hardwareMap); - aprilTagSubsystem = AprilTagSubsystem.getInstance(hardwareMap,telemetry); - chassisSubsystem = ChassisSubsystem.getInstance(hardwareMap,telemetry); - gyroscopeSubsystem = GyroscopeSubsystem.getInstance(hardwareMap); + //aprilTagSubsystem = AprilTagSubsystem.getInstance(hardwareMap,telemetry); + //chassisSubsystem = ChassisSubsystem.getInstance(hardwareMap,telemetry); + //gyroscopeSubsystem = GyroscopeSubsystem.getInstance(hardwareMap); //elevatorSubsystem = ElevatorSubsystem.getInstance(hardwareMap); //odometrySubsystem = OdometrySubsystem.getInstance(hardwareMap, chassisSubsystem); - waitForStart(); - while (opModeIsActive()) { - telemetry.addData("Status", "Running"); - //telemetry.addData("Width: ", openCVSubsystem.width); - telemetry.update(); - } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index 2a4b9de..cc3ab55 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -3,6 +3,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; @@ -11,138 +13,88 @@ import org.openftc.easyopencv.OpenCvCameraFactory; import org.openftc.easyopencv.OpenCvCameraRotation; import org.openftc.easyopencv.OpenCvPipeline; - import java.util.ArrayList; +import java.util.Arrays; import java.util.List; public class OpenCVSubsystem { - public static HardwareMap hardwareMap; - public OpenCVSubsystem(HardwareMap hardwareMap) { + public static HardwareMap hardwareMap; + public static Telemetry telemetry; + public OpenCVSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { OpenCVSubsystem.hardwareMap = hardwareMap; initOpenCV(); } - public static OpenCVSubsystem getInstance(HardwareMap hardwareMap){ - OpenCVSubsystem instance = new OpenCVSubsystem(hardwareMap); + OpenCVSubsystem instance = new OpenCVSubsystem(hardwareMap, telemetry); if (instance == null) { } return instance; } - double cX = 0; double cY = 0; public double width = 0; - private OpenCvCamera controlHubCam; // Use OpenCvCamera class from FTC SDK private static final int CAMERA_WIDTH = 640; // width of wanted camera resolution private static final int CAMERA_HEIGHT = 360; // height of wanted camera resolution - - // Calculate the distance using the formula public static final double objectWidthInRealWorldUnits = 3.75; // Replace with the actual width of the object in real-world units public static final double focalLength = 728; // Replace with the focal length of the camera in pixels private void initOpenCV() { - // Create an instance of the camera int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + controlHubCam = OpenCvCameraFactory.getInstance().createWebcam( hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + controlHubCam.setPipeline(new PropDetectionPipeline()); - // Use OpenCvCameraFactory class from FTC SDK to create camera instance - controlHubCam = OpenCvCameraFactory.getInstance().createWebcam( - hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); - - controlHubCam.setPipeline(new YellowBlobDetectionPipeline()); - - controlHubCam.openCameraDevice(); - controlHubCam.startStreaming(CAMERA_WIDTH, CAMERA_HEIGHT, OpenCvCameraRotation.UPRIGHT); - } - public MatOfPoint largestContour; - public class YellowBlobDetectionPipeline extends OpenCvPipeline { - - public Mat processFrame(Mat input) { - // Preprocess the frame to detect yellow regions - Mat yellowMask = preprocessFrame(input); - - // Find contours of the detected yellow regions - List contours = new ArrayList<>(); - Mat hierarchy = new Mat(); - Imgproc.findContours(yellowMask, contours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); - - // Find the largest yellow contour (blob) - largestContour = findLargestContour(contours); - - if (largestContour != null) { - // Draw a red outline around the largest detected object - Imgproc.drawContours(input, contours, contours.indexOf(largestContour), new Scalar(255, 0, 0), 2); - // Calculate the width of the bounding box - width = calculateWidth(largestContour); - - // Display the width next to the label - String widthLabel = "Width: " + (int) width + " pixels"; - Imgproc.putText(input, widthLabel, new Point(cX + 10, cY + 20), Imgproc.FONT_HERSHEY_SIMPLEX, 0.5, new Scalar(0, 255, 0), 2); - //Display the Distance - String distanceLabel = "Distance: " + String.format("%.2f", getDistance(width)) + " inches"; - Imgproc.putText(input, distanceLabel, new Point(cX + 10, cY + 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.5, new Scalar(0, 255, 0), 2); - // Calculate the centroid of the largest contour - Moments moments = Imgproc.moments(largestContour); - cX = moments.get_m10() / moments.get_m00(); - cY = moments.get_m01() / moments.get_m00(); - - // Draw a dot at the centroid - String label = "(" + (int) cX + ", " + (int) cY + ")"; - Imgproc.putText(input, label, new Point(cX + 10, cY), Imgproc.FONT_HERSHEY_COMPLEX, 0.5, new Scalar(0, 255, 0), 2); - Imgproc.circle(input, new Point(cX, cY), 5, new Scalar(0, 255, 0), -1); - + controlHubCam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + controlHubCam.startStreaming(640,480,OpenCvCameraRotation.UPRIGHT); } - - - return input; - } - - private Mat preprocessFrame(Mat frame) { - Mat hsvFrame = new Mat(); - Imgproc.cvtColor(frame, hsvFrame, Imgproc.COLOR_BGR2HSV); - - Scalar lowerYellow = new Scalar(5,50,50); - Scalar upperYellow = new Scalar(15,255,255); - - - Mat yellowMask = new Mat(); - Core.inRange(hsvFrame, lowerYellow, upperYellow, yellowMask); - - Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); - Imgproc.morphologyEx(yellowMask, yellowMask, Imgproc.MORPH_OPEN, kernel); - Imgproc.morphologyEx(yellowMask, yellowMask, Imgproc.MORPH_CLOSE, kernel); - - return yellowMask; - } - - private MatOfPoint findLargestContour(List contours) { - double maxArea = 0; - MatOfPoint largestContour = null; - - for (MatOfPoint contour : contours) { - double area = Imgproc.contourArea(contour); - if (area > maxArea) { - maxArea = area; - largestContour = contour; - } + @Override + public void onError(int errorCode) { + //telemetry.addData("errorFUCK", errorCode); } - - return largestContour; - } - private double calculateWidth(MatOfPoint contour) { - Rect boundingRect = Imgproc.boundingRect(contour); - return boundingRect.width; - } + }); + //controlHubCam.openCameraDevice(); + //telemetry.addLine("Waiting for start"); + //telemetry.update(); } - private static double getDistance(double width){ - double distance = (objectWidthInRealWorldUnits * focalLength) / width; - return distance; + + public static class PropDetectionPipeline extends OpenCvPipeline{ + Mat mat = new Mat(); + Mat slekerr = new Mat(); + Moments moment = new Moments(); + Mat bgrImage = new Mat(); + + + @Override + public Mat processFrame(Mat input){ + //Mat mat = input.clone(); + Imgproc.cvtColor(input,mat,Imgproc.COLOR_RGB2HSV); + Scalar lowerBlue = new Scalar(94,120,140,0); + Scalar upperBlue = new Scalar(108,255,255,255); + Core.inRange(mat,lowerBlue,upperBlue,mat); + //kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(1,1)); + //Imgproc.morphologyEx(mat,mat,Imgproc.MORPH_OPEN, kernel); + //kernel.release(); + //Mat[] hsvChannels = new Mat[3]; + //Core.split(mat, Arrays.asList(hsvChannels)); + moment = Imgproc.moments(mat); + double cx = moment.get_m10()/moment.get_m00(); + double cy = moment.get_m01()/moment.get_m00(); + Point centroid = new Point(cx,cy); + Imgproc.cvtColor(mat,bgrImage, Imgproc.COLOR_GRAY2RGB); + Imgproc.line(bgrImage,centroid,centroid,new Scalar(0,255,0),10); + //Imgproc.rectangle(input + // new Point(input.cols()/4,)); + return bgrImage; + } } + } \ No newline at end of file From ec7740a910b0091639bd04974742cae1211b07b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ccasta42=E2=80=9D?= Date: Tue, 23 Jan 2024 16:36:40 -0600 Subject: [PATCH 04/11] Blue Color Filter implementation, in autonomous.java file all subsystem were commented --- .../firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index cc3ab55..a60ea02 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -66,7 +66,7 @@ public void onError(int errorCode) { public static class PropDetectionPipeline extends OpenCvPipeline{ Mat mat = new Mat(); - Mat slekerr = new Mat(); + Mat sleker = new Mat(); Moments moment = new Moments(); Mat bgrImage = new Mat(); From 4835d6add049e10697db41c364e6714bcb2a8f4c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ccasta42=E2=80=9D?= Date: Tue, 23 Jan 2024 17:02:01 -0600 Subject: [PATCH 05/11] Blue Color Filter implementation, in autonomous.java file all subsystem were commented --- .../firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index a60ea02..0e5cef6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -66,7 +66,7 @@ public void onError(int errorCode) { public static class PropDetectionPipeline extends OpenCvPipeline{ Mat mat = new Mat(); - Mat sleker = new Mat(); + Mat sleke = new Mat(); Moments moment = new Moments(); Mat bgrImage = new Mat(); From 3db662a40c9647505b38f4ed13cc1fe4a2e8c1c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ccasta42=E2=80=9D?= Date: Tue, 23 Jan 2024 17:03:33 -0600 Subject: [PATCH 06/11] Blue Color Filter implementation, in autonomous.java file all subsystem were commented --- .../firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index 0e5cef6..55c45ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -66,7 +66,7 @@ public void onError(int errorCode) { public static class PropDetectionPipeline extends OpenCvPipeline{ Mat mat = new Mat(); - Mat sleke = new Mat(); + Mat slek = new Mat(); Moments moment = new Moments(); Mat bgrImage = new Mat(); From 52786eb3a36d684dfbbe6d86d46f2f32901becaa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ccasta42=E2=80=9D?= Date: Tue, 23 Jan 2024 17:05:00 -0600 Subject: [PATCH 07/11] Blue Color Filter implementation, in autonomous.java file all subsystem were commented --- .../firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index 55c45ef..bf9e1e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -66,7 +66,7 @@ public void onError(int errorCode) { public static class PropDetectionPipeline extends OpenCvPipeline{ Mat mat = new Mat(); - Mat slek = new Mat(); + Mat sleek = new Mat(); Moments moment = new Moments(); Mat bgrImage = new Mat(); From f84601b6b7f73069e73629c8bbfc4a19829773c1 Mon Sep 17 00:00:00 2001 From: VOLTEC6647 <116227776+SecondaryProgrammer@users.noreply.github.com> Date: Wed, 24 Jan 2024 21:58:08 -0600 Subject: [PATCH 08/11] dolor (odometria y deteccion de colores) --- .../firstinspires/ftc/teamcode/Chassis.java | 10 +- .../ftc/teamcode/autonomous.java | 116 +++++++++++++-- .../org/firstinspires/ftc/teamcode/info.java | 4 +- .../subsystems/AprilTagSubsystem.java | 139 ------------------ .../ftc/teamcode/subsystems/ArmSubsystem.java | 26 +++- .../subsystems/CameraSubsystem.java.bkk | 39 ----- .../teamcode/subsystems/ChassisSubsystem.java | 64 +++++--- .../subsystems/GyroscopeSubsystem.java | 4 + .../subsystems/OdometrySubsystem.java | 100 ++++++++----- .../teamcode/subsystems/OpenCVSubsystem.java | 48 ++++-- .../ftc/teamcode/subsystems/Parameters.java | 2 +- .../teamcode/subsystems/PivotSubsystem.java | 20 ++- 12 files changed, 293 insertions(+), 279 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java.bkk diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java index e5afb58..6a4501f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java @@ -16,10 +16,10 @@ import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.Odometry; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.teamcode.subsystems.Parameters; +import org.firstinspires.ftc.teamcode.subsystems.PivotSubsystem; @TeleOp @@ -134,13 +134,14 @@ void ArmMethods(){ } + @Override public void runOpMode() { //this.telemetry = telemetry; control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); //imu = hardwareMap.get(IMU.class, "imu"); if(Parameters.robot=="marvin"){ - intake = hardwareMap.get(DcMotor.class, "IntakeMotor"); + intake = hardwareMap.get(DcMotor.class, "intake"); } @@ -151,7 +152,8 @@ public void runOpMode() { //Odometry odometry = Odometry.getInstance(hardwareMap,chassis); //ArmSubsystem arm = ArmSubsystem.getInstance(hardwareMap, telemetry); GyroscopeSubsystem gyroscope = GyroscopeSubsystem.getInstance(hardwareMap); - ///////////////////////////// + PivotSubsystem pivot = PivotSubsystem.getInstance(hardwareMap, telemetry);///////////////////////////// + //orientation=imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); //GyroscopeSubsystem gyroscope=GyroscopeSubsystem.getInstance(); @@ -172,6 +174,8 @@ public void runOpMode() { ArmMethods(); + pivot.pivotControls(this.gamepad2.a,this.gamepad2.b,this.gamepad2.left_bumper, this.gamepad2.right_bumper); + telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java index e412fdd..a87ba42 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java @@ -7,7 +7,7 @@ import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.Orientation; -import org.firstinspires.ftc.teamcode.subsystems.AprilTagSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; import org.firstinspires.ftc.teamcode.subsystems.OpenCVSubsystem; import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; @@ -17,12 +17,12 @@ @Autonomous public class autonomous extends LinearOpMode { - private OpenCVSubsystem openCVSubsystem; - private AprilTagSubsystem aprilTagSubsystem; - private ChassisSubsystem chassisSubsystem; - private GyroscopeSubsystem gyroscopeSubsystem; - //private ElevatorSubsystem elevatorSubsystem; - //private OdometrySubsystem odometrySubsystem; + private OpenCVSubsystem camera; + private ChassisSubsystem chassis; + private GyroscopeSubsystem gyroscope; + private ElevatorSubsystem elevator; + private OdometrySubsystem odometry; + private ArmSubsystem arm; private Blinker control_Hub; private double speed = 1; private IMU imu; @@ -31,23 +31,111 @@ public class autonomous extends LinearOpMode { private DcMotor intake; boolean hasTarget = false; public double gyr; + +// public void rotateAngle(double target) { +// double difference = Math.abs(target - gyroscope.getRotation()); +// while (difference > 2) { +// chassis.targetAngle = target; +// chassis. +// difference = Math.abs(target - gyroscope.getRotation()); +// } +// odometry.resetEncoders(); +// } @Override public void runOpMode() { control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); //imu = hardwareMap.get(IMU.class, "imu"); if (Parameters.robot == "marvin") { - intake = hardwareMap.get(DcMotor.class, "IntakeMotor"); + intake = hardwareMap.get(DcMotor.class, "intake"); } - openCVSubsystem = OpenCVSubsystem.getInstance(hardwareMap); - //aprilTagSubsystem = AprilTagSubsystem.getInstance(hardwareMap,telemetry); - //chassisSubsystem = ChassisSubsystem.getInstance(hardwareMap,telemetry); - //gyroscopeSubsystem = GyroscopeSubsystem.getInstance(hardwareMap); - //elevatorSubsystem = ElevatorSubsystem.getInstance(hardwareMap); - //odometrySubsystem = OdometrySubsystem.getInstance(hardwareMap, chassisSubsystem); + camera = OpenCVSubsystem.getInstance(hardwareMap, telemetry); + chassis = ChassisSubsystem.getInstance(hardwareMap,telemetry); + gyroscope = GyroscopeSubsystem.getInstance(hardwareMap); + elevator = ElevatorSubsystem.getInstance(hardwareMap); + odometry = OdometrySubsystem.getInstance(hardwareMap, chassis, telemetry); + arm = ArmSubsystem.getInstance(hardwareMap, telemetry); + + + odometry.resetEncoders(); + + gyroscope.reset(); + + boolean rotation = true; + boolean test = true; + waitForStart(); + while (opModeIsActive()) { + boolean dolor = true; + + + odometry.resetEncoders(); + + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + sleep(500); + + dolor = true; + while (dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), -90); + } + odometry.resetEncoders(); + break; +// if (gyroscope.getRotation() < -88) { +// arm.open(); +// sleep(1000); +// arm.setPosition(1); +// } +// chassis.setMotors(0,0,0,0); +// +// //arm.close(); + //arm.setPosition(0); + //elevator.goUp(); + + /*switch (camera.findObjectSide()) { + case 1: + odometry.goTo(0,63); + odometry.rotateTo(gyroscope.getRotation(), -90); + if (gyroscope.getRotation() < -88) { + arm.open(); + sleep(1000); + arm.setPosition(1); + } + + break; + + case 2: + odometry.goTo(-15,63); + if (odometry.getXDist() > 22) { + arm.open(); + arm.setPosition(1); + } + + break; + + case 3: + odometry.goTo(0,63); + chassis.targetAngle = 90; + if (gyroscope.getRotation() < 88) { + arm.open(); + sleep(1000); + arm.setPosition(1); + } + + break; + }*/ + + //telemetry.addData("cx: ", camera.getcx()); + //telemetry.addData("cy: ", camera.getcy()); +// odometry.printEncoders(); +// telemetry.addData("rotation: ", gyroscope.getRotation()); +// telemetry.update(); + + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java index dc7b269..f4d7239 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode; public class info { - //public static final String name = "rev"; - public static final String name = "gobilda"; + public static final String name = "rev"; + //public static final String name = "gobilda"; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java deleted file mode 100644 index e6bda75..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagSubsystem.java +++ /dev/null @@ -1,139 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import android.util.Size; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -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.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; - -import java.util.List; - -public class AprilTagSubsystem { - private static AprilTagSubsystem instance; - private HardwareMap hardwareMap; - private Telemetry telemetry; - public AprilTagSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { - this.hardwareMap = hardwareMap; - this.telemetry = telemetry; - initAprilTag(); - } - - public static AprilTagSubsystem getInstance(HardwareMap hardwareMap, Telemetry telemetry) { - if (instance == null) { - instance = new AprilTagSubsystem(hardwareMap, telemetry); - } - return instance; - } - - private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - - /** - * The variable to store our instance of the AprilTag processor. - */ - private AprilTagProcessor aprilTag; - - /** - * The variable to store our instance of the vision portal. - */ - private VisionPortal visionPortal; - - - private void initAprilTag() { - - // Create the AprilTag processor. - aprilTag = new AprilTagProcessor.Builder() - - // The following default settings are available to un-comment and edit as needed. - .setDrawAxes(false) - .setDrawCubeProjection(false) - .setDrawTagOutline(true) - .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) - .setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) - .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) - - // == CAMERA CALIBRATION == - // If you do not manually specify calibration parameters, the SDK will attempt - // to load a predefined calibration for your camera. - .setLensIntrinsics(578.272, 578.272, 402.145, 221.506) - // ... these parameters are fx, fy, cx, cy. - - .build(); - - // Adjust Image Decimation to trade-off detection-range for detection-rate. - // eg: Some typical detection data using a Logitech C920 WebCam - // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second - // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second - // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) - // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) - // Note: Decimation can be changed on-the-fly to adapt during a match. - aprilTag.setDecimation(3); - - // Create the vision portal by using a builder. - 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); - } - - // Choose a camera resolution. Not all cameras support all resolutions. - builder.setCameraResolution(new Size(640, 360)); - - // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. - builder.enableLiveView(true); - - // Set the stream format; MJPEG uses less bandwidth than default YUY2. - builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); - - // Choose whether or not LiveView stops if no processors are enabled. - // If set "true", monitor shows solid orange screen if no processors enabled. - // If set "false", monitor shows camera view without annotations. - builder.setAutoStopLiveView(true); - - // Set and enable the processor. - builder.addProcessor(aprilTag); - - // Build the Vision Portal, using the above settings. - visionPortal = builder.build(); - - // Disable or re-enable the aprilTag processor at any time. - visionPortal.setProcessorEnabled(aprilTag, true); - - } // end method initAprilTag() - - private void telemetryAprilTag() { - - List 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.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); - telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } 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)"); - telemetry.addLine("RBE = Range, Bearing & Elevation"); - - } // end method telemetryAprilTag() -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java index fc0d3d4..6c7c233 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java @@ -11,6 +11,8 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.Chassis; +import javax.crypto.spec.DESedeKeySpec; + public class ArmSubsystem { private static ArmSubsystem instance; public Servo servoL; @@ -40,8 +42,8 @@ public static ArmSubsystem getInstance(HardwareMap hardwareMap, Telemetry teleme return instance; } public ArmSubsystem(HardwareMap hardwareMap, Telemetry telemetry){ - this.servoL=hardwareMap.get(Servo.class, "servoL"); - this.servoR=hardwareMap.get(Servo.class, "servoR"); + //this.servoL=hardwareMap.get(Servo.class, "servoL"); + //this.servoR=hardwareMap.get(Servo.class, "servoR"); this.claw=hardwareMap.get(Servo.class, "claw"); this.axis=hardwareMap.get(Servo.class, "axis"); //this.crservoL=hardwareMap.get(CRServo.class, "crservoL"); @@ -51,20 +53,29 @@ public ArmSubsystem(HardwareMap hardwareMap, Telemetry telemetry){ } public void showPositions(){ - telemetry.addData("servoLl", servoL.getPosition()); - telemetry.addData("servoRr", servoR.getPosition()); + //telemetry.addData("servoLl", servoL.getPosition()); + //telemetry.addData("servoRr", servoR.getPosition()); //telemetry.addData("crservoL", crservoL.getPower()); //telemetry.addData("crservoR", crservoR.getPower()); + telemetry.addData("Axis: ", axis.getPosition()); + telemetry.addData("Claw: ", claw.getPosition()); } public void setZero(){ angle=0.07; } - public void setPosition(double position){ - angle=position; - } + //public void setPosition(double position){ + // angle=position; + //} + public void setPosition(int desiredPosition) { + if (desiredPosition == 1) { + axis.setPosition(1); + } else if (desiredPosition == 0) { + axis.setPosition(0); + } + } public void updateArm(){ if(going_down){ servoL.setPosition(angle-0.3); @@ -74,6 +85,7 @@ public void updateArm(){ servoR.setPosition(servoR.MAX_POSITION-angle); } + updateAxis(); } public void updateAxis(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java.bkk b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java.bkk deleted file mode 100644 index 378616c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java.bkk +++ /dev/null @@ -1,39 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import android.util.Size; - -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.vision.VisionPortal; - -import org.firstinspires.ftc.robotcore.external.stream.*; - -import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; - -public class CameraSubsystem { - private static HardwareMap hardwareMap; - private static CameraSubsystem instance; - - private static CameraStreamClient camera; - - private static VisionPortal visionPortal; - private CameraSubsystem(HardwareMap hardwareMap){ - this.hardwareMap = hardwareMap; - - - } - public static CameraSubsystem getInstance(HardwareMap hardwareMap){ - if (instance == null) { - instance = new CameraSubsystem(hardwareMap); - } - return instance; - } - private void initVisionPortal(){ - camera = CameraStreamClient.getInstance(); - } - - private void getFrame(){ - vulforiaFrame = visionPortal.getFrameOnce(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java index 667baad..d37bad7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java @@ -51,27 +51,27 @@ public void rotateAngle(int angle) { public ChassisSubsystem(HardwareMap hardwareMap, Telemetry telemetry){ this.hardwareMap=hardwareMap; + this.telemetry = telemetry; /* FrontLeftMotor = hardwareMap.get(DcMotor.class, "BackRightMotor");// FrontRightMotor = hardwareMap.get(DcMotor.class, "BackLeftMotor"); BackLeftMotor = hardwareMap.get(DcMotor.class, "FrontRightMotor"); BackRightMotor = hardwareMap.get(DcMotor.class, "FrontLeftMotor"); */ - this.telemetry=telemetry; FrontLeftMotor = hardwareMap.get(DcMotor.class, "FL");// FrontRightMotor = hardwareMap.get(DcMotor.class, "FR"); BackLeftMotor = hardwareMap.get(DcMotor.class, "BL"); BackRightMotor = hardwareMap.get(DcMotor.class, "BR"); - //FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - //FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - //BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - //BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + //FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); } public void updateTargetAngle(){ targetAngle = lastDir; @@ -82,6 +82,33 @@ public static ChassisSubsystem getInstance(HardwareMap hardwareMap, Telemetry te } return instance; } + public void moveY(double speed) { + + frontLeftPower= -speed*(1); + backLeftPower = -speed*(1); + backRightPower = -speed*(1); + frontRightPower = -speed*(1); + + setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); + } + + public void moveX(double speed) { + frontLeftPower= speed*(1); + backLeftPower = -speed*(1); + backRightPower = -speed*(1); + frontRightPower = speed*(1); + + setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); + } + + public void moveR(double speed) { + frontLeftPower= -speed*(1); + backLeftPower = -speed*(1); + backRightPower = speed*(1); + frontRightPower = speed*(1); + + setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); + } public void arcadeDrive(double x, double y, double r, double speed, double degrees) { @@ -113,11 +140,15 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre }else{ angularError = calculateRotation((int) degrees, (int) targetAngle); + final double KPPP = 0.03; + double errror = targetAngle - degrees; + double speeed = errror * KPPP; + telemetry.addData("angularError",angularError); double angularThreshold = 10; - if(Math.abs(angularError)>angularThreshold){ - r=angularError*0.5; - } + //if(Math.abs(angularError)>angularThreshold) { + //} + r = speeed; } } @@ -165,7 +196,7 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre if(info.name=="rev"){ - frontLeftPower=-speed*(y - x + r); + frontLeftPower= -speed*(y - x + r); backLeftPower = -speed*(y + x + r); backRightPower = -speed*(y + x - r); @@ -173,10 +204,6 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre }else{ } - // = /*-*/speed*(y + x - r); - - - if(info.name=="rev"){ telemetry.addData("NotRobot","Rev"); @@ -185,9 +212,6 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre } - - - setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/GyroscopeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/GyroscopeSubsystem.java index 6930de7..fc012cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/GyroscopeSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/GyroscopeSubsystem.java @@ -27,10 +27,14 @@ public GyroscopeSubsystem(HardwareMap hardwareMap){ public double getRotation(){ return navx.getYaw()+offset; } + public double robotYaw() { + return navx.getYaw(); + } public void reset(){ navx.zeroYaw(); } + public void setOffset(int degrees){ offset = degrees; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index a96ecd2..9daa96d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -4,6 +4,11 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import java.text.BreakIterator; +import java.time.Year; + public class OdometrySubsystem { public int fieldX; public int fieldY; @@ -13,24 +18,26 @@ public class OdometrySubsystem { private HardwareMap hardwareMap; - private DcMotor xEncoder; - private DcMotor yEncoder; - + public DcMotor xEncoder; + public DcMotor yEncoder; + private Telemetry telemetry; private ChassisSubsystem chassis; - private static OdometrySubsystem instance; - public OdometrySubsystem(HardwareMap hardwareMap, ChassisSubsystem chassis){ + + public OdometrySubsystem(HardwareMap hardwareMap, ChassisSubsystem chassis, Telemetry telemetry){ this.hardwareMap=hardwareMap; this.xEncoder = hardwareMap.get(DcMotor.class, "xEncoder"); - this.yEncoder = hardwareMap.get(DcMotor.class, "yEncoder"); + this.yEncoder = hardwareMap.get(DcMotor.class, "FL"); this.chassis = chassis; + this.telemetry = telemetry; } - public static OdometrySubsystem getInstance(HardwareMap hardwareMap, ChassisSubsystem chassis){ + public static OdometrySubsystem getInstance(HardwareMap hardwareMap, ChassisSubsystem chassis, Telemetry telemetry){ if (instance == null) { - instance = new OdometrySubsystem(hardwareMap, chassis); + instance = new OdometrySubsystem(hardwareMap, chassis, telemetry); } return instance; } + public class coordinates{ public int x; public int y; @@ -46,45 +53,62 @@ public void update(){ fieldX = robotX + xEncoder.getCurrentPosition(); fieldY = robotY + yEncoder.getCurrentPosition(); } - public void moveToCoords(int x, int y){ - int xDistance = x - fieldX; - int yDistance = y - fieldY; + public void resetEncoders(){ + xEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + yEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + xEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + yEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } - int maxpower = 1; - int minpower = 0; - int xpower = 0; - int ypower = 0; - int breakDistance = 10000; + final double WHEELCIRCUMFERENCE = 4.8 * Math.PI; + final int TICKS = 2000; + public double getXDist() { + return (xEncoder.getCurrentPosition() * WHEELCIRCUMFERENCE) / TICKS; + } + public double getYDist() { + return (yEncoder.getCurrentPosition() * WHEELCIRCUMFERENCE) / TICKS; + } + public void printEncoders() { + telemetry.addData("xEncoder", xEncoder.getCurrentPosition()); + telemetry.addData("yEncoder", yEncoder.getCurrentPosition()); + telemetry.addData("X: ", getXDist()); + telemetry.addData("Y: ", getYDist()); + } + final double KP = 0.05; + public boolean goTo(int x, int y) { +// resetEncoders(); - if(xDistance>breakDistance){ - xpower = maxpower; - } - else if(xDistance<-breakDistance){ - xpower = -maxpower; - }else if (xDistance>0){ - xpower = maxpower*(xDistance/breakDistance); - }else if (xDistance<0){ - xpower = -maxpower*(xDistance/breakDistance); - } +// while (xDifference > 5 || yDifference > 5) { + double yError = y - getYDist() * -1; + double xError = x - getXDist() * 1; + double ySpeed = yError * KP; + double xSpeed = xError * KP; + telemetry.addData("yError", yError); + telemetry.addData("xError: ", xError); + telemetry.addData("poss", getYDist()); - if(yDistance>breakDistance){ - ypower = maxpower; - } - else if(yDistance<-breakDistance){ - ypower = -maxpower; - } else if (yDistance>0){ - ypower = maxpower*(yDistance/breakDistance); - }else if (yDistance<0){ - ypower = -maxpower*(yDistance/breakDistance); - } - chassis.arcadeDrive(xpower,ypower,0,0.4,0); - } + //chassis.arcadeDrive(xSpeed,ySpeed,0,1,gyr.getRotation()); + chassis.moveY(ySpeed); + chassis.moveX(xSpeed); + telemetry.addData("aaaa", xError); + telemetry.addData("aaaayyy", yError); + telemetry.update(); + return (Math.abs(xError) + Math.abs(yError) > 5) ? true : false; +// } + } + public boolean rotateTo(double degree, double target) { + final double KP2 = 0.018; + double rError = target - degree; + double rSpeed = rError * KP2; + chassis.moveR(rSpeed); + return (Math.abs(rError) > 2); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index bf9e1e8..f0bfc50 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -19,13 +19,15 @@ public class OpenCVSubsystem { - public static HardwareMap hardwareMap; - public static Telemetry telemetry; + + public HardwareMap hardwareMap; + public Telemetry telemetry; public OpenCVSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { - OpenCVSubsystem.hardwareMap = hardwareMap; + this.hardwareMap = hardwareMap; initOpenCV(); + this.telemetry=telemetry; } - public static OpenCVSubsystem getInstance(HardwareMap hardwareMap){ + public static OpenCVSubsystem getInstance(HardwareMap hardwareMap, Telemetry telemetry){ OpenCVSubsystem instance = new OpenCVSubsystem(hardwareMap, telemetry); if (instance == null) { } @@ -61,16 +63,16 @@ public void onError(int errorCode) { //controlHubCam.openCameraDevice(); //telemetry.addLine("Waiting for start"); //telemetry.update(); - } + public static double cx; + public static double cy; - public static class PropDetectionPipeline extends OpenCvPipeline{ + public class PropDetectionPipeline extends OpenCvPipeline{ Mat mat = new Mat(); Mat sleek = new Mat(); Moments moment = new Moments(); Mat bgrImage = new Mat(); - @Override public Mat processFrame(Mat input){ //Mat mat = input.clone(); @@ -84,17 +86,43 @@ public Mat processFrame(Mat input){ //Mat[] hsvChannels = new Mat[3]; //Core.split(mat, Arrays.asList(hsvChannels)); moment = Imgproc.moments(mat); - double cx = moment.get_m10()/moment.get_m00(); - double cy = moment.get_m01()/moment.get_m00(); + OpenCVSubsystem.cx = moment.get_m10()/moment.get_m00(); + OpenCVSubsystem.cy = moment.get_m01()/moment.get_m00(); Point centroid = new Point(cx,cy); Imgproc.cvtColor(mat,bgrImage, Imgproc.COLOR_GRAY2RGB); Imgproc.line(bgrImage,centroid,centroid,new Scalar(0,255,0),10); //Imgproc.rectangle(input - // new Point(input.cols()/4,)); + // new Point(input.cols()/4,)); return bgrImage; } + } + final int LEFTSIDE = 1; + final int CENTERSIDE = 2; + final int RIGHTSIDE = 3; + + public int findObjectSide() { + int side = 0; + if (getcx() < 220) { + side = LEFTSIDE; + controlHubCam.stopStreaming(); + } else if (getcx() > 220 && getcx() < 440) { + side = CENTERSIDE; + controlHubCam.stopStreaming(); + } else if (getcx() > 440) { + side = RIGHTSIDE; + controlHubCam.stopStreaming(); + } + return side; + } + public double getcx() { + return cx; + } + + public double getcy() { + return cy; + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Parameters.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Parameters.java index bfc28c2..12056e1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Parameters.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Parameters.java @@ -2,5 +2,5 @@ public class Parameters { - public static String robot="gobilda"; + public static String robot="marvin"; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java index 8fa78c9..52a0b5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java @@ -11,11 +11,11 @@ public class PivotSubsystem { public Servo servoClaw; private Telemetry telemetry; - private final double CLAW_OPEN = 0.3; + private final double CLAW_OPEN = 1; private final double CLAW_CLOSE = 0; - private final double PIVOT_UP = 0; + private final double PIVOT_UP = 1; - private final double PIVOT_DOWN = 0.5; + private final double PIVOT_DOWN = 0; public PivotSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { this.servoClaw = hardwareMap.get(Servo.class, "claw"); @@ -47,12 +47,20 @@ public void down() { servoPivot.setPosition(PIVOT_DOWN); } + public void pivotControls(boolean buttonA, boolean buttonB, boolean leftBumper, boolean rightBumper) { + if (buttonA) { + up(); + } else if (buttonB) { + down(); + } else if (leftBumper) { + open(); + } else if (rightBumper) { + close(); + } + } public void printPositions() { telemetry.addData("claw", servoClaw.getPosition()); telemetry.addData("pivot", servoPivot.getPosition()); telemetry.update(); } - - - } \ No newline at end of file From e3dd7a87fd1c380f42f8884a688496189dee5036 Mon Sep 17 00:00:00 2001 From: VOLTEC6647 <116227776+SecondaryProgrammer@users.noreply.github.com> Date: Thu, 25 Jan 2024 12:05:46 -0600 Subject: [PATCH 09/11] progreso autonomo --- .../firstinspires/ftc/teamcode/Chassis.java | 24 ++- .../ftc/teamcode/autonomous.java | 189 ++++++++++++------ .../ftc/teamcode/subsystems/ArmSubsystem.java | 8 +- .../teamcode/subsystems/OpenCVSubsystem.java | 6 +- .../teamcode/subsystems/PivotSubsystem.java | 3 + 5 files changed, 162 insertions(+), 68 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java index 6a4501f..8d99eed 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java @@ -17,6 +17,7 @@ import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.teamcode.subsystems.Parameters; import org.firstinspires.ftc.teamcode.subsystems.PivotSubsystem; @@ -34,6 +35,8 @@ public class Chassis extends LinearOpMode { private DcMotor intake; boolean hasTarget=false; public double gyr; + public Gamepad controller1; + public Gamepad controller2; public ChassisSubsystem chassis; @@ -46,8 +49,9 @@ void ElevatorMethods(ElevatorSubsystem elevator){ elevator.goDown(this.gamepad1.left_trigger); } */ + if(!this.gamepad1.start) { - if (this.gamepad1.dpad_up && this.gamepad1.b) { + /*if (this.gamepad1.dpad_up && this.gamepad1.b) { elevator.elevator1.setPower(-1); } else if (this.gamepad1.dpad_down && this.gamepad1.b) { elevator.elevator1.setPower(1); @@ -64,8 +68,19 @@ void ElevatorMethods(ElevatorSubsystem elevator){ } else { elevator.elevator1.setPower(0); elevator.elevator2.setPower(0); + }*/ + if(Math.abs(controller2.right_stick_y)>0.3) { + elevator.DebugSpeed = Math.abs(controller2.right_stick_y); + if (controller2.right_stick_y > 0.3) { + elevator.goDown(); + } else if (controller2.right_stick_y < -0.3) { + elevator.goUp(); + } + }else { + elevator.stop(); } } + } void ChassisMethods(ChassisSubsystem chassis, GyroscopeSubsystem gyroscope){ //IMUMethods(); @@ -162,8 +177,15 @@ public void runOpMode() { //telemetry.addData("GyroY", AndroidOrientation().getAngle().secondPosition()); telemetry.update(); + waitForStart(); + controller1 = this.gamepad1; + controller2 = this.gamepad2; + + if(this.gamepad2.start&this.gamepad2.back){ + controller1=this.gamepad2; + } //arm.setZero(); while(opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java index a87ba42..90b883f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java @@ -14,6 +14,7 @@ import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; import org.firstinspires.ftc.teamcode.subsystems.Parameters; +import org.firstinspires.ftc.teamcode.subsystems.PivotSubsystem; @Autonomous public class autonomous extends LinearOpMode { @@ -23,6 +24,7 @@ public class autonomous extends LinearOpMode { private ElevatorSubsystem elevator; private OdometrySubsystem odometry; private ArmSubsystem arm; + private PivotSubsystem pivot; private Blinker control_Hub; private double speed = 1; private IMU imu; @@ -32,7 +34,7 @@ public class autonomous extends LinearOpMode { boolean hasTarget = false; public double gyr; -// public void rotateAngle(double target) { + // public void rotateAngle(double target) { // double difference = Math.abs(target - gyroscope.getRotation()); // while (difference > 2) { // chassis.targetAngle = target; @@ -51,11 +53,12 @@ public void runOpMode() { } camera = OpenCVSubsystem.getInstance(hardwareMap, telemetry); - chassis = ChassisSubsystem.getInstance(hardwareMap,telemetry); + chassis = ChassisSubsystem.getInstance(hardwareMap, telemetry); gyroscope = GyroscopeSubsystem.getInstance(hardwareMap); elevator = ElevatorSubsystem.getInstance(hardwareMap); odometry = OdometrySubsystem.getInstance(hardwareMap, chassis, telemetry); arm = ArmSubsystem.getInstance(hardwareMap, telemetry); + pivot = PivotSubsystem.getInstance(hardwareMap, telemetry); odometry.resetEncoders(); @@ -67,75 +70,141 @@ public void runOpMode() { waitForStart(); while (opModeIsActive()) { - boolean dolor = true; - - - odometry.resetEncoders(); - while (dolor) { - dolor = odometry.goTo(0,-63); - } - chassis.setMotors(0,0,0,0); - sleep(500); - - dolor = true; - while (dolor) { - dolor = odometry.rotateTo(gyroscope.getRotation(), -90); - } - odometry.resetEncoders(); - break; - -// if (gyroscope.getRotation() < -88) { -// arm.open(); -// sleep(1000); -// arm.setPosition(1); -// } -// chassis.setMotors(0,0,0,0); -// -// //arm.close(); - //arm.setPosition(0); - //elevator.goUp(); - - /*switch (camera.findObjectSide()) { - case 1: - odometry.goTo(0,63); - odometry.rotateTo(gyroscope.getRotation(), -90); - if (gyroscope.getRotation() < -88) { - arm.open(); - sleep(1000); - arm.setPosition(1); - } + /////////////////////////////////////////////////////////// + //////////////////////REV AUTONOMOUS/////////////////////// + /////////////////////////////////////////////////////////// + boolean dolor = true; + if (info.name == "rev"); { +// pivot.close(); +// sleep(50); +// pivot.up(); +// elevator.goUp(); +// odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); break; - - case 2: - odometry.goTo(-15,63); - if (odometry.getXDist() > 22) { - arm.open(); - arm.setPosition(1); + /* + sleep(300); + pivot.open(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,62); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(-100,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), -90); + } + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(40,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,40); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders();*/ + + /*if (info.name == "gobilda") { + arm.setAxisPosition(1); + arm.going_down = false; + arm.updateArm(); + elevator.goUp(); + + odometry.resetEncoders(); + + while (dolor) { + dolor = odometry.goTo(-10, -63); } + chassis.setMotors(0, 0, 0, 0); + odometry.resetEncoders(); + sleep(500); + pivot.open(); + + dolor = true; + while (dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), -90); + } + odometry.resetEncoders(); - break; - - case 3: - odometry.goTo(0,63); - chassis.targetAngle = 90; - if (gyroscope.getRotation() < 88) { - arm.open(); - sleep(1000); - arm.setPosition(1); + dolor = true; + while (dolor) { + odometry.goTo(0, -100); + } + chassis.setMotors(0, 0, 0, 0); + sleep(500); + + sleep(100); + switch (camera.findObjectSide()) { + case 1: + odometry.goTo(0, 63); + odometry.rotateTo(gyroscope.getRotation(), -90); + if (gyroscope.getRotation() < -88) { + arm.open(); + sleep(1000); + arm.updateArm(); + } + break; + + case 2: + odometry.goTo(-15, 63); + if (odometry.getXDist() > 22) { + arm.open(); + arm.setPosition(1); + } + break; + + case 3: + odometry.goTo(0, 63); + chassis.targetAngle = 90; + if (gyroscope.getRotation() < 88) { + arm.open(); + sleep(1000); + arm.setPosition(1); + } } - break; - }*/ + }*/ - //telemetry.addData("cx: ", camera.getcx()); - //telemetry.addData("cy: ", camera.getcy()); + } + + //telemetry.addData("cx: ", camera.getcx()); + //telemetry.addData("cy: ", camera.getcy()); // odometry.printEncoders(); // telemetry.addData("rotation: ", gyroscope.getRotation()); // telemetry.update(); + } + } + + + /////////////////////////////////////////////////////////// + //////////////////GOBILDA AUTONOMOUS/////////////////////// + /////////////////////////////////////////////////////////// + } - } -} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java index 6c7c233..54037d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java @@ -65,11 +65,11 @@ public void setZero(){ angle=0.07; } - //public void setPosition(double position){ - // angle=position; - //} + public void setPosition(double position){ + angle=position; + } - public void setPosition(int desiredPosition) { + public void setAxisPosition(int desiredPosition) { if (desiredPosition == 1) { axis.setPosition(1); } else if (desiredPosition == 0) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index f0bfc50..81298e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -104,13 +104,13 @@ public Mat processFrame(Mat input){ public int findObjectSide() { int side = 0; - if (getcx() < 220) { + if (getcx() < 213) { side = LEFTSIDE; controlHubCam.stopStreaming(); - } else if (getcx() > 220 && getcx() < 440) { + } else if (getcx() > 212 && getcx() < 426) { side = CENTERSIDE; controlHubCam.stopStreaming(); - } else if (getcx() > 440) { + } else if (getcx() > 425) { side = RIGHTSIDE; controlHubCam.stopStreaming(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java index 52a0b5e..ea67c09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems; +import android.icu.text.IDNA; + import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -9,6 +11,7 @@ public class PivotSubsystem { private static PivotSubsystem instance; public Servo servoPivot; public Servo servoClaw; + public Servo axis; private Telemetry telemetry; private final double CLAW_OPEN = 1; From b575ed7d1fb454c1f47d74431b63c93a7ca0fd53 Mon Sep 17 00:00:00 2001 From: CrisianFG <147004159+CrisianFG@users.noreply.github.com> Date: Fri, 26 Jan 2024 12:24:39 -0600 Subject: [PATCH 10/11] iufhweuf --- .../firstinspires/ftc/teamcode/Chassis.java | 10 +- .../ftc/teamcode/autonomous.java | 353 ++++++++++++------ .../org/firstinspires/ftc/teamcode/info.java | 3 + .../ftc/teamcode/subsystems/ArmSubsystem.java | 22 +- .../subsystems/OdometrySubsystem.java | 11 +- .../teamcode/subsystems/OpenCVSubsystem.java | 19 +- .../teamcode/subsystems/PivotSubsystem.java | 4 +- 7 files changed, 274 insertions(+), 148 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java index 8d99eed..d54a5e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java @@ -69,11 +69,11 @@ void ElevatorMethods(ElevatorSubsystem elevator){ elevator.elevator1.setPower(0); elevator.elevator2.setPower(0); }*/ - if(Math.abs(controller2.right_stick_y)>0.3) { - elevator.DebugSpeed = Math.abs(controller2.right_stick_y); - if (controller2.right_stick_y > 0.3) { + if(Math.abs(gamepad2.right_stick_y)>0.3) { + elevator.DebugSpeed = Math.abs(gamepad2.right_stick_y); + if (gamepad2.right_stick_y > 0.3) { elevator.goDown(); - } else if (controller2.right_stick_y < -0.3) { + } else if (gamepad2.right_stick_y < -0.3) { elevator.goUp(); } }else { @@ -196,6 +196,8 @@ public void runOpMode() { ArmMethods(); + elevator.goDown(); + pivot.pivotControls(this.gamepad2.a,this.gamepad2.b,this.gamepad2.left_bumper, this.gamepad2.right_bumper); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java index 90b883f..6295e58 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java @@ -34,15 +34,6 @@ public class autonomous extends LinearOpMode { boolean hasTarget = false; public double gyr; - // public void rotateAngle(double target) { -// double difference = Math.abs(target - gyroscope.getRotation()); -// while (difference > 2) { -// chassis.targetAngle = target; -// chassis. -// difference = Math.abs(target - gyroscope.getRotation()); -// } -// odometry.resetEncoders(); -// } @Override public void runOpMode() { @@ -60,13 +51,8 @@ public void runOpMode() { arm = ArmSubsystem.getInstance(hardwareMap, telemetry); pivot = PivotSubsystem.getInstance(hardwareMap, telemetry); - odometry.resetEncoders(); - - gyroscope.reset(); - - boolean rotation = true; - boolean test = true; + //gyroscope.reset(); waitForStart(); while (opModeIsActive()) { @@ -76,135 +62,276 @@ public void runOpMode() { /////////////////////////////////////////////////////////// boolean dolor = true; - if (info.name == "rev"); { + int objectSide = camera.findObjectSide(); + + /*if (info.name == "popo"); { // pivot.close(); // sleep(50); // pivot.up(); // elevator.goUp(); -// odometry.resetEncoders(); - - dolor = true; - while (dolor) { - dolor = odometry.goTo(0,-63); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - break; - /* - sleep(300); - pivot.open(); - - dolor = true; - while (dolor) { - dolor = odometry.goTo(0,62); - } - chassis.setMotors(0,0,0,0); odometry.resetEncoders(); - dolor = true; - while (dolor) { - dolor = odometry.goTo(-100,0); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); + switch (objectSide) { + case 0: + telemetry.addData("OBJECT NOT FOUND. ", "SKILL ISSUE IM AFRAID"); + sleep(5000); + break; + + case 1: + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while(dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), -90); + } + chassis.setMotors(0,0,0,0); + gyroscope.reset(); + odometry.resetEncoders(); + + sleep(300); + pivot.up(); + pivot.open(); + + dolor = true; + while(dolor) { + dolor = odometry.goTo(20,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while(dolor) { + dolor = odometry.goTo(0,-100); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + break; + case 2: + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(10,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + sleep(300); + pivot.open(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(-20,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-20); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(-100,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + break; + case 3: + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while(dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), 90); + } + chassis.setMotors(0,0,0,0); + gyroscope.reset(); + odometry.resetEncoders(); + + sleep(300); + pivot.up(); + pivot.open(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(20,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-50); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + break; + }*/ - dolor = true; - while (dolor) { - dolor = odometry.rotateTo(gyroscope.getRotation(), -90); - } - odometry.resetEncoders(); - dolor = true; - while (dolor) { - dolor = odometry.goTo(40,0); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); + /////////////////////////////////////////////////////////// + ///////// GOBILDA AUTONOMOUS ////////// + /////////////////////////////////////////////////////////// - dolor = true; - while (dolor) { - dolor = odometry.goTo(0,40); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders();*/ + ///////////// BLUE TEAM ///////////// - /*if (info.name == "gobilda") { - arm.setAxisPosition(1); - arm.going_down = false; - arm.updateArm(); + if (info.name == "gobilda") { + arm.close(); + arm.setPosition(1); + arm.updateArm(0); elevator.goUp(); odometry.resetEncoders(); - while (dolor) { - dolor = odometry.goTo(-10, -63); - } - chassis.setMotors(0, 0, 0, 0); - odometry.resetEncoders(); - sleep(500); - pivot.open(); + switch (objectSide) { + case 0: + telemetry.addData("OBJECT NOT FOUND. ", "SKILL ISSUE IM AFRAID"); + sleep(5000); + break; - dolor = true; - while (dolor) { - dolor = odometry.rotateTo(gyroscope.getRotation(), -90); - } - odometry.resetEncoders(); + case 1: + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); - dolor = true; - while (dolor) { - odometry.goTo(0, -100); - } - chassis.setMotors(0, 0, 0, 0); - sleep(500); + dolor = true; + while(dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), -90); + } + chassis.setMotors(0,0,0,0); + gyroscope.reset(); + odometry.resetEncoders(); - sleep(100); - switch (camera.findObjectSide()) { - case 1: - odometry.goTo(0, 63); - odometry.rotateTo(gyroscope.getRotation(), -90); - if (gyroscope.getRotation() < -88) { - arm.open(); - sleep(1000); - arm.updateArm(); + sleep(300); + arm.setPosition(1); + arm.updateArm(1); + + dolor = true; + while(dolor) { + dolor = odometry.goTo(20,0); } - break; + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); - case 2: - odometry.goTo(-15, 63); - if (odometry.getXDist() > 22) { - arm.open(); - arm.setPosition(1); + dolor = true; + while(dolor) { + dolor = odometry.goTo(0,-100); } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + break; + case 2: + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(10,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + sleep(300); + pivot.open(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(-20,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-20); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(-100,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + break; case 3: - odometry.goTo(0, 63); - chassis.targetAngle = 90; - if (gyroscope.getRotation() < 88) { - arm.open(); - sleep(1000); - arm.setPosition(1); + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); } - } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); - }*/ + dolor = true; + while(dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), 90); + } + chassis.setMotors(0,0,0,0); + gyroscope.reset(); + odometry.resetEncoders(); - } + sleep(300); + pivot.up(); + pivot.open(); - //telemetry.addData("cx: ", camera.getcx()); - //telemetry.addData("cy: ", camera.getcy()); -// odometry.printEncoders(); -// telemetry.addData("rotation: ", gyroscope.getRotation()); -// telemetry.update(); - } - } + dolor = true; + while (dolor) { + dolor = odometry.goTo(20,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-50); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); - /////////////////////////////////////////////////////////// - //////////////////GOBILDA AUTONOMOUS/////////////////////// - /////////////////////////////////////////////////////////// + break; + } + + } + elevator.goUp(); + telemetry.addData("object side: ", objectSide); + + telemetry.update(); + } } + } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java index f4d7239..c363f54 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java @@ -3,4 +3,7 @@ public class info { public static final String name = "rev"; //public static final String name = "gobilda"; + public static final String OPMODE = "autonomous"; + //public static final String OPMODE = "teleop"; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java index 54037d6..32d30c8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java @@ -1,8 +1,10 @@ package org.firstinspires.ftc.teamcode.subsystems; import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode; import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -10,6 +12,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.Chassis; +import org.firstinspires.ftc.teamcode.info; import javax.crypto.spec.DESedeKeySpec; @@ -69,14 +72,7 @@ public void setPosition(double position){ angle=position; } - public void setAxisPosition(int desiredPosition) { - if (desiredPosition == 1) { - axis.setPosition(1); - } else if (desiredPosition == 0) { - axis.setPosition(0); - } - } - public void updateArm(){ + public void updateArm(int axisPosition) { if(going_down){ servoL.setPosition(angle-0.3); servoR.setPosition(servoR.MAX_POSITION-angle+0.3); @@ -85,9 +81,15 @@ public void updateArm(){ servoR.setPosition(servoR.MAX_POSITION-angle); } - - updateAxis(); + if (axisPosition == 0) { + setZero(); + } + if (axisPosition == 1) { + updateAxis(); + } } + + public void updateAxis(){ if(angle>0.1){ outside=true; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index 9daa96d..0edae10 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -74,12 +74,9 @@ public void printEncoders() { telemetry.addData("X: ", getXDist()); telemetry.addData("Y: ", getYDist()); } - final double KP = 0.05; + final double KP = 0.03; public boolean goTo(int x, int y) { -// resetEncoders(); - -// while (xDifference > 5 || yDifference > 5) { double yError = y - getYDist() * -1; double xError = x - getXDist() * 1; double ySpeed = yError * KP; @@ -88,8 +85,6 @@ public boolean goTo(int x, int y) { telemetry.addData("xError: ", xError); telemetry.addData("poss", getYDist()); - - //chassis.arcadeDrive(xSpeed,ySpeed,0,1,gyr.getRotation()); chassis.moveY(ySpeed); chassis.moveX(xSpeed); telemetry.addData("aaaa", xError); @@ -97,9 +92,6 @@ public boolean goTo(int x, int y) { telemetry.update(); return (Math.abs(xError) + Math.abs(yError) > 5) ? true : false; -// } - - } public boolean rotateTo(double degree, double target) { final double KP2 = 0.018; @@ -110,5 +102,4 @@ public boolean rotateTo(double degree, double target) { return (Math.abs(rError) > 2); } - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java index 81298e3..7978f12 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -104,16 +104,17 @@ public Mat processFrame(Mat input){ public int findObjectSide() { int side = 0; - if (getcx() < 213) { - side = LEFTSIDE; - controlHubCam.stopStreaming(); - } else if (getcx() > 212 && getcx() < 426) { - side = CENTERSIDE; - controlHubCam.stopStreaming(); - } else if (getcx() > 425) { - side = RIGHTSIDE; - controlHubCam.stopStreaming(); + final int CALIBRATION_REPETITIONS = 50; + for (int i = 0; i < CALIBRATION_REPETITIONS; i++) { + if (getcx() < 213) { + side = LEFTSIDE; + } else if (getcx() > 213 && getcx() < 425) { + side = CENTERSIDE; + } else if (getcx() > 425) { + side = RIGHTSIDE; + } } + controlHubCam.stopStreaming(); return side; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java index ea67c09..bb72895 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java @@ -55,9 +55,9 @@ public void pivotControls(boolean buttonA, boolean buttonB, boolean leftBumper, up(); } else if (buttonB) { down(); - } else if (leftBumper) { - open(); } else if (rightBumper) { + open(); + } else if (leftBumper) { close(); } } From 77df9e8d07c17a1095ac750d5cd374f1f93dc06c Mon Sep 17 00:00:00 2001 From: CrisianFG <147004159+CrisianFG@users.noreply.github.com> Date: Sat, 27 Jan 2024 10:48:51 -0600 Subject: [PATCH 11/11] FUNCIONA (movimiento chasis meh) --- .../firstinspires/ftc/teamcode/Chassis.java | 229 +++------------ .../ftc/teamcode/autonomous.java | 271 +++++++++--------- .../org/firstinspires/ftc/teamcode/info.java | 4 +- .../ftc/teamcode/subsystems/ArmSubsystem.java | 6 +- .../teamcode/subsystems/ChassisSubsystem.java | 16 +- .../subsystems/DroneLauncherSubsystem.java | 6 +- .../subsystems/OdometrySubsystem.java | 8 +- .../teamcode/subsystems/PivotSubsystem.java | 14 +- 8 files changed, 216 insertions(+), 338 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java index 47497d4..621510a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java @@ -17,7 +17,6 @@ import org.firstinspires.ftc.teamcode.subsystems.DroneLauncherSubsystem; import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.Odometry; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.IMU; @@ -40,277 +39,141 @@ public class Chassis extends LinearOpMode { private DcMotor intake; boolean hasTarget=false; public double gyr; - public Gamepad controller1; - public Gamepad controller2; - public ChassisSubsystem chassis; void ElevatorMethods(ElevatorSubsystem elevator){ - /* - if(controller1.right_trigger>0.1){ - elevator.goUp(controller1.right_trigger); - }else if(controller1.left_trigger<0.1){ - elevator.goDown(controller1.left_trigger); - } - */ - elevator.getPosition(telemetry); - if(!controller2.start) { - /* - if (controller1.dpad_up && controller1.b) { - elevator.elevator1.setPower(-1); - } else if (controller1.dpad_down && controller1.b) { - elevator.elevator1.setPower(1); - } else if (controller1.dpad_up && controller1.a) { - elevator.elevator1.setPower(-1); - } else if (controller1.dpad_down && controller1.a) { - elevator.elevator1.setPower(1); - } else if (controller1.dpad_up) { - elevator.elevator2.setPower(-1); - elevator.elevator1.setPower(-1); - } else if (controller1.dpad_down) { - elevator.elevator2.setPower(1); - elevator.elevator1.setPower(1); - } else { - elevator.elevator1.setPower(0); - elevator.elevator2.setPower(0); - } - */ + if (!controller2.start) { if(Math.abs(controller2.right_stick_y)>0.3) { elevator.DebugSpeed = Math.abs(controller2.right_stick_y); if (controller2.right_stick_y > 0.3) { - elevator.goUp(); - } else if (controller2.right_stick_y < -0.3) { elevator.goDown(); + } else if (controller2.right_stick_y < -0.3) { + elevator.goUp(); } - }else { + } else { elevator.stop(); } } } + void ChassisMethods(ChassisSubsystem chassis, GyroscopeSubsystem gyroscope){ - //IMUMethods(); gyr = gyroscope.getRotation(); - //gyr=0; telemetry.addData("gyr",gyr); if(controller1.back){ if(controller1.a){ gyroscope.reset(); chassis.updateTargetAngle(); } - } - if(controller1.y){ + + if (controller1.y) { chassis.FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); chassis.FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); chassis.BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); chassis.BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - }else{ + } else { chassis.FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); chassis.FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); chassis.BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); chassis.BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } - if(controller1.start){ - if (controller1.dpad_up){ + + if (controller1.start) { + if (controller1.dpad_up) { gyroscope.setOffset(180); chassis.updateTargetAngle(); } - if (controller1.dpad_right){ + + if (controller1.dpad_right) { gyroscope.setOffset(270); chassis.updateTargetAngle(); } - if (controller1.dpad_down){ + + if (controller1.dpad_down) { gyroscope.setOffset(0); chassis.updateTargetAngle(); } - if (controller1.dpad_left){ + + if (controller1.dpad_left) { gyroscope.setOffset(90); chassis.updateTargetAngle(); } } + chassis.arcadeDrive(controller1.left_stick_x,-controller1.left_stick_y,controller1.right_stick_x,speed,gyr); - if(controller1.right_trigger>0.1){ - speed=baseSpeed*(1.4-controller1.right_trigger); - }else{ + if (controller1.right_trigger>0.1) { + speed=baseSpeed * (1.4-controller1.right_trigger); + } else { speed=baseSpeed; } - - if(controller2.x){ - if(controller1.back){ - intake.setPower(-1); - }else { - intake.setPower(1); - } - }else{ - intake.setPower(0); - } - - } - void IMUMethods(){ - orientation=imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - //telemetry.addData("GyroX", AGyroscope.getX()); - //telemetry.addData("GyroY", AGyroscope.getY()); - telemetry.addData("Gyro", orientation.firstAngle); - - - if(controller1.right_stick_button){ - //imu.resetYaw(); - - } - } - void ArmMethods(ArmSubsystem arm, PivotSubsystem pivot){ - - if(info.name=="gobilda"){ - arm.going_down =Chassis.controller2.left_stick_y < -0.2; - arm.going_down = false; - } - - - - if(!controller2.start){ - if (controller2.dpad_right) { - if(info.name=="gobilda") { - arm.setPosition(0.5); - arm.updateArm(); - }else { - pivot.up(); - } - } else if(controller2.dpad_left) { - if(info.name=="gobilda") { - if(!arm.open){ - arm.setZero(); - arm.updateArm(); - } - }else { - pivot.down(); - } - - }else if(controller2.dpad_up){ - if(info.name=="gobilda") { - arm.setPosition(0.7); - arm.updateArm(); - }else { - - } - - } + void PivotMethods(PivotSubsystem pivot) { + if (gamepad2.a) { + pivot.pickup(); + } else if (gamepad2.x) { + pivot.down(); + } else if (gamepad2.b) { + pivot.up(); + } else if (gamepad2.left_bumper) { + pivot.close(); + } else if (gamepad2.right_bumper) { + pivot.open(); } - - if (controller2.right_bumper) { - if(info.name=="rev"){ - pivot.open(); - }else{ - arm.open(); - } - - } else if(controller2.left_bumper) { - if(info.name=="rev"){ - pivot.close(); - }else{ - arm.close(); - } - } - if(info.name=="gobilda") { - if (arm.servoDelta.seconds() > 0.5) { - arm.servoDelta.reset(); - arm.showPositions(); - - } - telemetry.addData("angle", arm.angle); - } - - } @Override public void runOpMode() { - //this.telemetry = telemetry; control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); - //imu = hardwareMap.get(IMU.class, "imu"); - intake = hardwareMap.get(DcMotor.class, "intake"); - - //control_Hub.setConstant(3); - telemetry.addData("Status", "Initialized"); - ///////////////////////////// + + //WORKS SO FAR I THINK ChassisSubsystem chassis=ChassisSubsystem.getInstance(hardwareMap,telemetry); - ElevatorSubsystem elevator = ElevatorSubsystem.getInstance(hardwareMap); - //Odometry odometry = Odometry.getInstance(hardwareMap,chassis); + ElevatorSubsystem elevator = ElevatorSubsystem.getInstance(hardwareMap,telemetry); GyroscopeSubsystem gyroscope = GyroscopeSubsystem.getInstance(hardwareMap); + //TESTING NEEDED DroneLauncherSubsystem launcher = DroneLauncherSubsystem.getInstance(hardwareMap,telemetry); - PivotSubsystem pivot = null; - ArmSubsystem arm = null; - if(info.name=="rev") { - pivot = PivotSubsystem.getInstance(hardwareMap, telemetry); - }else{ - arm = ArmSubsystem.getInstance(hardwareMap, telemetry); - } - ///////////////////////////// - //orientation=imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - - //GyroscopeSubsystem gyroscope=GyroscopeSubsystem.getInstance(); - - //telemetry.addData("GyroX", orientation.secondAngle); - //telemetry.addData("GyroY", AndroidOrientation().getAngle().secondPosition()); - ////telemetry.update(); - + PivotSubsystem pivot = PivotSubsystem.getInstance(hardwareMap,telemetry); + //Maybe this messes something up?? idk maybe maybe + gyroscope.reset(); //launcher.reset(); waitForStart(); - chassis.updateTargetAngle(); - - if(info.name=="gobilda"){ - arm.setZero(); - arm.updateArm(); - } - - - //arm.setZero(); - controller1 = this.gamepad1; controller2 = this.gamepad2; - if(this.gamepad2.start&this.gamepad2.back){ + if (this.gamepad2.start&this.gamepad2.back) { controller1=this.gamepad2; } - while(opModeIsActive()) { + while (opModeIsActive()) { ChassisMethods(chassis,gyroscope); ElevatorMethods(elevator); - ArmMethods(arm,pivot); +// ArmMethods(arm,pivot); - telemetry.update(); + PivotMethods(pivot); if (controller2.y&&!(controller2.start||controller2.back)) { launcher.launch(); } else { - //launcher.reset(); + launcher.reset(); } - + //telemetry.addData("angle", arm.angle); + telemetry.update(); } - /* - odometry.update(); - telemetry.addData("X", odometry.position().x); - if(hasTarget){ - odometry.moveToCoords(2000,3000); - - } - */ } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java index 6295e58..75cc1a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java @@ -36,23 +36,19 @@ public class autonomous extends LinearOpMode { @Override public void runOpMode() { - control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); - //imu = hardwareMap.get(IMU.class, "imu"); - if (Parameters.robot == "marvin") { - intake = hardwareMap.get(DcMotor.class, "intake"); - } camera = OpenCVSubsystem.getInstance(hardwareMap, telemetry); chassis = ChassisSubsystem.getInstance(hardwareMap, telemetry); gyroscope = GyroscopeSubsystem.getInstance(hardwareMap); - elevator = ElevatorSubsystem.getInstance(hardwareMap); +// elevator = ElevatorSubsystem.getInstance(hardwareMap); odometry = OdometrySubsystem.getInstance(hardwareMap, chassis, telemetry); - arm = ArmSubsystem.getInstance(hardwareMap, telemetry); +// arm = ArmSubsystem.getInstance(hardwareMap, telemetry); pivot = PivotSubsystem.getInstance(hardwareMap, telemetry); odometry.resetEncoders(); - //gyroscope.reset(); + //Maybe maybe break + gyroscope.reset(); waitForStart(); while (opModeIsActive()) { @@ -196,140 +192,137 @@ public void runOpMode() { ///////////// BLUE TEAM ///////////// - if (info.name == "gobilda") { - arm.close(); - arm.setPosition(1); - arm.updateArm(0); - elevator.goUp(); - - odometry.resetEncoders(); - - switch (objectSide) { - case 0: - telemetry.addData("OBJECT NOT FOUND. ", "SKILL ISSUE IM AFRAID"); - sleep(5000); - break; - - case 1: - dolor = true; - while (dolor) { - dolor = odometry.goTo(0,-63); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - dolor = true; - while(dolor) { - dolor = odometry.rotateTo(gyroscope.getRotation(), -90); - } - chassis.setMotors(0,0,0,0); - gyroscope.reset(); - odometry.resetEncoders(); - - sleep(300); - arm.setPosition(1); - arm.updateArm(1); - - dolor = true; - while(dolor) { - dolor = odometry.goTo(20,0); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - dolor = true; - while(dolor) { - dolor = odometry.goTo(0,-100); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - break; - case 2: - dolor = true; - while (dolor) { - dolor = odometry.goTo(0,-63); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - dolor = true; - while (dolor) { - dolor = odometry.goTo(10,0); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - sleep(300); - pivot.open(); - - dolor = true; - while (dolor) { - dolor = odometry.goTo(-20,0); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - dolor = true; - while (dolor) { - dolor = odometry.goTo(0,-20); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - dolor = true; - while (dolor) { - dolor = odometry.goTo(-100,0); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - break; - case 3: - dolor = true; - while (dolor) { - dolor = odometry.goTo(0,-63); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - dolor = true; - while(dolor) { - dolor = odometry.rotateTo(gyroscope.getRotation(), 90); - } - chassis.setMotors(0,0,0,0); - gyroscope.reset(); - odometry.resetEncoders(); - - sleep(300); - pivot.up(); - pivot.open(); - - dolor = true; - while (dolor) { - dolor = odometry.goTo(20,0); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - dolor = true; - while (dolor) { - dolor = odometry.goTo(0,-50); - } - chassis.setMotors(0,0,0,0); - odometry.resetEncoders(); - - break; - } - - } - elevator.goUp(); - telemetry.addData("object side: ", objectSide); +// if (info.name == "gobilda") { +// arm.close(); +// arm.setPosition(1); +// arm.updateArm(0); +// elevator.goUp(); +// +// odometry.resetEncoders(); +// +// switch (objectSide) { +// case 0: +// telemetry.addData("OBJECT NOT FOUND. ", "SKILL ISSUE IM AFRAID"); +// sleep(5000); +// break; +// +// case 1: +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-63); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while(dolor) { +// dolor = odometry.rotateTo(gyroscope.getRotation(), -90); +// } +// chassis.setMotors(0,0,0,0); +// gyroscope.reset(); +// odometry.resetEncoders(); +// +// sleep(300); +// arm.setPosition(1); +// arm.updateArm(1); +// +// dolor = true; +// while(dolor) { +// dolor = odometry.goTo(20,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while(dolor) { +// dolor = odometry.goTo(0,-100); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// break; +// case 2: +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-63); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(10,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// sleep(300); +// pivot.open(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(-20,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-20); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(-100,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// break; +// case 3: +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-63); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while(dolor) { +// dolor = odometry.rotateTo(gyroscope.getRotation(), 90); +// } +// chassis.setMotors(0,0,0,0); +// gyroscope.reset(); +// odometry.resetEncoders(); +// +// sleep(300); +// pivot.up(); +// pivot.open(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(20,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-50); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// break; +// } +// +// } + telemetry.addData("object side: ", objectSide); telemetry.update(); } - - } } //} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java index c363f54..181533f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java @@ -3,7 +3,7 @@ public class info { public static final String name = "rev"; //public static final String name = "gobilda"; - public static final String OPMODE = "autonomous"; - //public static final String OPMODE = "teleop"; + //public static final String OPMODE = "autonomous"; + public static final String OPMODE = "teleop"; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java index 299da32..096a3cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java @@ -78,7 +78,7 @@ public void setPosition(double position){ extended = true; } - public void updateArm(int axisPosition) { + public void updateArm(/*int axisPosition*/) { if(going_down){ servoL.setPosition(angle-0.3); servoR.setPosition(servoR.MAX_POSITION-angle+0.3); @@ -86,13 +86,15 @@ public void updateArm(int axisPosition) { servoL.setPosition(angle); servoR.setPosition(servoR.MAX_POSITION-angle); } - +/* if (axisPosition == 0) { setZero(); } if (axisPosition == 1) { updateAxis(); } + + */ } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java index d37bad7..816ff29 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java @@ -110,8 +110,18 @@ public void moveR(double speed) { setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); } + + public void arcadeDrive(double x, double y, double r, double speed, double degrees) { +// if (info.name == "rev") { +// double theta = Math.atan2(y, x); +// double power = Math.hypot(x, y); +// double sin = Math.sin(theta - Math.PI / 4); +// double cos = Math.cos(theta - Math.PI / 4); +// double max = Math.max(Math.abs(sin), Math.abs(cos)); +// } + telemetry.addData("targetAngle",targetAngle); if(Math.abs(targetAngle-180)<20){ targetAngle=180; @@ -139,7 +149,7 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre } }else{ - angularError = calculateRotation((int) degrees, (int) targetAngle); + /*angularError = calculateRotation((int) degrees, (int) targetAngle); final double KPPP = 0.03; double errror = targetAngle - degrees; double speeed = errror * KPPP; @@ -148,7 +158,7 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre double angularThreshold = 10; //if(Math.abs(angularError)>angularThreshold) { //} - r = speeed; + r = speeed;*/ } } @@ -248,6 +258,4 @@ public void rotateDeg(double deg){ } } - - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DroneLauncherSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DroneLauncherSubsystem.java index 414906b..90349f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DroneLauncherSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DroneLauncherSubsystem.java @@ -27,12 +27,12 @@ public DroneLauncherSubsystem(HardwareMap hardwareMap,Telemetry telemetry){ } - + //POSSIBLY SWAP MAX_POSITION AND MIN_POSITION OUT public void launch(){ - launcher.setPosition(launcher.MIN_POSITION); + launcher.setPosition(launcher.MAX_POSITION); telemetry.addData("launcher",launcher.getPosition()); } - public void reset(){launcher.setPosition(launcher.MAX_POSITION); + public void reset(){launcher.setPosition(launcher.MIN_POSITION); telemetry.addData("launcher",launcher.getPosition()); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index 0edae10..4a80ecc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -46,6 +46,7 @@ public coordinates(int x, int y){ this.y=y; } } + public coordinates position(){ return new coordinates(fieldX,fieldY); } @@ -53,6 +54,7 @@ public void update(){ fieldX = robotX + xEncoder.getCurrentPosition(); fieldY = robotY + yEncoder.getCurrentPosition(); } + public void resetEncoders(){ xEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); yEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -62,19 +64,22 @@ public void resetEncoders(){ final double WHEELCIRCUMFERENCE = 4.8 * Math.PI; final int TICKS = 2000; + public double getXDist() { return (xEncoder.getCurrentPosition() * WHEELCIRCUMFERENCE) / TICKS; } + public double getYDist() { return (yEncoder.getCurrentPosition() * WHEELCIRCUMFERENCE) / TICKS; } + public void printEncoders() { telemetry.addData("xEncoder", xEncoder.getCurrentPosition()); telemetry.addData("yEncoder", yEncoder.getCurrentPosition()); telemetry.addData("X: ", getXDist()); telemetry.addData("Y: ", getYDist()); } - final double KP = 0.03; + private final double KP = 0.03; public boolean goTo(int x, int y) { double yError = y - getYDist() * -1; @@ -93,6 +98,7 @@ public boolean goTo(int x, int y) { return (Math.abs(xError) + Math.abs(yError) > 5) ? true : false; } + public boolean rotateTo(double degree, double target) { final double KP2 = 0.018; double rError = target - degree; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java index bb72895..99f3d00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java @@ -17,7 +17,7 @@ public class PivotSubsystem { private final double CLAW_OPEN = 1; private final double CLAW_CLOSE = 0; private final double PIVOT_UP = 1; - + private final double PIVOT_PICKUP = 0.3; private final double PIVOT_DOWN = 0; public PivotSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { @@ -50,15 +50,21 @@ public void down() { servoPivot.setPosition(PIVOT_DOWN); } - public void pivotControls(boolean buttonA, boolean buttonB, boolean leftBumper, boolean rightBumper) { + public void pickup() { + servoPivot.setPosition(PIVOT_PICKUP); + } + + public void pivotControls(boolean buttonA, boolean buttonB, boolean buttonX, boolean leftBumper, boolean rightBumper) { if (buttonA) { - up(); + pickup(); } else if (buttonB) { - down(); + up(); } else if (rightBumper) { open(); } else if (leftBumper) { close(); + } else if (buttonX) { + down(); } } public void printPositions() {